From 5e0b0e05cdd91cfe2230e71d32cdab317acf4173 Mon Sep 17 00:00:00 2001 From: misift Date: Mon, 16 Oct 2017 12:21:24 -0400 Subject: [PATCH] FW 0.25 added pin read command --- firmware/stepper_nano_zero/board.h | 3 +- firmware/stepper_nano_zero/commands.cpp | 1559 ++++++++++++----------- 2 files changed, 802 insertions(+), 760 deletions(-) diff --git a/firmware/stepper_nano_zero/board.h b/firmware/stepper_nano_zero/board.h index 7692833..4c7287e 100644 --- a/firmware/stepper_nano_zero/board.h +++ b/firmware/stepper_nano_zero/board.h @@ -44,7 +44,7 @@ //#define ENABLE_PHASE_PREDICTION //this enables prediction of phase at high velocity to increase motor speed //as of FW0.11 it is considered development only -#define VERSION "FW: 0.24" //this is what prints on LCD during splash screen +#define VERSION "FW: 0.25" //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 @@ -121,6 +121,7 @@ * 0.22 - Added home command; * 0.23 -- added motor voltage sense to remove stepping on power up * 0.24 - Disabled the home command which used the enable pin if you do not have enable pin + * 0.25 - Added pin read command * */ diff --git a/firmware/stepper_nano_zero/commands.cpp b/firmware/stepper_nano_zero/commands.cpp index 7dc0d96..6b7499d 100644 --- a/firmware/stepper_nano_zero/commands.cpp +++ b/firmware/stepper_nano_zero/commands.cpp @@ -18,9 +18,9 @@ sCmdUart SerialUart; static int isPowerOfTwo (unsigned int x) { - while (((x % 2) == 0) && x > 1) /* While x is even and > 1 */ - x /= 2; - return (x == 1); + while (((x % 2) == 0) && x > 1) /* While x is even and > 1 */ + x /= 2; + return (x == 1); } @@ -34,11 +34,11 @@ CMD_STR(feedback, "enable or disable feedback controller, 'feedback 0' - disable CMD_STR(readpos, "reads the current angle as 16bit number, applies calibration if valid"); CMD_STR(encoderdiag, "Prints encoder diagnostic") CMD_STR(spid, "with no arguments prints SIMPLE PID parameters, with arguments sets PID 'sPID Kp Ki Kd' " - "Where Kp,Ki,Kd are floating point numbers"); + "Where Kp,Ki,Kd are floating point numbers"); CMD_STR(vpid, "with no arguments prints VELOCITY PID parameters, with arguments sets PID 'sPID Kp Ki Kd' " - "Where Kp,Ki,Kd are floating point numbers"); + "Where Kp,Ki,Kd are floating point numbers"); CMD_STR(ppid, "with no arguments prints POSITIONAL PID parameters, with arguments sets PID 'sPID Kp Ki Kd' " - "Where Kp,Ki,Kd are floating point numbers"); + "Where Kp,Ki,Kd are floating point numbers"); //CMD_STR(testringing ,"Steps motor at various currents and measures encoder"); //CMD_STR(microsteperror ,"test error on microstepping") CMD_STR(dirpin, "with no arguments read dirpin setting, with argument sets direction pin rotation"); @@ -75,106 +75,147 @@ CMD_STR(reboot, "reboots the unit") #ifdef PIN_ENABLE CMD_STR(home, "moves the motor until home switch (enable pin) is pulled low. example 'home 360 0.5' move up to 360 degrees at 0.5 RPM ") #endif +CMD_STR(pinread, "reads pins as binary (bit 0-step, bit 1 - Dir, bit 2 - Enable, bit 3 - Error, bit 4 - A3, bit 5- TX, bit 6 - RX") //List of supported commands sCommand Cmds[] = { - COMMAND(help), - COMMAND(calibrate), - COMMAND(getcal), - COMMAND(testcal), - COMMAND(microsteps), - COMMAND(step), - COMMAND(feedback), - COMMAND(readpos), - COMMAND(encoderdiag), - COMMAND(spid), - COMMAND(vpid), - COMMAND(ppid), - //COMMAND(testringing), - //COMMAND(microsteperror), - COMMAND(dirpin), + COMMAND(help), + COMMAND(calibrate), + COMMAND(getcal), + COMMAND(testcal), + COMMAND(microsteps), + COMMAND(step), + COMMAND(feedback), + COMMAND(readpos), + COMMAND(encoderdiag), + COMMAND(spid), + COMMAND(vpid), + COMMAND(ppid), + //COMMAND(testringing), + //COMMAND(microsteperror), + COMMAND(dirpin), #ifndef PIN_ENABLE - COMMAND(errorpinmode), + COMMAND(errorpinmode), #else - COMMAND(enablepinmode), + COMMAND(enablepinmode), #endif - COMMAND(errorlimit), - COMMAND(ctrlmode), - COMMAND(maxcurrent), - COMMAND(holdcurrent), - COMMAND(motorwiring), - COMMAND(stepsperrotation), - - //COMMAND(sysparams), - //COMMAND(motorparams), - COMMAND(boot), - COMMAND(move), - //COMMAND(printdata), - COMMAND(velocity), - COMMAND(factoryreset), - COMMAND(stop), - COMMAND(setzero), - COMMAND(data), - COMMAND(looptime), - COMMAND(eepromerror), - COMMAND(eepromloc), - COMMAND(eepromwrite), - COMMAND(setpos), - COMMAND(reboot), - COMMAND(eepromsetloc), + COMMAND(errorlimit), + COMMAND(ctrlmode), + COMMAND(maxcurrent), + COMMAND(holdcurrent), + COMMAND(motorwiring), + COMMAND(stepsperrotation), + + //COMMAND(sysparams), + //COMMAND(motorparams), + COMMAND(boot), + COMMAND(move), + //COMMAND(printdata), + COMMAND(velocity), + COMMAND(factoryreset), + COMMAND(stop), + COMMAND(setzero), + COMMAND(data), + COMMAND(looptime), + COMMAND(eepromerror), + COMMAND(eepromloc), + COMMAND(eepromwrite), + COMMAND(setpos), + COMMAND(reboot), + COMMAND(eepromsetloc), #ifdef PIN_ENABLE - COMMAND(home), + COMMAND(home), #endif - {"",0,""}, //End of list signal + COMMAND(pinread), + {"",0,""}, //End of list signal }; +static int pinread_cmd(sCmdUart *ptrUart,int argc, char * argv[]) +{ + uint8_t ret=0; + + if (digitalRead(PIN_STEP_INPUT)) + { + ret |= 0x01; + } + if (digitalRead(PIN_DIR_INPUT)) + { + ret |= 0x02; + } +#ifdef PIN_ENABLE + if (digitalRead(PIN_ENABLE)) + { + ret |= 0x04; + } +#endif + if (digitalRead(PIN_ERROR)) + { + ret |= 0x08; + } + if (digitalRead(analogInputToDigitalPin(PIN_A3))) + { + ret |= 0x10; + } + if (digitalRead(30)) + { + ret |= 0x20; + } + if (digitalRead(31)) + { + ret |= 0x40; + } + CommandPrintf(ptrUart,"%02X\n\r",ret); + return 0; +} + #ifdef PIN_ENABLE static void errorPinISR(void) { SmartPlanner.stop(); //stop the planner } + static int home_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - float rpm=1; - float startDegrees=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); - float finalDegrees=startDegrees+360.0; - char str[20]; - float deg; - - if (argc>=1) - { - finalDegrees=startDegrees+atof(argv[0]); - } - - if (argc>=2) - { - rpm=atof(argv[1]); - } - - //setup a interrupt for the enable pin - attachInterrupt(digitalPinToInterrupt(PIN_ENABLE), errorPinISR, FALLING); - - SmartPlanner.moveConstantVelocity(finalDegrees,rpm); - - while(!SmartPlanner.done()) - { - //do nothing - } - detachInterrupt(digitalPinToInterrupt(PIN_ENABLE)); - deg=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); - ftoa(deg,str,2,'f'); - CommandPrintf(ptrUart,"home is %s deg\n\r",str); - stepperCtrl.setZero(); - - return 0; + float rpm=1; + float startDegrees=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); + float finalDegrees=startDegrees+360.0; + char str[20]; + float deg; + + if (argc>=1) + { + finalDegrees=startDegrees+atof(argv[0]); + } + + if (argc>=2) + { + rpm=atof(argv[1]); + } + + //setup a interrupt for the enable pin + attachInterrupt(digitalPinToInterrupt(PIN_ENABLE), errorPinISR, FALLING); + + SmartPlanner.moveConstantVelocity(finalDegrees,rpm); + + while(!SmartPlanner.done()) + { + //do nothing + } + detachInterrupt(digitalPinToInterrupt(PIN_ENABLE)); + deg=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); + ftoa(deg,str,2,'f'); + CommandPrintf(ptrUart,"home is %s deg\n\r",str); + stepperCtrl.setZero(); + + return 0; } #endif static int reboot_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - NVIC_SystemReset(); - return 0; + NVIC_SystemReset(); + return 0; } static int setpos_cmd(sCmdUart *ptrUart,int argc, char * argv[]) @@ -213,23 +254,23 @@ static int eepromerror_cmd(sCmdUart *ptrUart,int argc, char * argv[]) deg=ANGLE_T0_DEGREES((uint16_t)a) ; if (deg>360.0) { - deg=deg-360.0; + deg=deg-360.0; } ftoa(deg,str,2,'f'); - CommandPrintf(ptrUart,"startup error(+/-) %s deg\n\r",str); - - a=(Angle)PowerupEEPROM.encoderAngle; - a=(a-(Angle)stepperCtrl.getEncoderAngle()); - deg=ANGLE_T0_DEGREES((uint16_t)a); - if (deg>360.0) - { - deg=deg-360.0; - } - ftoa(deg,str,2,'f'); - CommandPrintf(ptrUart,"current error(+/-) %s deg\n\r",str); - - return 0; + CommandPrintf(ptrUart,"startup error(+/-) %s deg\n\r",str); + + a=(Angle)PowerupEEPROM.encoderAngle; + a=(a-(Angle)stepperCtrl.getEncoderAngle()); + deg=ANGLE_T0_DEGREES((uint16_t)a); + if (deg>360.0) + { + deg=deg-360.0; + } + ftoa(deg,str,2,'f'); + CommandPrintf(ptrUart,"current error(+/-) %s deg\n\r",str); + + return 0; } static int eepromsetloc_cmd(sCmdUart *ptrUart,int argc, char * argv[]) @@ -243,7 +284,7 @@ static int eepromsetloc_cmd(sCmdUart *ptrUart,int argc, char * argv[]) deg=PowerupEEPROM.angle+x; stepperCtrl.setAngle(deg); - return 0; + return 0; } static int eepromloc_cmd(sCmdUart *ptrUart,int argc, char * argv[]) @@ -257,40 +298,40 @@ static int eepromloc_cmd(sCmdUart *ptrUart,int argc, char * argv[]) deg=(deg*360*100)/(int32_t)ANGLE_STEPS; x=(deg)/100; y=abs(deg-(x*100)); - CommandPrintf(ptrUart,"%d.%0.2d deg\n\r",x,y); - return 0; + CommandPrintf(ptrUart,"%d.%0.2d deg\n\r",x,y); + return 0; } static int looptime_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - CommandPrintf(ptrUart,"%dus",stepperCtrl.getLoopTime()); - return 0; + CommandPrintf(ptrUart,"%dus",stepperCtrl.getLoopTime()); + return 0; } static int setzero_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - stepperCtrl.setZero(); - return 0; + stepperCtrl.setZero(); + return 0; } static int stop_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - SmartPlanner.stop(); - return 0; + SmartPlanner.stop(); + return 0; } static int data_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 1) - { - uint32_t x; - - x=atol(argv[0]); - dataEnabled=x; - return 0; - } - return 1; + if (argc == 1) + { + uint32_t x; + + x=atol(argv[0]); + dataEnabled=x; + return 0; + } + return 1; } @@ -298,471 +339,471 @@ static int data_cmd(sCmdUart *ptrUart,int argc, char * argv[]) static int stepsperrotation_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 0) - { - uint32_t x; - x=NVM->motorParams.fullStepsPerRotation; - CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x); - return 0; - } + if (argc == 0) + { + uint32_t x; + x=NVM->motorParams.fullStepsPerRotation; + CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x); + return 0; + } - if (argc == 1) - { - uint32_t x; + if (argc == 1) + { + uint32_t x; - x=atol(argv[0]); + x=atol(argv[0]); - if (x==200 || x==400) - { - MotorParams_t motorParams; + if (x==200 || x==400) + { + MotorParams_t motorParams; - memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); - motorParams.fullStepsPerRotation=x; + memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); + motorParams.fullStepsPerRotation=x; - nvmWriteMotorParms(motorParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteMotorParms(motorParams); + stepperCtrl.updateParamsFromNVM(); - x=NVM->motorParams.fullStepsPerRotation; - CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x); - CommandPrintf(ptrUart,"please power cycle board\n\r"); - return 0; - } + x=NVM->motorParams.fullStepsPerRotation; + CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x); + CommandPrintf(ptrUart,"please power cycle board\n\r"); + return 0; + } - } - CommandPrintf(ptrUart,"usage 'stepsperrotation 200' or 'stepsperrotation 400'\n\r"); + } + CommandPrintf(ptrUart,"usage 'stepsperrotation 200' or 'stepsperrotation 400'\n\r"); - return 1; + return 1; } static int motorwiring_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 0) - { - uint32_t x; - x=NVM->motorParams.motorWiring; - CommandPrintf(ptrUart,"motor wiring %u\n\r",x); - return 0; - } + if (argc == 0) + { + uint32_t x; + x=NVM->motorParams.motorWiring; + CommandPrintf(ptrUart,"motor wiring %u\n\r",x); + return 0; + } - if (argc == 1) - { - uint32_t x; + if (argc == 1) + { + uint32_t x; - x=atol(argv[0]); + x=atol(argv[0]); - if (x<=1) - { - MotorParams_t motorParams; + if (x<=1) + { + MotorParams_t motorParams; - memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); - motorParams.motorWiring=x; + memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); + motorParams.motorWiring=x; - nvmWriteMotorParms(motorParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteMotorParms(motorParams); + stepperCtrl.updateParamsFromNVM(); - x=NVM->motorParams.motorWiring; - CommandPrintf(ptrUart,"motor wiring %u\n\r",x); - CommandPrintf(ptrUart,"please power cycle board\n\r"); - return 0; - } + x=NVM->motorParams.motorWiring; + CommandPrintf(ptrUart,"motor wiring %u\n\r",x); + CommandPrintf(ptrUart,"please power cycle board\n\r"); + return 0; + } - } - CommandPrintf(ptrUart,"usage 'motorwiring 0' or 'motorwiring 1'\n\r"); + } + CommandPrintf(ptrUart,"usage 'motorwiring 0' or 'motorwiring 1'\n\r"); - return 1; + return 1; } static int holdcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 0) - { - uint32_t x; - x=NVM->motorParams.currentHoldMa; - CommandPrintf(ptrUart,"hold current %u mA\n\r",x); - return 0; - } + if (argc == 0) + { + uint32_t x; + x=NVM->motorParams.currentHoldMa; + CommandPrintf(ptrUart,"hold current %u mA\n\r",x); + return 0; + } - if (argc == 1) - { - uint32_t x; + if (argc == 1) + { + uint32_t x; - x=atol(argv[0]); + x=atol(argv[0]); - MotorParams_t motorParams; + MotorParams_t motorParams; - memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); - motorParams.currentHoldMa=x; + memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); + motorParams.currentHoldMa=x; - nvmWriteMotorParms(motorParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteMotorParms(motorParams); + stepperCtrl.updateParamsFromNVM(); - x=NVM->motorParams.currentHoldMa; - CommandPrintf(ptrUart,"hold current %u mA\n\r",x); - return 0; + x=NVM->motorParams.currentHoldMa; + CommandPrintf(ptrUart,"hold current %u mA\n\r",x); + return 0; - } - CommandPrintf(ptrUart, "use 'holdcurrent 1000' to set maximum current to 1.0A"); + } + CommandPrintf(ptrUart, "use 'holdcurrent 1000' to set maximum current to 1.0A"); - return 1; + return 1; } static int maxcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 0) - { - uint32_t x; - x=NVM->motorParams.currentMa; - CommandPrintf(ptrUart,"max current %u mA\n\r",x); - return 0; - } + if (argc == 0) + { + uint32_t x; + x=NVM->motorParams.currentMa; + CommandPrintf(ptrUart,"max current %u mA\n\r",x); + return 0; + } - if (argc == 1) - { - uint32_t x; + if (argc == 1) + { + uint32_t x; - x=atol(argv[0]); + x=atol(argv[0]); - MotorParams_t motorParams; + MotorParams_t motorParams; - memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); + memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) ); - motorParams.currentMa=x; - nvmWriteMotorParms(motorParams); - stepperCtrl.updateParamsFromNVM(); + motorParams.currentMa=x; + nvmWriteMotorParms(motorParams); + stepperCtrl.updateParamsFromNVM(); - x=NVM->motorParams.currentMa; - CommandPrintf(ptrUart,"max current %u mA\n\r",x); - return 0; + x=NVM->motorParams.currentMa; + CommandPrintf(ptrUart,"max current %u mA\n\r",x); + return 0; - } - CommandPrintf(ptrUart, "use 'maxcurrent 2000' to set maximum current to 2.0A"); + } + CommandPrintf(ptrUart, "use 'maxcurrent 2000' to set maximum current to 2.0A"); - return 1; + return 1; } static int ctrlmode_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; - if (argc == 0) - { - switch(NVM->SystemParams.controllerMode) - { - case CTRL_OFF: - CommandPrintf(ptrUart,"controller Off(0)"); - return 0; - case CTRL_OPEN: - CommandPrintf(ptrUart,"controller Open-loop(1)"); - return 0; - case CTRL_SIMPLE: - CommandPrintf(ptrUart,"controller Simple-Position-PID(2)"); - return 0; - case CTRL_POS_PID: - CommandPrintf(ptrUart,"controller Current-Position-PID(3)"); - return 0; - case CTRL_POS_VELOCITY_PID: - CommandPrintf(ptrUart,"controller Velocity-PID(4)"); - return 0; - - } - return 1; - } - - if (argc == 1) - { - uint32_t x; - - x=atol(argv[0]); - - if (x<=4) - { - SystemParams_t systemParams; - - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - - systemParams.controllerMode=(feedbackCtrl_t)(x); - - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); - - switch(NVM->SystemParams.controllerMode) - { - case CTRL_OFF: - CommandPrintf(ptrUart,"controller Off(0)"); - return 0; - case CTRL_OPEN: - CommandPrintf(ptrUart,"controller Open-loop(1)"); - return 0; - case CTRL_SIMPLE: - CommandPrintf(ptrUart,"controller Simple-Position-PID(2)"); - return 0; - case CTRL_POS_PID: - CommandPrintf(ptrUart,"controller Current-Position-PID(3)"); - return 0; - case CTRL_POS_VELOCITY_PID: - CommandPrintf(ptrUart,"controller Velocity-PID(4)"); - return 0; - - } - return 1; - } - - } - CommandPrintf(ptrUart, "use 'ctrlmode [0 .. 4]' to set control mode"); - - return 1; + bool ret; + if (argc == 0) + { + switch(NVM->SystemParams.controllerMode) + { + case CTRL_OFF: + CommandPrintf(ptrUart,"controller Off(0)"); + return 0; + case CTRL_OPEN: + CommandPrintf(ptrUart,"controller Open-loop(1)"); + return 0; + case CTRL_SIMPLE: + CommandPrintf(ptrUart,"controller Simple-Position-PID(2)"); + return 0; + case CTRL_POS_PID: + CommandPrintf(ptrUart,"controller Current-Position-PID(3)"); + return 0; + case CTRL_POS_VELOCITY_PID: + CommandPrintf(ptrUart,"controller Velocity-PID(4)"); + return 0; + + } + return 1; + } + + if (argc == 1) + { + uint32_t x; + + x=atol(argv[0]); + + if (x<=4) + { + SystemParams_t systemParams; + + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + + systemParams.controllerMode=(feedbackCtrl_t)(x); + + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); + + switch(NVM->SystemParams.controllerMode) + { + case CTRL_OFF: + CommandPrintf(ptrUart,"controller Off(0)"); + return 0; + case CTRL_OPEN: + CommandPrintf(ptrUart,"controller Open-loop(1)"); + return 0; + case CTRL_SIMPLE: + CommandPrintf(ptrUart,"controller Simple-Position-PID(2)"); + return 0; + case CTRL_POS_PID: + CommandPrintf(ptrUart,"controller Current-Position-PID(3)"); + return 0; + case CTRL_POS_VELOCITY_PID: + CommandPrintf(ptrUart,"controller Velocity-PID(4)"); + return 0; + + } + return 1; + } + + } + CommandPrintf(ptrUart, "use 'ctrlmode [0 .. 4]' to set control mode"); + + return 1; } static int errorlimit_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; - char str[20]; - if (argc == 0) - { - float x; - x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit); - ftoa(x,str,2,'f'); - CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str); - return 0; - } + bool ret; + char str[20]; + if (argc == 0) + { + float x; + x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit); + ftoa(x,str,2,'f'); + CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str); + return 0; + } - if (argc == 1) - { - float x; + if (argc == 1) + { + float x; - x=fabs(atof(argv[0])); + x=fabs(atof(argv[0])); - SystemParams_t systemParams; + SystemParams_t systemParams; - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - systemParams.errorLimit=ANGLE_FROM_DEGREES(x); + systemParams.errorLimit=ANGLE_FROM_DEGREES(x); - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); - x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit); - ftoa(x,str,2,'f'); - CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str); - return 0; + x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit); + ftoa(x,str,2,'f'); + CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str); + return 0; - } - CommandPrintf(ptrUart, "use 'errorlimit 1.8' to set error limit to 1.8 degrees"); + } + CommandPrintf(ptrUart, "use 'errorlimit 1.8' to set error limit to 1.8 degrees"); - return 1; + return 1; } static int dirpin_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; + bool ret; - if (argc == 0) - { - if (CW_ROTATION == NVM->SystemParams.dirPinRotation) - { - CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); - }else - { - CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); - } - return 0; - } + if (argc == 0) + { + if (CW_ROTATION == NVM->SystemParams.dirPinRotation) + { + CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); + }else + { + CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); + } + return 0; + } - if (argc == 1) - { - uint32_t x; + if (argc == 1) + { + uint32_t x; - x=abs(atol(argv[0])); - if (x<=1) - { + x=abs(atol(argv[0])); + if (x<=1) + { - SystemParams_t systemParams; + SystemParams_t systemParams; - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - systemParams.dirPinRotation=(RotationDir_t)x; + systemParams.dirPinRotation=(RotationDir_t)x; - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); - if (CW_ROTATION == NVM->SystemParams.dirPinRotation) - { - CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); - }else - { - CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); - } - return 0; + if (CW_ROTATION == NVM->SystemParams.dirPinRotation) + { + CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); + }else + { + CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation); + } + return 0; - } - } - CommandPrintf(ptrUart, "used 'dirpin 0' for CW rotation and 'dirpin 1' for CCW"); + } + } + CommandPrintf(ptrUart, "used 'dirpin 0' for CW rotation and 'dirpin 1' for CCW"); - return 1; + return 1; } #ifndef PIN_ENABLE static int errorpinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; - - if (argc == 0) - { - if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } - - return 0; - } - - if (argc == 1) - { - uint32_t x; - - x=abs(atol(argv[0])); - if (x<=3) - { - - SystemParams_t systemParams; - - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - - systemParams.errorPinMode=(ErrorPinMode_t)x; - - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); - - if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Error pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } - return 0; - - } - } - CommandPrintf(ptrUart, "use 'errorpinmode 0' for enable active high, 'errorpinmode 1' for enable active low and 'errorpinmode 2' for error output" ); - - - return 1; + bool ret; + + if (argc == 0) + { + if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } + + return 0; + } + + if (argc == 1) + { + uint32_t x; + + x=abs(atol(argv[0])); + if (x<=3) + { + + SystemParams_t systemParams; + + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + + systemParams.errorPinMode=(ErrorPinMode_t)x; + + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); + + if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Error pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } + return 0; + + } + } + CommandPrintf(ptrUart, "use 'errorpinmode 0' for enable active high, 'errorpinmode 1' for enable active low and 'errorpinmode 2' for error output" ); + + + return 1; } #else static int enablepinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; - - if (argc == 0) - { - if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } else - { - CommandPrintf(ptrUart,"UNDEFINED Pin Mode error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } - - return 0; - } - - if (argc == 1) - { - uint32_t x; - - x=abs(atol(argv[0])); - - if (x<=1) - { - - SystemParams_t systemParams; - - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - - systemParams.errorPinMode=(ErrorPinMode_t)x; - - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); - - if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - }else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) - { - CommandPrintf(ptrUart,"Enable pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); - } - return 0; - - } - } - CommandPrintf(ptrUart, "use 'enablepinmode 0' for enable active high, 'enablepinmode 1' for enable active low " ); - - - return 1; + bool ret; + + if (argc == 0) + { + if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } else + { + CommandPrintf(ptrUart,"UNDEFINED Pin Mode error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } + + return 0; + } + + if (argc == 1) + { + uint32_t x; + + x=abs(atol(argv[0])); + + if (x<=1) + { + + SystemParams_t systemParams; + + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + + systemParams.errorPinMode=(ErrorPinMode_t)x; + + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); + + if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + }else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode) + { + CommandPrintf(ptrUart,"Enable pin - Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode); + } + return 0; + + } + } + CommandPrintf(ptrUart, "use 'enablepinmode 0' for enable active high, 'enablepinmode 1' for enable active low " ); + + + return 1; } #endif static int factoryreset_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - nvmErase(); //erase all of the flash - NVIC_SystemReset(); + nvmErase(); //erase all of the flash + NVIC_SystemReset(); } static int velocity_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - int64_t x; + int64_t x; - if (1 == argc) - { - float rpm; - rpm=atof(argv[0]); - x=(int64_t)(DIVIDE_WITH_ROUND(rpm*ANGLE_STEPS,60)); //divide with r + if (1 == argc) + { + float rpm; + rpm=atof(argv[0]); + x=(int64_t)(DIVIDE_WITH_ROUND(rpm*ANGLE_STEPS,60)); //divide with r - stepperCtrl.setVelocity(x); - } - int64_t y; - x=(stepperCtrl.getVelocity()*100 *60)/(ANGLE_STEPS); - y=abs(x-((x/100)*100)); - CommandPrintf(ptrUart,"Velocity is %d.%02d - %d\n\r",(int32_t)(x/100),(int32_t)y,(int32_t)stepperCtrl.getVelocity()); + stepperCtrl.setVelocity(x); + } + int64_t y; + x=(stepperCtrl.getVelocity()*100 *60)/(ANGLE_STEPS); + y=abs(x-((x/100)*100)); + CommandPrintf(ptrUart,"Velocity is %d.%02d - %d\n\r",(int32_t)(x/100),(int32_t)y,(int32_t)stepperCtrl.getVelocity()); - return 0; + return 0; } // @@ -778,72 +819,72 @@ static int velocity_cmd(sCmdUart *ptrUart,int argc, char * argv[]) static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - int32_t x,ma; - //CommandPrintf(ptrUart, "Move %d",argc); - - if (1 == argc) - { - float f; - - f=atof(argv[0]); - // if (f>1.8) - // f=1.8; - // if (f<-1.8) - // f=-1.8; - x=ANGLE_FROM_DEGREES(f); - LOG("moving %d", x); - - stepperCtrl.moveToAbsAngle(x); - } - if (2 == argc) - { - float f,rpm,a,y; - float pos,dx; - - f=atof(argv[0]); - rpm=atof(argv[1]); - // if (f>1.8) - // f=1.8; - // if (f<-1.8) - // f=-1.8; - - SmartPlanner.moveConstantVelocity(f,rpm); - return 0; - a=360*rpm/60/1000; //rotations/100ms - - pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); - y=pos; - if (y>f) a=-a; - - SerialUSB.println(f); - SerialUSB.println(y); - SerialUSB.println(a); - - while (abs(y-f)>(2*abs(a))) - { - // SerialUSB.println(); - // SerialUSB.println(f); - // SerialUSB.println(y); - // SerialUSB.println(a); - y=y+a; - - x=ANGLE_FROM_DEGREES(y); - //LOG("moving %d", x); - stepperCtrl.moveToAbsAngle(x); - delay(1); - //y=stepperCtrl.getCurrentAngle(); - } - x=ANGLE_FROM_DEGREES(f); - LOG("moving %d", x); - stepperCtrl.moveToAbsAngle(x); - } - - return 0; + int32_t x,ma; + //CommandPrintf(ptrUart, "Move %d",argc); + + if (1 == argc) + { + float f; + + f=atof(argv[0]); + // if (f>1.8) + // f=1.8; + // if (f<-1.8) + // f=-1.8; + x=ANGLE_FROM_DEGREES(f); + LOG("moving %d", x); + + stepperCtrl.moveToAbsAngle(x); + } + if (2 == argc) + { + float f,rpm,a,y; + float pos,dx; + + f=atof(argv[0]); + rpm=atof(argv[1]); + // if (f>1.8) + // f=1.8; + // if (f<-1.8) + // f=-1.8; + + SmartPlanner.moveConstantVelocity(f,rpm); + return 0; + a=360*rpm/60/1000; //rotations/100ms + + pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); + y=pos; + if (y>f) a=-a; + + SerialUSB.println(f); + SerialUSB.println(y); + SerialUSB.println(a); + + while (abs(y-f)>(2*abs(a))) + { + // SerialUSB.println(); + // SerialUSB.println(f); + // SerialUSB.println(y); + // SerialUSB.println(a); + y=y+a; + + x=ANGLE_FROM_DEGREES(y); + //LOG("moving %d", x); + stepperCtrl.moveToAbsAngle(x); + delay(1); + //y=stepperCtrl.getCurrentAngle(); + } + x=ANGLE_FROM_DEGREES(f); + LOG("moving %d", x); + stepperCtrl.moveToAbsAngle(x); + } + + return 0; } static int boot_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - initiateReset(250); + initiateReset(250); } /* @@ -965,275 +1006,275 @@ static int motorparams_cmd(sCmdUart *ptrUart,int argc, char * argv[]) */ static int vpid_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - CommandPrintf(ptrUart, "args %d\n\r",argc); - if (0 == argc) - { - int32_t x,y; - x=(int32_t)NVM->vPID.Kp; - y=abs(1000*NVM->vPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->vPID.Ki; - y=abs(1000*NVM->vPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->vPID.Kd; - y=abs(1000*NVM->vPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - if (3 == argc) - { - float Kp,Ki,Kd; - int32_t x,y; - - Kp=atof(argv[0]); - Ki=atof(argv[1]); - Kd=atof(argv[2]); - - nvmWrite_vPID(Kp,Ki,Kd); - stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters - - x=(int32_t)NVM->vPID.Kp; - y=abs(1000*NVM->vPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->vPID.Ki; - y=abs(1000*NVM->vPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->vPID.Kd; - y=abs(1000*NVM->vPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - return 0; + CommandPrintf(ptrUart, "args %d\n\r",argc); + if (0 == argc) + { + int32_t x,y; + x=(int32_t)NVM->vPID.Kp; + y=abs(1000*NVM->vPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->vPID.Ki; + y=abs(1000*NVM->vPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->vPID.Kd; + y=abs(1000*NVM->vPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + if (3 == argc) + { + float Kp,Ki,Kd; + int32_t x,y; + + Kp=atof(argv[0]); + Ki=atof(argv[1]); + Kd=atof(argv[2]); + + nvmWrite_vPID(Kp,Ki,Kd); + stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters + + x=(int32_t)NVM->vPID.Kp; + y=abs(1000*NVM->vPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->vPID.Ki; + y=abs(1000*NVM->vPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->vPID.Kd; + y=abs(1000*NVM->vPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + return 0; } static int ppid_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (0 == argc) - { - int32_t x,y; - x=(int32_t)NVM->pPID.Kp; - y=abs(1000*NVM->pPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->pPID.Ki; - y=abs(1000*NVM->pPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->pPID.Kd; - y=abs(1000*NVM->pPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - if (3 == argc) - { - float Kp,Ki,Kd; - int32_t x,y; - - Kp=atof(argv[0]); - Ki=atof(argv[1]); - Kd=atof(argv[2]); - - nvmWrite_pPID(Kp,Ki,Kd); - stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters - - x=(int32_t)NVM->pPID.Kp; - y=abs(1000*NVM->pPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->pPID.Ki; - y=abs(1000*NVM->pPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->pPID.Kd; - y=abs(1000*NVM->pPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - return 0; + if (0 == argc) + { + int32_t x,y; + x=(int32_t)NVM->pPID.Kp; + y=abs(1000*NVM->pPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->pPID.Ki; + y=abs(1000*NVM->pPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->pPID.Kd; + y=abs(1000*NVM->pPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + if (3 == argc) + { + float Kp,Ki,Kd; + int32_t x,y; + + Kp=atof(argv[0]); + Ki=atof(argv[1]); + Kd=atof(argv[2]); + + nvmWrite_pPID(Kp,Ki,Kd); + stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters + + x=(int32_t)NVM->pPID.Kp; + y=abs(1000*NVM->pPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->pPID.Ki; + y=abs(1000*NVM->pPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->pPID.Kd; + y=abs(1000*NVM->pPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + return 0; } static int spid_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (0 == argc) - { - int32_t x,y; - x=(int32_t)NVM->sPID.Kp; - y=abs(1000*NVM->sPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->sPID.Ki; - y=abs(1000*NVM->sPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->sPID.Kd; - y=abs(1000*NVM->sPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - if (3 == argc) - { - float Kp,Ki,Kd; - int32_t x,y; - - Kp=atof(argv[0]); - Ki=atof(argv[1]); - Kd=atof(argv[2]); - - nvmWrite_sPID(Kp,Ki,Kd); - stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters - - x=(int32_t)NVM->sPID.Kp; - y=abs(1000*NVM->sPID.Kp-(x*1000)); - CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); - - x=(int32_t)NVM->sPID.Ki; - y=abs(1000*NVM->sPID.Ki-(x*1000)); - CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); - - x=(int32_t)NVM->sPID.Kd; - y=abs(1000*NVM->sPID.Kd-(x*1000)); - CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); - } - return 0; + if (0 == argc) + { + int32_t x,y; + x=(int32_t)NVM->sPID.Kp; + y=abs(1000*NVM->sPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->sPID.Ki; + y=abs(1000*NVM->sPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->sPID.Kd; + y=abs(1000*NVM->sPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + if (3 == argc) + { + float Kp,Ki,Kd; + int32_t x,y; + + Kp=atof(argv[0]); + Ki=atof(argv[1]); + Kd=atof(argv[2]); + + nvmWrite_sPID(Kp,Ki,Kd); + stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters + + x=(int32_t)NVM->sPID.Kp; + y=abs(1000*NVM->sPID.Kp-(x*1000)); + CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y); + + x=(int32_t)NVM->sPID.Ki; + y=abs(1000*NVM->sPID.Ki-(x*1000)); + CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y); + + x=(int32_t)NVM->sPID.Kd; + y=abs(1000*NVM->sPID.Kd-(x*1000)); + CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y); + } + return 0; } static int encoderdiag_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - char str[512]; - stepperCtrl.encoderDiagnostics(str); - CommandPrintf(ptrUart,"%s",str); - return 0; + char str[512]; + stepperCtrl.encoderDiagnostics(str); + CommandPrintf(ptrUart,"%s",str); + return 0; } static int readpos_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - float pos; - int32_t x,y; - - pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); - x=int(pos); - y=abs((pos-x)*100); - CommandPrintf(ptrUart,"encoder %d.%02d",x,y); - return 0; + float pos; + int32_t x,y; + + pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle()); + x=int(pos); + y=abs((pos-x)*100); + CommandPrintf(ptrUart,"encoder %d.%02d",x,y); + return 0; } static int feedback_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (0 == argc) - { - CommandPrintf(ptrUart,"must pass argument, 'feedback 0' - disables, 'feedback 1' - enables"); - return 1; - } - stepperCtrl.feedback(atoi(argv[0])); - return 0; + if (0 == argc) + { + CommandPrintf(ptrUart,"must pass argument, 'feedback 0' - disables, 'feedback 1' - enables"); + return 1; + } + stepperCtrl.feedback(atoi(argv[0])); + return 0; } static int step_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - if (argc == 0 ) - { - stepperCtrl.move(0, 1); - //stepperCtrl.step(STEPPER_FORWARD); - }else - { - int d, steps=1; - d=atoi(argv[0]); - if (argc >1) - { - steps=atoi(argv[1]); - } - if (1 == d) - { - stepperCtrl.move(1, steps); - } else - { - stepperCtrl.move(0, steps); - } - } - return 0; + if (argc == 0 ) + { + stepperCtrl.move(0, 1); + //stepperCtrl.step(STEPPER_FORWARD); + }else + { + int d, steps=1; + d=atoi(argv[0]); + if (argc >1) + { + steps=atoi(argv[1]); + } + if (1 == d) + { + stepperCtrl.move(1, steps); + } else + { + stepperCtrl.move(0, steps); + } + } + return 0; } static int microsteps_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - bool ret; + bool ret; - if (argc != 1) - { - CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps); - return 0; - } + if (argc != 1) + { + CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps); + return 0; + } - int32_t x; + int32_t x; - x=atol(argv[0]); - if (isPowerOfTwo(x) && x>0 && x<=256) - { - SystemParams_t systemParams; + x=atol(argv[0]); + if (isPowerOfTwo(x) && x>0 && x<=256) + { + SystemParams_t systemParams; - memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); + memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) ); - systemParams.microsteps=atol(argv[0]); + systemParams.microsteps=atol(argv[0]); - nvmWriteSystemParms(systemParams); - stepperCtrl.updateParamsFromNVM(); + nvmWriteSystemParms(systemParams); + stepperCtrl.updateParamsFromNVM(); - CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps); + CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps); - }else - { - CommandPrintf(ptrUart,"number of microsteps must be a power of 2 between 1 and 256"); - return 1; //return error - } + }else + { + CommandPrintf(ptrUart,"number of microsteps must be a power of 2 between 1 and 256"); + return 1; //return error + } - return 0; + return 0; } // print out the help strings for the commands static int help_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - sCommand cmd_list; - int i; - - //now let's parse the command - i=0; - memcpy(&cmd_list, &Cmds[i], sizeof(sCommand)); - while(cmd_list.function!=0) - { - - CommandPrintf(ptrUart,(cmd_list.name)); - CommandPrintf(ptrUart,(" - ")); - CommandPrintf(ptrUart,(cmd_list.help)); - CommandPrintf(ptrUart,("\n\r")); - i=i+1; - memcpy(&cmd_list, &Cmds[i], sizeof(sCommand)); - } - return 0; + sCommand cmd_list; + int i; + + //now let's parse the command + i=0; + memcpy(&cmd_list, &Cmds[i], sizeof(sCommand)); + while(cmd_list.function!=0) + { + + CommandPrintf(ptrUart,(cmd_list.name)); + CommandPrintf(ptrUart,(" - ")); + CommandPrintf(ptrUart,(cmd_list.help)); + CommandPrintf(ptrUart,("\n\r")); + i=i+1; + memcpy(&cmd_list, &Cmds[i], sizeof(sCommand)); + } + return 0; } static int getcal_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - stepperCtrl.calTable.printCalTable(); - return 0; + stepperCtrl.calTable.printCalTable(); + return 0; } static int calibrate_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - stepperCtrl.calibrateEncoder(); - CommandPrintf(ptrUart,"Calibration DONE!\n\r"); - return 0; + stepperCtrl.calibrateEncoder(); + CommandPrintf(ptrUart,"Calibration DONE!\n\r"); + return 0; } static int testcal_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - Angle a; - int32_t x; + Angle a; + int32_t x; - a=stepperCtrl.maxCalibrationError(); - x=(uint16_t)a*(int32_t)360000L/(int32_t)ANGLE_MAX; + a=stepperCtrl.maxCalibrationError(); + x=(uint16_t)a*(int32_t)360000L/(int32_t)ANGLE_MAX; - CommandPrintf(ptrUart,"Max error is %d.%03d degrees\n\r", x/1000,abs(x)%1000); - return 0; + CommandPrintf(ptrUart,"Max error is %d.%03d degrees\n\r", x/1000,abs(x)%1000); + return 0; } @@ -1241,45 +1282,45 @@ static int testcal_cmd(sCmdUart *ptrUart,int argc, char * argv[]) uint8_t kbhit(void) { - return SerialUSB.available(); - //return SerialUSB.peek() != -1; + return SerialUSB.available(); + //return SerialUSB.peek() != -1; } uint8_t getChar(void) { - return SerialUSB.read(); + return SerialUSB.read(); } uint8_t putch(char data) { - return SerialUSB.write((uint8_t)data); + return SerialUSB.write((uint8_t)data); } uint8_t kbhit_hw(void) { - return Serial5.available(); - //return SerialUSB.peek() != -1; + return Serial5.available(); + //return SerialUSB.peek() != -1; } uint8_t getChar_hw(void) { - return Serial5.read(); + return Serial5.read(); } uint8_t putch_hw(char data) { - return Serial5.write((uint8_t)data); + return Serial5.write((uint8_t)data); } void commandsInit(void) { - CommandInit(&UsbUart, kbhit, getChar, putch ,NULL); //set up the UART structure + CommandInit(&UsbUart, kbhit, getChar, putch ,NULL); //set up the UART structure #ifdef CMD_SERIAL_PORT - CommandInit(&SerialUart, kbhit_hw, getChar_hw, putch_hw ,NULL); //set up the UART structure - Serial5.print("\n\rPower Up\n\r"); - Serial5.print(COMMANDS_PROMPT); + CommandInit(&SerialUart, kbhit_hw, getChar_hw, putch_hw ,NULL); //set up the UART structure + Serial5.print("\n\rPower Up\n\r"); + Serial5.print(COMMANDS_PROMPT); #endif - SerialUSB.print("\n\rPower Up\n\r"); - SerialUSB.print(COMMANDS_PROMPT); + SerialUSB.print("\n\rPower Up\n\r"); + SerialUSB.print(COMMANDS_PROMPT); } int commandsProcess(void) @@ -1287,5 +1328,5 @@ int commandsProcess(void) #ifdef CMD_SERIAL_PORT CommandProcess(&SerialUart,Cmds,' ',COMMANDS_PROMPT); #endif - return CommandProcess(&UsbUart,Cmds,' ',COMMANDS_PROMPT); + return CommandProcess(&UsbUart,Cmds,' ',COMMANDS_PROMPT); }