Trikarus/firmware_smartstepper_trikarus/stepper_nano_zero/planner.cpp
2020-07-20 22:21:36 +02:00

208 lines
4.9 KiB
C++

/**********************************************************************
* Author: tstern
*
Copyright (C) 2018 MisfitTech, All rights reserved.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "planner.h"
#include "board.h"
#include "wiring_private.h"
#include "syslog.h"
#include "angle.h"
#include "Arduino.h"
#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY);
//define the planner class as being global
Planner SmartPlanner;
static bool enterTC3CriticalSection()
{
bool state=NVIC_IS_IRQ_ENABLED(TC3_IRQn);
NVIC_DisableIRQ(TC3_IRQn);
return state;
}
static void exitTC3CriticalSection(bool prevState)
{
if (prevState)
{
NVIC_EnableIRQ(TC3_IRQn);
} //else do nothing
}
void TC3_Init(void)
{
// Enable GCLK for TC3
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
while (GCLK->STATUS.bit.SYNCBUSY);
TC3->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable TCx
WAIT_TC16_REGS_SYNC(TC3) // wait for sync
TC3->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; // Set Timer counter Mode to 16 bits
WAIT_TC16_REGS_SYNC(TC3)
TC3->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC as normal Normal Frq
WAIT_TC16_REGS_SYNC(TC3)
TC3->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; // Set perscaler
WAIT_TC16_REGS_SYNC(TC3)
TC3->COUNT16.CC[0].reg = F_CPU/PLANNER_UPDATE_RATE_HZ/2; //divide by two because of prescaler
WAIT_TC16_REGS_SYNC(TC3)
TC3->COUNT16.INTENSET.reg = 0; // disable all interrupts
TC3->COUNT16.INTENSET.bit.OVF = 1; // enable overfollow
NVIC_SetPriority(TC3_IRQn, 3);
// Enable InterruptVector
NVIC_EnableIRQ(TC3_IRQn);
// Enable TC
TC3->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
WAIT_TC16_REGS_SYNC(TC3);
}
void TC3_Handler(void)
{
interrupts(); //allow other interrupts
//do the planner tick
SmartPlanner.tick();
//SerialUSB.println('x');
TC3->COUNT16.INTFLAG.bit.OVF = 1; //clear interrupt by writing 1 to flag
}
void Planner::begin(StepperCtrl *ptrStepper)
{
ptrStepperCtrl=ptrStepper;
currentMode=PLANNER_NONE;
//setup the timer and interrupt as the last thing
TC3_Init();
}
void Planner::tick(void)
{
if (currentMode == PLANNER_NONE)
{
return; //do nothing
}
if (PLANNER_CV == currentMode)
{
// SerialUSB.println(currentSetAngle);
// SerialUSB.println(endAngle);
// SerialUSB.println(tickIncrement);
// SerialUSB.println(fabs(currentSetAngle-endAngle));
// SerialUSB.println(fabs(tickIncrement*2));
// SerialUSB.println();
int32_t x;
if (fabs(currentSetAngle-endAngle) >= fabs(tickIncrement))
{
currentSetAngle+=tickIncrement;
x=ANGLE_FROM_DEGREES(currentSetAngle);
ptrStepperCtrl->moveToAbsAngle(x);
}else
{
//we are done, make sure we end at the right point
//SerialUSB.println("done");
x=ANGLE_FROM_DEGREES(endAngle);
ptrStepperCtrl->moveToAbsAngle(x);
currentMode=PLANNER_NONE;
}
}
}
void Planner::stop(void)
{
bool state;
state = enterTC3CriticalSection();
currentMode=PLANNER_NONE;
exitTC3CriticalSection(state);
}
bool Planner::moveConstantVelocity(float finalAngle, float rpm)
{
bool state;
state = enterTC3CriticalSection();
//first determine if operation is in progress
if (PLANNER_NONE != currentMode)
{
#ifndef MECHADUINO_HARDWARE
//we are in operation return false
SerialUSB.println("planner operational");
#else
Serial5.println("planner operational");
#endif
exitTC3CriticalSection(state);
return false;
}
//get current posistion
startAngle = ANGLE_T0_DEGREES(ptrStepperCtrl->getCurrentAngle());
//deterime the tick increment
tickIncrement=360.0*fabs(rpm)/60/PLANNER_UPDATE_RATE_HZ;
//set the desired end angle
endAngle=finalAngle;
//set the current angle
currentSetAngle=startAngle;
if (startAngle>endAngle)
{
#ifndef MECHADUINO_HARDWARE
SerialUSB.println("reverse");
#endif
tickIncrement=-tickIncrement;
}
// SerialUSB.println(currentSetAngle);
// SerialUSB.println(endAngle);
// SerialUSB.println(tickIncrement);
// SerialUSB.println();
currentMode=PLANNER_CV;
exitTC3CriticalSection(state);
return true;
}