vmario/Trikarus
0
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
ec6085f726
Trikarus/firmware_smartstepper_trikarus/stepper_nano_zero/stepper_controller.cpp
Mario Voigt ec6085f726 Initial commit
2020-07-20 22:21:36 +02:00

1952 lines
42 KiB
C++
Raw Blame History

/**********************************************************************
* 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 "stepper_controller.h"
#include "nonvolatile.h" //for programmable parameters
#include <Wire.h>
#include <inttypes.h>
#include "steppin.h"
#pragma GCC push_options
#pragma GCC optimize ("-Ofast")
#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY);
volatile bool TC5_ISR_Enabled=false;
void setupTCInterrupts() {
// Enable GCLK for TC4 and TC5 (timer counter input clock)
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5));
while (GCLK->STATUS.bit.SYNCBUSY);
TC5->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable TCx
WAIT_TC16_REGS_SYNC(TC5) // wait for sync
TC5->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; // Set Timer counter Mode to 16 bits
WAIT_TC16_REGS_SYNC(TC5)
TC5->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC as normal Normal Frq
WAIT_TC16_REGS_SYNC(TC5)
TC5->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1; // Set perscaler
WAIT_TC16_REGS_SYNC(TC5)
TC5->COUNT16.CC[0].reg = F_CPU/NZS_CONTROL_LOOP_HZ;
WAIT_TC16_REGS_SYNC(TC5)
TC5->COUNT16.INTENSET.reg = 0; // disable all interrupts
TC5->COUNT16.INTENSET.bit.OVF = 1; // enable overfollow
// TC5->COUNT16.INTENSET.bit.MC0 = 1; // enable compare match to CC0
NVIC_SetPriority(TC5_IRQn, 2);
// Enable InterruptVector
NVIC_EnableIRQ(TC5_IRQn);
// Enable TC
TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
WAIT_TC16_REGS_SYNC(TC5)
}
static void enableTCInterrupts() {
TC5_ISR_Enabled=true;
NVIC_EnableIRQ(TC5_IRQn);
TC5->COUNT16.INTENSET.bit.OVF = 1;
// TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; //Enable TC5
// WAIT_TC16_REGS_SYNC(TC5) //wait for sync
}
static void disableTCInterrupts() {
TC5_ISR_Enabled=false;
//NVIC_DisableIRQ(TC5_IRQn);
TC5->COUNT16.INTENCLR.bit.OVF = 1;
}
static bool enterCriticalSection()
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
return state;
}
static void exitCriticalSection(bool prevState)
{
if (prevState)
{
enableTCInterrupts();
} //else do nothing
}
void StepperCtrl::updateParamsFromNVM(void)
{
bool state=enterCriticalSection();
pPID.Kd=NVM->pPID.Kd*CTRL_PID_SCALING;
pPID.Ki=NVM->pPID.Ki*CTRL_PID_SCALING;
pPID.Kp=NVM->pPID.Kp*CTRL_PID_SCALING;
vPID.Kd=NVM->vPID.Kd*CTRL_PID_SCALING;
vPID.Ki=NVM->vPID.Ki*CTRL_PID_SCALING;
vPID.Kp=NVM->vPID.Kp*CTRL_PID_SCALING;
sPID.Kd=NVM->sPID.Kd*CTRL_PID_SCALING;
sPID.Ki=NVM->sPID.Ki*CTRL_PID_SCALING;
sPID.Kp=NVM->sPID.Kp*CTRL_PID_SCALING;
if (NVM->SystemParams.parametersVaild)
{
memcpy((void *)&systemParams, (void *)&NVM->SystemParams, sizeof(systemParams));
LOG("Home pin %d",systemParams.homePin);
}else
{
ERROR("This should never happen but just in case");
systemParams.microsteps=16;
#if defined(CTRL_POS_PID_AS_DEFAULT)
systemParams.controllerMode=CTRL_POS_PID;
#else
systemParams.controllerMode=CTRL_SIMPLE;
#endif
systemParams.dirPinRotation=CW_ROTATION; //default to clockwise rotation when dir is high
systemParams.errorLimit=(int32_t)ANGLE_FROM_DEGREES(1.8);
systemParams.errorPinMode=ERROR_PIN_MODE_ENABLE; //default to enable pin
systemParams.errorLogic=false;
systemParams.homeAngleDelay=ANGLE_FROM_DEGREES(10);
systemParams.homePin=-1; //no homing pin configured
}
//default the error pin to input, if it is an error pin the
// handler for this will change the pin to be an output.
// for bidirection error it has to handle input/output it's self as well.
// This is not the cleanest way to handle this...
// TODO implement this cleaner?
pinMode(PIN_ERROR, INPUT_PULLUP); //we have input pin
if (NVM->motorParams.parametersVaild)
{
memcpy((void *)&motorParams, (void *)&NVM->motorParams, sizeof(motorParams));
} else
{
//MotorParams_t Params;
motorParams.fullStepsPerRotation=200;
motorParams.currentHoldMa=500;
motorParams.currentMa=800;
motorParams.homeHoldMa=200;
motorParams.homeMa=800;
motorParams.motorWiring=true;
//memcpy((void *)&Params, (void *)&motorParams, sizeof(motorParams));
//nvmWriteMotorParms(Params);
}
stepperDriver.setRotationDirection(motorParams.motorWiring);
exitCriticalSection(state);
}
void StepperCtrl::motorReset(void)
{
//when we reset the motor we want to also sync the motor
// phase. Therefore we move forward a few full steps then back
// to sync motor phasing, leaving the motor at "phase 0"
bool state=enterCriticalSection();
// stepperDriver.move(0,motorParams.currentMa);
// delay(100);
// stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa);
// delay(100);
// stepperDriver.move(A4954_NUM_MICROSTEPS*2,motorParams.currentMa);
// delay(100);
// stepperDriver.move(A4954_NUM_MICROSTEPS*3,motorParams.currentMa);
// delay(100);
// stepperDriver.move(A4954_NUM_MICROSTEPS*2,motorParams.currentMa);
// delay(100);
// stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa);
// delay(100);
stepperDriver.move(0,motorParams.currentMa);
delay(1000);
setLocationFromEncoder(); //measure new starting point
exitCriticalSection(state);
}
void StepperCtrl::setLocationFromEncoder(void)
{
numSteps=0;
currentLocation=0;
if (calTable.calValid())
{
int32_t n,x;
int32_t calIndex;
Angle a;
//set our angles based on previous cal data
x=sampleMeanEncoder(200);
a=calTable.fastReverseLookup(x);
//our cal table starts at angle zero, so lets set starting based on this and stepsize
LOG("start angle %d, encoder %d", (uint16_t)a,x);
// we were rounding to nearest full step, but this should not be needed TBS 10/12/2017
// //TODO we need to handle 0.9 degree motor
// if (CALIBRATION_TABLE_SIZE == motorParams.fullStepsPerRotation)
// {
// n=(int32_t)ANGLE_STEPS/CALIBRATION_TABLE_SIZE;
//
// calIndex=((int32_t)((uint16_t)a+n/2)*CALIBRATION_TABLE_SIZE)/ANGLE_STEPS; //find calibration index
// if (calIndex>CALIBRATION_TABLE_SIZE)
// {
// calIndex-=CALIBRATION_TABLE_SIZE;
// }
// a=(uint16_t)((calIndex*ANGLE_STEPS)/CALIBRATION_TABLE_SIZE);
// }
x=(int32_t)((((float)(uint16_t)a)*360.0/(float)ANGLE_STEPS)*1000);
LOG("start angle after rounding %d %d.%03d", (uint16_t)a,x/1000,x%1000);
//we need to set our numSteps
numSteps=DIVIDE_WITH_ROUND( ((int32_t)a *motorParams.fullStepsPerRotation*systemParams.microsteps),ANGLE_STEPS);
currentLocation=(uint16_t)a;
}
zeroAngleOffset=getCurrentLocation(); //zero the angle shown on LCD
}
void StepperCtrl::acceptPositionAndStealthSwitchMode(feedbackCtrl_t m)
{
bool state=enterCriticalSection();
currentLocationIsDesiredLocation();
systemParams.controllerMode=m;
exitCriticalSection(state);
}
void StepperCtrl::stealthSwitchMode(feedbackCtrl_t m)
{
bool state=enterCriticalSection();
systemParams.controllerMode=m;
exitCriticalSection(state);
}
int64_t StepperCtrl::getZeroAngleOffset(void)
{
int64_t x;
bool state=enterCriticalSection();
x=zeroAngleOffset;
exitCriticalSection(state);
return x;
}
void StepperCtrl::setAngle(int64_t angle)
{
bool state=enterCriticalSection();
zeroAngleOffset=getCurrentLocation()-angle;
exitCriticalSection(state);
}
void StepperCtrl::setZero(void)
{
//we want to set the starting angle to zero.
bool state=enterCriticalSection();
zeroAngleOffset=getCurrentLocation();
exitCriticalSection(state);
}
void StepperCtrl::encoderDiagnostics(char *ptrStr)
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
encoder.diagnostics(ptrStr);
if (state) enableTCInterrupts();
}
//TODO This function does two things, set rotation direction
// and measures step size, it should be two functions.
//return is anlge in degreesx100 ie 360.0 is returned as 36000
float StepperCtrl::measureStepSize(void)
{
int32_t angle1,angle2,x,i;
bool feedback=enableFeedback;
int32_t microsteps=systemParams.microsteps;
systemParams.microsteps=1;
enableFeedback=false;
motorParams.motorWiring=true; //assume we are forward wiring to start with
stepperDriver.setRotationDirection(motorParams.motorWiring);
/////////////////////////////////////////
//// Measure the full step size /////
/// Note we assume machine can take one step without issue///
LOG("reset motor");
motorReset(); //this puts stepper motor at stepAngle of zero
LOG("sample encoder");
angle1=sampleMeanEncoder(200);
LOG("move");
stepperDriver.move(A4954_NUM_MICROSTEPS/2,motorParams.currentMa); //move one half step 'forward'
delay(100);
stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa); //move one half step 'forward'
delay(500);
LOG("sample encoder");
angle2=sampleMeanEncoder(200);
LOG("Angles %d %d",angle1,angle2);
if ((abs(angle2-angle1))>(ANGLE_STEPS/2))
{
//we crossed the wrap around
if (angle1>angle2)
{
angle1=angle1+(int32_t)ANGLE_STEPS;
}else
{
angle2=angle2+(int32_t)ANGLE_STEPS;
}
}
LOG("Angles %d %d",angle1,angle2);
//when we are increase the steps in the stepperDriver.move() command
// we want the encoder increasing. This ensures motor is moving clock wise
// when encoder is increasing.
// if (angle2>angle1)
// {
// motorParams.motorWiring=true;
// stepperDriver.setRotationDirection(true);
// LOG("Forward rotating");
// }else
// {
// //the motor is wired backwards so correct in stepperDriver
// motorParams.motorWiring=false;
// stepperDriver.setRotationDirection(false);
// LOG("Reverse rotating");
// }
x=((int64_t)(angle2-angle1)*36000)/(int32_t)ANGLE_STEPS;
// if x is ~180 we have a 1.8 degree step motor, if it is ~90 we have 0.9 degree step
LOG("%angle delta %d %d (%d %d)",x,abs(angle2-angle1),angle1,angle2 );
//move back
stepperDriver.move(-A4954_NUM_MICROSTEPS/2,motorParams.currentMa); //move one half step 'forward'
delay(100);
stepperDriver.move(-A4954_NUM_MICROSTEPS,motorParams.currentMa); //move one half step 'forward'
systemParams.microsteps=microsteps;
enableFeedback=feedback;
return ((float)x)/100.0;
}
int32_t StepperCtrl::measureError(void)
{
//LOG("current %d desired %d %d",(int32_t) currentLocation, (int32_t)getDesiredLocation(), numSteps);
return ((int32_t)currentLocation-(int32_t)getDesiredLocation());
}
/*
bool StepperCtrl::changeMicrostep(uint16_t microSteps)
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
systemParams.microsteps=microSteps;
motorReset();
if (state) enableTCInterrupts();
return true;
}
*/
Angle StepperCtrl::maxCalibrationError(void)
{
//Angle startingAngle;
bool done=false;
int32_t mean;
int32_t maxError=0, j;
int16_t dist;
uint16_t angle=0;
bool feedback=enableFeedback;
uint16_t microSteps=systemParams.microsteps;
int32_t steps;
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
if (false == calTable.calValid())
{
return ANGLE_MAX;
}
enableFeedback=false;
j=0;
LOG("Running calibration test");
systemParams.microsteps=1;
motorReset();
steps=0;
while(!done)
{
Angle cal;
Angle act, desiredAngle;
//todo we should measure mean and wait until stable.
delay(200);
mean=sampleMeanEncoder(200);
desiredAngle=(uint16_t)(getDesiredLocation() & 0x0FFFFLL);
cal=calTable.getCal(desiredAngle);
dist=Angle(mean)-cal;
act=calTable.fastReverseLookup(cal);
LOG("actual %d, cal %d",mean,(uint16_t)cal);
LOG("desired %d",(uint16_t)desiredAngle);
LOG("numSteps %d", numSteps);
LOG("cal error for step %d is %d",j,dist);
LOG("mean %d, cal %d",mean, (uint16_t)cal);
updateStep(0,1);
// move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
delay(50);
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
if (400==motorParams.fullStepsPerRotation)
{
delay(100);
updateStep(0,1);
// move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
delay(100);
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
}
//delay(400);
if (abs(dist)>maxError)
{
maxError=abs(dist);
}
j++;
if (j>=(1*CALIBRATION_TABLE_SIZE+3))
{
done=true;
}
}
systemParams.microsteps=microSteps;
motorReset();
enableFeedback=feedback;
if (state) enableTCInterrupts();
LOG("max error is %d cnts", maxError);
return Angle((uint16_t)maxError);
}
//The encoder needs to be calibrated to the motor.
// we will assume full step detents are correct,
// ex 1.8 degree motor will have 200 steps for 360 degrees.
// We also need to calibrate the phasing of the motor
// to the A4954. This requires that the A4954 "step angle" of
// zero is the first entry in the calibration table.
bool StepperCtrl::calibrateEncoder(void)
{
int32_t x,i,j;
uint32_t angle=0;
int32_t steps;
bool done=false;
int32_t mean;
uint16_t microSteps=systemParams.microsteps;
bool feedback=enableFeedback;
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
enableFeedback=false;
systemParams.microsteps=1;
LOG("reset motor");
motorReset();
LOG("Starting calibration");
delay(200);
steps=0;
j=0;
while(!done)
{
int ii,N;
Angle cal,desiredAngle;
desiredAngle=(uint16_t)(getDesiredLocation() & 0x0FFFFLL);
cal=calTable.getCal(desiredAngle);
delay(200);
mean=sampleMeanEncoder(200);
LOG("Previous cal distance %d, %d, mean %d, cal %d",j, cal-Angle((uint16_t)mean), mean, (uint16_t)cal);
calTable.updateTableValue(j,mean);
updateStep(0,1);
N=2;
// move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
for (ii=0; ii<N; ii++)
{
steps+=A4954_NUM_MICROSTEPS/N;
stepperDriver.move(steps,motorParams.currentMa);
delay(50);
}
//steps+=A4954_NUM_MICROSTEPS/2;
//stepperDriver.move(steps,motorParams.currentMa);
if (400==motorParams.fullStepsPerRotation)
{
delay(100);
updateStep(0,1);
// move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
delay(100);
steps+=A4954_NUM_MICROSTEPS/2;
stepperDriver.move(steps,motorParams.currentMa);
}
j++;
if (j>=CALIBRATION_TABLE_SIZE)
{
done=true;
}
}
//calTable.printCalTable();
//calTable.smoothTable();
//calTable.printCalTable();
calTable.saveToFlash(); //saves the calibration to flash
calTable.printCalTable();
systemParams.microsteps=microSteps;
motorReset();
enableFeedback=feedback;
if (state) enableTCInterrupts();
return done;
}
stepCtrlError_t StepperCtrl::begin(void)
{
int i;
float x;
enableFeedback=false;
velocity=0;
currentLocation=0;
numSteps=0;
//we have to update from NVM before moving motor
updateParamsFromNVM(); //update the local cache from the NVM
LOG("start up encoder");
if (false == encoder.begin(PIN_AS5047D_CS))
{
return STEPCTRL_NO_ENCODER;
}
LOG("cal table init");
calTable.init();
startUpEncoder=(uint16_t)getEncoderAngle();
WARNING("start up encoder %d",startUpEncoder);
LOG("start stepper driver");
stepperDriver.begin();
#ifdef NEMA17_SMART_STEPPER_3_21_2017
if (NVM->motorParams.parametersVaild)
{
//lets read the motor voltage
if (GetMotorVoltage()<5)
{
//if we have less than 5 volts the motor is not powered
uint32_t x;
x=(uint32_t)(GetMotorVoltage()*1000.0);
ERROR("Motor voltage is %" PRId32 "mV",x); //default printf does not support floating point numbers
ERROR("Motor may not have power");
return STEPCTRL_NO_POWER;
}
bool state=enterCriticalSection();
setLocationFromEncoder(); //measure new starting point
exitCriticalSection(state);
}else
{
LOG("measuring step size");
x=measureStepSize();
if (abs(x)<0.5)
{
ERROR("Motor may not have power");
return STEPCTRL_NO_POWER;
}
}
#else
LOG("measuring step size");
x=measureStepSize();
if (abs(x)<0.5)
{
ERROR("Motor may not have power");
return STEPCTRL_NO_POWER;
}
#endif
LOG("Checking the motor parameters");
//todo we might want to move this up a level to the NZS
// especially since it has default values
if (false == NVM->motorParams.parametersVaild)
{
MotorParams_t params;
WARNING("NVM motor parameters are not set, we will update");
//power could have just been applied and step size read wrong
// if we are more than 200 steps/rotation which is most common
// lets read again just to be sure.
if (abs(x)<1.5)
{
//run step test a second time to be sure
x=measureStepSize();
}
if (x>0)
{
motorParams.motorWiring=true;
} else
{
motorParams.motorWiring=false;
}
if (abs(x)<=1.2)
{
motorParams.fullStepsPerRotation=400;
}else
{
motorParams.fullStepsPerRotation=200;
}
memcpy((void *)&params, (void *)&motorParams,sizeof(motorParams));
nvmWriteMotorParms(params);
}
LOG("Motor params are now good");
LOG("fullSteps %d", motorParams.fullStepsPerRotation);
LOG("motorWiring %d", motorParams.motorWiring);
LOG("currentMa %d", motorParams.currentMa);
LOG("holdCurrentMa %d", motorParams.currentHoldMa);
LOG("homeMa %d", motorParams.homeMa);
LOG("homeHoldMa %d", motorParams.homeHoldMa);
updateParamsFromNVM(); //update the local cache from the NVM
if (false == calTable.calValid())
{
return STEPCTRL_NO_CAL;
}
enableFeedback=true;
setupTCInterrupts();
enableTCInterrupts();
return STEPCTRL_NO_ERROR;
}
Angle StepperCtrl::sampleAngle(void)
{
uint16_t angle;
int32_t x,y;
#ifdef NZS_AS5047_PIPELINE
//read encoder twice such that we get the latest sample as the pipeline is always once sample behind
y=encoder.readEncoderAnglePipeLineRead(); //convert the 14 bit encoder value to a 16 bit number
x=encoder.readEncoderAnglePipeLineRead();
angle=((uint32_t)(x)*4); //convert the 14 bit encoder value to a 16 bit number
#else
angle=((uint32_t)encoder.readEncoderAngle())<<2; //convert the 14 bit encoder value to a 16 bit number
#endif
return Angle(angle);
}
//when sampling the mean of encoder if we are on roll over
// edge we can have an issue so we have this function
// to do the mean correctly
Angle StepperCtrl::sampleMeanEncoder(int32_t numSamples)
{
int32_t i,last,x=0;
int64_t mean=0;
int32_t min,max;
mean=0;
for (i=0; i<(numSamples); i++)
{
int32_t d;
last=x;
x=(((int32_t)encoder.readEncoderAngle())*4);
if (encoder.getError())
{
#ifndef MECHADUINO_HARDWARE
SerialUSB.println("AS5047 Error");
#else
Serial5.print("AS5047 Error");
#endif
delay(1000);
return 0;
}
if(i==0)
{
last=x;
min=x;
max=x;
}
//LOG("i %d,min %d, max %d, last %d, x %d", i, min, max, last, x);
if (abs(last-x)>65536/2)
{
if (last>x)
{
x=x+65536;
} else
{
x=x-65536;
}
}
if (x>max)
{
max=x;
}
if (x<min)
{
min=x;
}
mean=mean+(x);
}
LOG("min %d, max %d, mean %d", min, max, (int32_t)(mean/numSamples));
return Angle(mean/numSamples);
}
void StepperCtrl::feedback(bool enable)
{
disableTCInterrupts();
motorReset();
enableFeedback=enable;
if (enable == true)
{
enableTCInterrupts();
}
}
void StepperCtrl::updateSteps(int64_t steps)
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
numSteps+=steps;
if (state) enableTCInterrupts();
}
void StepperCtrl::updateStep(int dir, uint16_t steps)
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
if (dir)
{
numSteps-=steps;
}else
{
numSteps+=steps;
}
if (state) enableTCInterrupts();
}
void StepperCtrl::requestStep(int dir, uint16_t steps)
{
bool state;
state=TC5_ISR_Enabled;
disableTCInterrupts();
if (dir)
{
numSteps-=steps;
}else
{
numSteps+=steps;
}
if (false == enableFeedback)
{
moveToAngle(getDesiredLocation(),motorParams.currentMa);
}
if (state) enableTCInterrupts();
}
void StepperCtrl::move(int dir, uint16_t steps)
{
int64_t ret;
int32_t n;
updateStep(dir,steps);
if (false == enableFeedback)
{
n=systemParams.microsteps;
ret=((int64_t)numSteps * A4954_NUM_MICROSTEPS+(n/2))/n;
n=A4954_NUM_MICROSTEPS*motorParams.fullStepsPerRotation;
while(ret>n)
{
ret-=n;
}
while(ret<-n)
{
ret+=n;
}
n=(int32_t)(ret);
LOG("s is %d %d",n,steps);
stepperDriver.move(n,motorParams.currentMa);
}
}
int64_t StepperCtrl::getDesiredLocation(void)
{
int64_t ret;
int32_t n;
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
n=motorParams.fullStepsPerRotation * systemParams.microsteps;
ret=((int64_t)numSteps * (int64_t)ANGLE_STEPS+(n/2))/n ;
if (state) enableTCInterrupts();
return ret;
}
//int32_t StepperCtrl::getSteps(void)
//{
// int32_t ret;
// bool state=enterCriticalSection();
// ret=numSteps;
// exitCriticalSection(state);
// return ret;
//}
void StepperCtrl::moveToAbsAngle(int32_t a)
{
int64_t ret;
int32_t n;
n=motorParams.fullStepsPerRotation * systemParams.microsteps;
ret=(((int64_t)a+zeroAngleOffset)*n+ANGLE_STEPS/2)/(int32_t)ANGLE_STEPS;
bool state=enterCriticalSection();
numSteps=ret;
exitCriticalSection(state);
}
void StepperCtrl::moveToAngle(int32_t a, uint32_t ma)
{
//we need to convert 'Angle' to A4954 steps
a=a % ANGLE_STEPS; //we only interested in the current angle
a=DIVIDE_WITH_ROUND( (a*motorParams.fullStepsPerRotation*A4954_NUM_MICROSTEPS), ANGLE_STEPS);
//LOG("move %d %d",a,ma);
stepperDriver.move(a,ma);
}
Angle StepperCtrl::getEncoderAngle(void)
{
Angle a;
bool state=enterCriticalSection();
a=calTable.fastReverseLookup(sampleAngle());
exitCriticalSection(state);
return a;
}
int64_t StepperCtrl::getCurrentLocation(void)
{
Angle a;
int32_t x;
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
a=calTable.fastReverseLookup(sampleAngle());
x=(int32_t)a - (int32_t)((currentLocation) & ANGLE_MAX);
if (x>((int32_t)ANGLE_STEPS/2))
{
currentLocation -= ANGLE_STEPS;
}
if (x<-((int32_t)ANGLE_STEPS/2))
{
currentLocation += ANGLE_STEPS;
}
currentLocation=(currentLocation & 0xFFFFFFFFFFFF0000UL) | (uint16_t)a;
if (state) enableTCInterrupts();
return currentLocation;
}
int64_t StepperCtrl::getCurrentAngle(void)
{
int64_t x;
x=getCurrentLocation()-zeroAngleOffset;
return x;
}
int64_t StepperCtrl::getDesiredAngle(void)
{
int64_t x;
x=getDesiredLocation()-zeroAngleOffset;
return x;
}
void StepperCtrl::setVelocity(int64_t vel)
{
bool state=enterCriticalSection();
velocity=vel;
exitCriticalSection(state);
}
void StepperCtrl::setTorque(int8_t tor)
{
bool state=enterCriticalSection();
torque=tor;
exitCriticalSection(state);
}
int8_t StepperCtrl::getTorque(void)
{
int8_t tor;
bool state=enterCriticalSection();
tor=torque;
exitCriticalSection(state);
return tor;
}
int64_t StepperCtrl::getVelocity(void)
{
int64_t vel;
bool state=enterCriticalSection();
vel=velocity;
exitCriticalSection(state);
return vel;
}
bool StepperCtrl::torqueLoop(int64_t currentLoc, Control_t *ptrCtrl)
{
int32_t U, ma;
int64_t u, y;
int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
y=currentLoc;
ma=motorParams.currentMa;
u=torque*ma/128; // Make ma the maximum torque at 128
// If dir pin changes meaning then torque also changes direction
if (CW_ROTATION == systemParams.dirPinRotation)
{
u=-u;
}
U=abs(u);
if (U>ma)
U=ma;
if (u>0)
y=y+fullStep;
else
y=y-fullStep;
ptrCtrl->ma=U;
ptrCtrl->angle=(int32_t)y;
moveToAngle(y,U);
}
void StepperCtrl::PrintData(void)
{
char s[128];
bool state=enterCriticalSection();
sprintf(s, "%u,%u,%u", (uint32_t)numSteps,(uint32_t)getDesiredAngle(),(uint32_t)getCurrentAngle());
#ifndef MECHADUINO_HARDWARE
SerialUSB.println(s);
#endif
exitCriticalSection(state);
}
//this is the velocity PID feedback loop
bool StepperCtrl::vpidFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
static int64_t lastY=getCurrentLocation();
static int32_t lastError=0;
static int64_t Iterm=0;
int64_t y,z;
int64_t v,dy;
int64_t u;
//get the current location
y =currentLoc;
v=y-lastY;
//add in phase prediction
y=y+calculatePhasePrediction(currentLoc);
z=y;
lastY=y;
v=v*NZS_CONTROL_LOOP_HZ;
if (enableFeedback) //if ((micros()-lastCall)>(updateRate/10))
{
int64_t error,U;
error = velocity-v;
Iterm += (vPID.Ki * error);
if (Iterm>(16*4096*CTRL_PID_SCALING *(int64_t)motorParams.currentMa))
{
Iterm=(16*4096*CTRL_PID_SCALING *(int64_t)motorParams.currentMa);
}
if (Iterm<-(16*4096*CTRL_PID_SCALING *(int64_t)motorParams.currentMa))
{
Iterm=-(16*4096*CTRL_PID_SCALING*(int64_t)motorParams.currentMa);
}
u=((vPID.Kp * error) + Iterm - (vPID.Kd *(lastError-error)));
U=abs(u)/CTRL_PID_SCALING/1024; //scale the error to make PID params close to 1.0;//scale the error to make PID params close to 1.0 by dividing by 1024
if (U>motorParams.currentMa)
{
U=motorParams.currentMa;
}
//when error is positive we need to move reverse direction
if (u>0)
{
z=z+(fullStep);
}else
{
z=z-(fullStep);
}
ptrCtrl->ma=U;
ptrCtrl->angle=(int32_t)z;
moveToAngle(z,U);
loopError=error;
lastError=error;
} else
{
lastError=0;
Iterm=0;
}
if (abs(lastError)>(systemParams.errorLimit))
{
return 1;
}
return 0;
}
//Since we are doing fixed point math our
// threshold needs to be large.
// We need a large threshold when we have fast update
// rate as well. But for most part it is random
bool StepperCtrl::pidFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
static int count=0;
static int32_t maxError=0;
static int32_t lastError=0;
static int32_t Iterm=0;
int32_t ma;
int64_t y;
int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
int32_t dy;
y=currentLoc;
//add in phase prediction
y=y+calculatePhasePrediction(currentLoc);
if (enableFeedback) //if ((micros()-lastCall)>(updateRate/10))
{
int64_t error,u;
int32_t U,x;
//error is in units of degrees when 360 degrees == 65536
error=(desiredLoc-y); //error is currentPos-desiredPos
Iterm+=(pPID.Ki * error);
if (systemParams.homePin>0 && digitalRead(systemParams.homePin)==0)
{
ma=motorParams.homeMa;
} else
{
ma=motorParams.currentMa;
}
//Over the long term we do not want error
// to be much more than our threshold
if (Iterm> (ma*CTRL_PID_SCALING) )
{
Iterm=(ma*CTRL_PID_SCALING) ;
}
if (Iterm<-(ma*CTRL_PID_SCALING) )
{
Iterm=-(ma*CTRL_PID_SCALING) ;
}
//u=((pPID.Kp * error) + Iterm - (pPID.Kd *(lastError-error)));
if(abs(error) > 90304) // 540 degrees
{
u=(int64_t)((float)ma*(float)CTRL_PID_SCALING*90304.0/(float)error);
}
else
{
u=((pPID.Kp * error) + Iterm - (pPID.Kd *(lastError-error)));
}
U=abs(u)/CTRL_PID_SCALING;
if (U>ma)
{
U=ma;
}
//when error is positive we need to move reverse direction
if (u>0)
{
y=y+fullStep;
}else
{
y=y-fullStep;
}
ptrCtrl->ma=U;
ptrCtrl->angle=(int32_t)y;
moveToAngle(y,U);
loopError=error;
lastError=error;
}else
{
lastError=0;
Iterm=0;
}
if (abs(lastError)>(systemParams.errorLimit))
{
return 1;
}
return 0;
}
// the phase prediction tries to predict error from sensor based
// on current location and previous location.
// TODO our error can help in the phase prediction.
// if the error
int64_t StepperCtrl::calculatePhasePrediction(int64_t currentLoc)
{
static int64_t lastLoc=0;
static int32_t mean=0;
int32_t dx,x;
#ifndef ENABLE_PHASE_PREDICTION
return 0;
#endif
//what was our change in the location
dx=currentLoc-lastLoc; //max value is typically less than 327(1.8 degrees) or 163(0.9 degree)
//if the motor direction changes, zero phase prediction
if (SIGN(dx) != SIGN(mean))
{
//last thing we want is phase prediction during direction change.
mean=0;
} else
{
if (abs(dx)>abs(mean))
{
//increase mean really slowly, 2048 ~ 1/3 second with 6khz processing loop
// in fixed point since the dx is so small we need to scale it up to average
// dx has be be greater than 8 to change the mean...
// this limits the acceleration of motor above max processing speed (6k*1.8)=1800RPM
// however I doubt the motor can accelerate that fast with any load...
// The average helps prevent external impulse error from causing prediction to cause issues.
mean=DIVIDE_WITH_ROUND(2047*mean + dx*128, 2048);
}else
{
//decrease fast
//do not add more phase prediction than the difference in last two samples.
mean=dx*128;
}
}
lastLoc=currentLoc;
x= mean/128; //scale back to normal
return x;
}
bool StepperCtrl::determineError(int64_t currentLoc,int64_t error)
{
static int64_t lastLocation=0;
static int64_t lastError=0;
static int64_t lastVelocity=0;
int64_t velocity;
//since this is called on periodic timer the velocity
// is propotional to the change in location
// one rotation per second is velocity of 10, assumming 6khz update rate
// one rotation per minute is 10/60 velocity units
// since this is less than 1 we will scale the velo
velocity=(currentLoc-lastLocation);
if (velocity>0 && lastVelocity>0)
{
}
lastVelocity=velocity;
lastError=error;
lastLocation=currentLoc;
}
//this was written to do the PID loop not modeling a DC servo
// but rather using features of stepper motor.
bool StepperCtrl::simpleFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
static uint32_t t0=0;
static uint32_t calls=0;
bool ret=false;
static int32_t maxError=0;
static int32_t lastError=0;
static int32_t i=0;
static int32_t iTerm=0;
//static int64_t lastY=getCurrentLocation();
static int32_t velocity=0;
static int32_t errorCount=0;
static bool lastProbeState=false;
static int64_t probeStartAngle=0;
static int32_t maxMa=0;
static int64_t filteredError=0;
static int32_t probeCount=0;
int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
int32_t ma=0;
int64_t y;
//estimate our current location based on the encoder
y=currentLoc;
//add in phase prediction
y=y+calculatePhasePrediction(currentLoc);
//we can limit the velocity by controlling the amount we move per call to this function
// this only works for velocity greater than 100rpm
/* if (velocity!=0)
{
fullStep=velocity/NZS_CONTROL_LOOP_HZ;
}
if (fullStep==0)
{
fullStep=1; //this RPM of (1*NZS_CONTROL_LOOP_HZ)/60 ie at 6Khz it is 100RPM
}
*/
if (enableFeedback)
{
int64_t error;
int32_t u;
int32_t x;
int32_t kp;
//error is in units of degrees when 360 degrees == 65536
error=(desiredLoc-y);//measureError(); //error is currentPos-desiredPos
//data[i]=(int16_t)error;
//i++;
if (i>=N_DATA)
{
i=0;
}
kp=sPID.Kp;
if (1)//(abs(error)<(fullStep))
{
iTerm+=(sPID.Ki*error);
x=iTerm/CTRL_PID_SCALING;
}else
{
kp=(CTRL_PID_SCALING*9)/10;
x=0;
iTerm=0;
}
if (x>fullStep)
{
x=fullStep;
iTerm=fullStep;
}
if (x<-fullStep)
{
x=-fullStep;
iTerm=-fullStep;
}
//u=(kp * error)/CTRL_PID_SCALING+x+(sPID.Kd *(error-lastError))/CTRL_PID_SCALING;
// Lower effort if we're very far off
if(abs(error) > 90304) // 540 degrees
{
u=(int64_t)((float)fullStep*90304.0/(float)error);
}
else
{
u=(kp * error)/CTRL_PID_SCALING+x+(sPID.Kd *(error-lastError))/CTRL_PID_SCALING;
}
//limit error to full step
if (u>fullStep)
{
u=fullStep;
}
if (u<-fullStep)
{
u=-fullStep;
}
ma=(abs(u)*(motorParams.currentMa-motorParams.currentHoldMa))/ fullStep + motorParams.currentHoldMa;
if (ma>motorParams.currentMa)
{
ma=motorParams.currentMa;
}
//maxMa=motorParams.currentMa;
if (systemParams.homePin>=0)
{
if (digitalRead(systemParams.homePin)==0)
{
if (lastProbeState==false)
{
//record our current angle for homing
probeStartAngle=desiredLoc;
probeCount=0;
maxMa=0;
}
lastProbeState=true;
probeCount++;
//we will lower current after whe have moved some amount
if (probeCount > NZS_CONTROL_LOOP_HZ && probeCount <(2* NZS_CONTROL_LOOP_HZ))
{
maxMa+=ma;
if (abs(error)>maxError)
{
maxError=abs(error);
}
}
if (probeCount>(2*NZS_CONTROL_LOOP_HZ))
{
// ma=(abs(u)*(maxMa))/ fullStep;// + motorParams.homeHoldMa;
// if (ma>motorParams.homeMa)
// {
// ma=motorParams.homeMa;
// }
//if (ma>maxMa/NZS_CONTROL_LOOP_HZ)
{
ma=((maxMa/NZS_CONTROL_LOOP_HZ)*9)/10;
}
}
} else
{
lastProbeState=false;
}
}else
{
maxError=0;
probeCount=0;
//maxMa=0;
}
y=y+u;
ptrCtrl->ma=ma;
ptrCtrl->angle=(int32_t)y;
moveToAngle(y,ma); //35us
lastError=error;
loopError=error;
//stepperDriver.limitCurrent(99);
}
//filteredError=(filteredError*15+lastError)/16;
if (probeCount>(2*NZS_CONTROL_LOOP_HZ))
{
if (abs(lastError) > maxError )
{
errorCount++;
if (errorCount>(10))
{
return 1;
}
return 0;
}
}
else
{
if (abs(lastError) > systemParams.errorLimit)
{
errorCount++;
if (errorCount>(NZS_CONTROL_LOOP_HZ/128)) // error needs to exist for some time period
{
return 1;
}
return 0;
}
}
if (errorCount>0)
{
errorCount--;
}
//errorCount=0;
stepperDriver.limitCurrent(99); //reduce noise on low error
return 0;
}
void StepperCtrl::currentLocationIsDesiredLocation(){
bool state=enterCriticalSection();
moveToAbsAngle(getCurrentAngle());
exitCriticalSection(state);
}
void StepperCtrl::enable(bool enable)
{
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
bool feedback=enableFeedback;
stepperDriver.enable(enable); //enable or disable the stepper driver as needed
if (enabled==true && enable==false)
{
feedback = false;
}
if (enabled==false && enable==true) //if we are enabling previous disabled motor
{
feedback = true;
setLocationFromEncoder();
}
enabled=enable;
enableFeedback=feedback;
if (state) enableTCInterrupts();
}
/*
void StepperCtrl::testRinging(void)
{
uint16_t c;
int32_t steps;
int32_t microSteps=systemParams.microsteps;
bool feedback=enableFeedback;
enableFeedback=false;
systemParams.microsteps=1;
motorReset();
for (c=2000; c>10; c=c-10)
{
SerialUSB.print("Current ");
SerialUSB.println(c);
steps+=A4954_NUM_MICROSTEPS;
stepperDriver.move(steps,NVM->SystemParams.currentMa);
currentLimit=false;
measure();
}
systemParams.microsteps=microSteps;
motorReset();
enableFeedback=feedback;
}
*/
//returns -1 if no data, else returns number of data points remaining.
int32_t StepperCtrl::getLocation(Location_t *ptrLoc)
{
bool state=enterCriticalSection();
int32_t n;
//check for empty
if (locReadIndx==locWriteIndx)
{
//empty data
exitCriticalSection(state);
return -1;
}
//else read data
memcpy(ptrLoc,(void *)&locs[locReadIndx], sizeof(Location_t));
//update the read index
locReadIndx=(locReadIndx+1)%MAX_NUM_LOCATIONS;
//calculate number of locations left
n=((locWriteIndx+MAX_NUM_LOCATIONS)-locReadIndx)%MAX_NUM_LOCATIONS;
exitCriticalSection(state);
return n;
}
void StepperCtrl::updateLocTable(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
bool state=enterCriticalSection();
int32_t next;
// set the next write location
next=(locWriteIndx+1)%MAX_NUM_LOCATIONS;
if (next==locReadIndx)
{
//we are full, exit
exitCriticalSection(state);
//RED_LED(true); //turn Red LED on to indciate buffer full
return;
}
//use ticks for the moment so we can tell if we miss data on the print.
locs[locWriteIndx].microSecs=(int32_t)micros();
locs[locWriteIndx].desiredLoc=(int32_t)(desiredLoc-zeroAngleOffset);
locs[locWriteIndx].actualLoc=(int32_t)(currentLoc-zeroAngleOffset);
locs[locWriteIndx].angle=(ptrCtrl->angle-zeroAngleOffset);
locs[locWriteIndx].ma=ptrCtrl->ma;
locWriteIndx=next;
exitCriticalSection(state);
}
bool StepperCtrl::processFeedback(void)
{
bool ret;
int32_t us,j;
Control_t ctrl;
int64_t desiredLoc;
int64_t currentLoc;
int32_t steps;
static int64_t mean=0;;
us=micros();
#ifdef USE_TC_STEP
static int64_t lastSteps;
int64_t x;
x=getSteps()-lastSteps;
updateSteps(x);
lastSteps+=x;
#endif
// steps=getSteps();
// if (steps>0)
// {
// requestStep(1, (uint16_t)steps);
// }else
// {
// requestStep(0, (uint16_t)(-steps));
// }
desiredLoc=getDesiredLocation();
currentLoc=getCurrentLocation();
mean=(31*mean+currentLoc+16)/32;
#ifdef A5995_DRIVER //the A5995 is has more driver noise
if (abs(currentLoc-mean)<ANGLE_FROM_DEGREES(0.9))
#else
if (abs(currentLoc-mean)<ANGLE_FROM_DEGREES(0.3))
#endif
{
currentLoc=mean;
}
switch (systemParams.controllerMode)
{
#if defined(CTRL_POS_PID_AS_DEFAULT)
default:
#endif
case CTRL_POS_PID:
{
ret=pidFeedback(desiredLoc, currentLoc, &ctrl);
break;
}
#if !defined(CTRL_POS_PID_AS_DEFAULT)
default:
#endif
case CTRL_SIMPLE:
{
ret=simpleFeedback(desiredLoc, currentLoc,&ctrl);
break;
}
case CTRL_TORQUE:
{
ret=torqueLoop(currentLoc, &ctrl);
break;
}
case CTRL_POS_VELOCITY_PID:
{
ret=vpidFeedback(desiredLoc, currentLoc,&ctrl);
break;
}
//TODO if disable feedback and someone switches mode
// they will have to turn feedback back on.
case CTRL_OFF:
{
enableFeedback=false;
break;
}
case CTRL_OPEN:
{
enableFeedback=false;
break;
}
}
ticks++;
updateLocTable(desiredLoc, currentLoc,&ctrl);
loopTimeus=micros()-us;
return ret;
}
//auto tuning of PID parameters based on documentation here:
// http://brettbeauregard.com/blog/2012/01/arduino-pid-autotune-library
// http://www.controleng.com/search/search-single-display/relay-method-automates-pid-loop-tuning/4a5774decc.html
void StepperCtrl::PID_Autotune(void)
{
int32_t noiseMin, noiseMax, error;
int32_t eMin, eMax;
int64_t mean;
int32_t startAngle, thres;
int32_t i,j;
int32_t t0,t1;
int32_t times[100];
int32_t angle;
//save previous state;
uint16_t microSteps=systemParams.microsteps;
bool feedback=enableFeedback;
feedbackCtrl_t prevCtrl=systemParams.controllerMode;
//disable interrupts and feedback controller
bool state=TC5_ISR_Enabled;
disableTCInterrupts();
systemParams.controllerMode=CTRL_POS_PID;
enableFeedback=false;
motorReset();
//nvmWritePID(1,0,0,2);
//set the number of microsteps to 1
//systemParams.microsteps=1;
for (i=0; i<10; i++)
{
angle=getCurrentLocation();
}
// pKp=NVM->PIDparams.Kp;
// pKi=NVM->PIDparams.Ki;
// pKd=NVM->PIDparams.Kd;
// threshold=NVM->PIDparams.Threshold;
//enableTCInterrupts();
moveToAngle(angle,motorParams.currentMa);
//moveToAngle(angle,NVM->SystemParams.currentMa);
/*
//next lets measure our noise on the encoder
noiseMin=(int32_t)ANGLE_MAX;
noiseMax=-(int32_t)ANGLE_MAX;
mean=0;
j=1000000UL/NZS_CONTROL_LOOP_HZ;
prevAngle=sampleAngle();
for (i=0; i<(NZS_CONTROL_LOOP_HZ/2); i++)
{
Angle a;
a=sampleAngle();
error=(int32_t)(prevAngle-a);
if (error<noiseMin)
{
noiseMin=error;
}
if (error>noiseMax)
{
noiseMax=error;
}
mean=mean+(int32_t)a;
delayMicroseconds(j);
}
mean=mean/i;
while (mean>ANGLE_MAX)
{
mean=mean-ANGLE_STEPS;
}
while (mean<0)
{
mean=mean+ANGLE_STEPS;
}
//mean is the average of the encoder.
*/
stepperDriver.move(0,motorParams.currentMa);
delay(1000);
//now we need to do the relay control
for (i=0; i<10; i++)
{
startAngle=getCurrentLocation();
LOG("Start %d", (int32_t)startAngle);
}
thres=startAngle + (int32_t)((ANGLE_STEPS/motorParams.fullStepsPerRotation)*10/9);
LOG("Thres %d, start %d",(int32_t)thres,(int32_t)startAngle);
eMin=(int32_t)ANGLE_MAX;
eMax=-(int32_t)ANGLE_MAX;
int32_t reset=0;
int32_t force=(motorParams.currentMa);
for (i=0; i<100; i++)
{
int32_t error;
if (reset)
{
motorReset();
stepperDriver.move(0,motorParams.currentMa);
delay(1000);
startAngle=getCurrentLocation();
LOG("Start %d", (int32_t)startAngle);
force=force-100;
eMin=(int32_t)ANGLE_MAX;
eMax=-(int32_t)ANGLE_MAX;
if (force<100)
{
i=100;
break;
}
LOG("force set to %d",force);
i=0;
}
reset=0;
stepperDriver.move(A4954_NUM_MICROSTEPS,force);
//moveToAngle(startAngle+(ANGLE_STEPS/motorParams.fullStepsPerRotation),force);
//stepperDriver.move(A4954_NUM_MICROSTEPS,NVM->SystemParams.currentMa);
t0=micros();
error=0;
while(error<=((ANGLE_STEPS/motorParams.fullStepsPerRotation)/2+40))
{
int32_t y;
y=getCurrentLocation();
error=y-startAngle;
//LOG("Error1 %d",error);
if (error<eMin)
{
eMin=error;
}
if (error>eMax)
{
eMax=error;
}
if (abs(error)>ANGLE_STEPS/motorParams.fullStepsPerRotation*2)
{
LOG("large Error1 %d, %d, %d",error, y, startAngle);
reset=1;
break;
}
}
stepperDriver.move(0,force);
//stepperDriver.move(0,NVM->SystemParams.currentMa);
t1=micros();
error=(ANGLE_STEPS/motorParams.fullStepsPerRotation);
while(error>=((ANGLE_STEPS/motorParams.fullStepsPerRotation)/2-40))
{
error=getCurrentLocation()-startAngle;
//LOG("Error2 %d",error);
if (error<eMin)
{
eMin=error;
}
if (error>eMax)
{
eMax=error;
}
if (abs(error)>ANGLE_STEPS/motorParams.fullStepsPerRotation*2)
{
LOG("large Error2 %d",error);
reset=1;
break;
}
}
times[i]=t1-t0;
}
for (i=0; i<100; i++)
{
LOG("Time %d %d",i,times[i]);
}
LOG("errorMin=%d",eMin);
LOG("errorMax=%d",eMax);
motorReset();
systemParams.controllerMode=prevCtrl;
systemParams.microsteps=microSteps;
enableFeedback=feedback;
if (state) enableTCInterrupts();
}
//void StepperCtrl::printData(void)
//{
// bool state=TC5_ISR_Enabled;
// disableTCInterrupts();
// int32_t i;
// for(i=0; i<N_DATA; i++)
// {
// LOG ("%d\n",data[i]);
// }
//
// if (state) enableTCInterrupts();
//
//}
#pragma GCC pop_options
Reference in New Issue View Git Blame Copy Permalink