From 695f743cfd4ee8b339c38a82dfeb5ff6e0f32520 Mon Sep 17 00:00:00 2001 From: David Ochoa Date: Thu, 24 May 2018 17:15:37 -0500 Subject: [PATCH] Implemented Constant Acceleration Move --- firmware/stepper_nano_zero/board.h | 13 ++-- firmware/stepper_nano_zero/commands.cpp | 27 ++++++++- firmware/stepper_nano_zero/nzs.cpp | 1 - firmware/stepper_nano_zero/nzs_lcd.cpp | 1 + firmware/stepper_nano_zero/planner.cpp | 79 +++++++++++++++++++++++++ firmware/stepper_nano_zero/planner.h | 5 +- 6 files changed, 117 insertions(+), 9 deletions(-) diff --git a/firmware/stepper_nano_zero/board.h b/firmware/stepper_nano_zero/board.h index c2652b0..40845dd 100644 --- a/firmware/stepper_nano_zero/board.h +++ b/firmware/stepper_nano_zero/board.h @@ -19,10 +19,10 @@ //uncomment the follow lines if using the NEMA 23 10A hardware -#define NEMA_23_10A_HW +//#define NEMA_23_10A_HW //uncomment the following if the board uses the A5995 driver (NEMA 23 3.2A boards) -//#define A5995_DRIVER +#define A5995_DRIVER //The March 21 2017 NEMA 17 Smart Stepper has changed some pin outs // A1 was changed to read motor voltage, hence SW4 is now using D4 @@ -42,7 +42,7 @@ #define NZS_AS5047_PIPELINE //does a pipeline read of encoder, which is slightly faster -#define NZS_CONTROL_LOOP_HZ (6000) //update rate of control loop +#define NZS_CONTROL_LOOP_HZ (4000) //update rate of control loop #define NZS_LCD_ABSOULTE_ANGLE //define this to show angle from zero in positive and negative direction @@ -54,14 +54,14 @@ #define VERSION "FW: 0.34" //this is what prints on LCD during splash screen //Define this to allow command out serial port, else hardware serial is debug log -//#define CMD_SERIAL_PORT +// #define CMD_SERIAL_PORT #define SERIAL_BAUD (115200) //baud rate for the serial ports //This section is for using the step and dir pins as serial port // when the enable pin is inactive. -#define USE_STEP_DIR_SERIAL -#define STEP_DIR_BAUD (19200) //this is the baud rate we will use +// #define USE_STEP_DIR_SERIAL +// #define STEP_DIR_BAUD (19200) //this is the baud rate we will use // These are used as an attempt to use TC4 to count steps // currently this is not working. @@ -504,3 +504,4 @@ static inline void DelayMs(uint32_t ms) #endif//__BOARD_H__ + diff --git a/firmware/stepper_nano_zero/commands.cpp b/firmware/stepper_nano_zero/commands.cpp index c39db25..64f4ba7 100644 --- a/firmware/stepper_nano_zero/commands.cpp +++ b/firmware/stepper_nano_zero/commands.cpp @@ -4,7 +4,7 @@ #include "stepper_controller.h" #include #include "nonvolatile.h" -#include "reset.h" +#include "Reset.h" #include "nzs.h" #include "ftoa.h" #include "board.h" @@ -62,6 +62,7 @@ CMD_STR(stepsperrotation, "gets/set the motor steps per rotation, should only be //CMD_STR(motorparams, "with no arguments read parameters, will set with arguments"); CMD_STR(boot, "Enters the bootloader"); CMD_STR(move, "moves encoder to absolute angle in degrees 'move 400.1'"); +CMD_STR(moveCA, "constant Accel move to a absolute angles 'move pos maxrpm'"); //CMD_STR(printdata, "prints last n error terms"); CMD_STR(velocity, "gets/set velocity in RPMs"); CMD_STR(factoryreset, "resets board to factory defaults"); @@ -119,6 +120,7 @@ sCommand Cmds[] = //COMMAND(motorparams), COMMAND(boot), COMMAND(move), + COMMAND(moveCA), //COMMAND(printdata), COMMAND(velocity), COMMAND(factoryreset), @@ -1054,6 +1056,28 @@ static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[]) return 0; } +static int moveCA_cmd(sCmdUart *ptrUart,int argc, char * argv[]) +{ + int32_t x,ma; + //CommandPrintf(ptrUart, "Move %d",argc); + + if (2 == argc) + { + float f,maxrpm,a,y; + float pos,dx; + + f=atof(argv[0]); + maxrpm=atof(argv[1]); + // if (f>1.8) + // f=1.8; + // if (f<-1.8) + // f=-1.8; + + SmartPlanner.moveConstantAccel(f,maxrpm); + } + + return 0; +} static int boot_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { initiateReset(250); @@ -1532,3 +1556,4 @@ int commandsProcess(void) #endif return CommandProcess(&UsbUart,Cmds,' ',COMMANDS_PROMPT); } + diff --git a/firmware/stepper_nano_zero/nzs.cpp b/firmware/stepper_nano_zero/nzs.cpp index 0b58efe..ce1f18a 100644 --- a/firmware/stepper_nano_zero/nzs.cpp +++ b/firmware/stepper_nano_zero/nzs.cpp @@ -664,7 +664,6 @@ void NZS::begin(void) #else Lcd.lcdShow("MisfitTech","NEMA 17", VERSION); #endif - #endif LOG("command init!"); diff --git a/firmware/stepper_nano_zero/nzs_lcd.cpp b/firmware/stepper_nano_zero/nzs_lcd.cpp index cfe6f7b..50616d3 100644 --- a/firmware/stepper_nano_zero/nzs_lcd.cpp +++ b/firmware/stepper_nano_zero/nzs_lcd.cpp @@ -586,3 +586,4 @@ void NZS_LCD::updateLCD(void) } + diff --git a/firmware/stepper_nano_zero/planner.cpp b/firmware/stepper_nano_zero/planner.cpp index b00c8fc..b274739 100644 --- a/firmware/stepper_nano_zero/planner.cpp +++ b/firmware/stepper_nano_zero/planner.cpp @@ -130,6 +130,40 @@ void Planner::tick(void) } } + if (PLANNER_CA == 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; + float t; + if (fabs(currentSetAngle-endAngle) >= 0.5) + { + t = fabs((currentSetAngle-endAngle) / (startAngle-endAngle)); + if (t < 0.5) + { + currentVelocity=maxVelocity*t*2; + } + else + { + currentVelocity=2*maxVelocity-maxVelocity*t*2; + } + currentSetAngle+=currentVelocity; + 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; + } + } } @@ -186,3 +220,48 @@ bool Planner::moveConstantVelocity(float finalAngle, float rpm) exitTC3CriticalSection(state); return true; } + + +bool Planner::moveConstantAccel(float finalAngle, float maxrpm) +{ + bool state; + state = enterTC3CriticalSection(); + + //first determine if operation is in progress + if (PLANNER_NONE != currentMode) + { + //we are in operation return false + SerialUSB.println("planner operational"); + exitTC3CriticalSection(state); + return false; + } + + //get current posistion + startAngle = ANGLE_T0_DEGREES(ptrStepperCtrl->getCurrentAngle()); + + //deterime the max velocity + maxVelocity=369.0*fabs(maxrpm)/60/PLANNER_UPDATE_RATE_HZ; + + //set the desired end angle + endAngle=finalAngle; + + //set the current angle + currentSetAngle=startAngle+4; + + if (startAngle>endAngle) + { + SerialUSB.println("reverse"); + maxVelocity=-maxVelocity; + currentSetAngle=startAngle-4; + } + +// SerialUSB.println(currentSetAngle); +// SerialUSB.println(endAngle); +// SerialUSB.println(tickIncrement); +// SerialUSB.println(); + + currentMode=PLANNER_CA; + + exitTC3CriticalSection(state); + return true; +} diff --git a/firmware/stepper_nano_zero/planner.h b/firmware/stepper_nano_zero/planner.h index 6a127d1..4ee4514 100644 --- a/firmware/stepper_nano_zero/planner.h +++ b/firmware/stepper_nano_zero/planner.h @@ -27,7 +27,7 @@ typedef enum { PLANNER_NONE =0, PLANNER_CV =1, //constant velocity - //PLANNER_CA =2, //constant accleration + PLANNER_CA =2, //constant accleration //PLANNER_S_CURVE =3, //s-curve move } PlannerMode; class Planner @@ -40,10 +40,13 @@ class Planner volatile float startAngle; volatile float currentSetAngle; volatile float tickIncrement; + volatile float currentVelocity; + volatile float maxVelocity; public: void begin(StepperCtrl *ptrStepper); bool moveConstantVelocity(float finalAngle, float rpm); //moves to the final location at a constant RPM + bool moveConstantAccel(float finalAngle, float maxrpm); //moves to the final location at a constant RPM void tick(void); //this is called on regulat tick interval void stop(void); bool done(void) {return currentMode==PLANNER_NONE;}