From f3bf91d9d7aa1a4661db6ce19c4cedcaa3ab41d6 Mon Sep 17 00:00:00 2001 From: trampas Date: Fri, 25 Nov 2016 17:03:19 -0500 Subject: [PATCH 1/2] FW.04 - put in more interrupt safe checking Add move and readpos commands to be in absolute degrees LCD is now in absolute degrees --- firmware/{ => stepper_nano_zero}/.cproject | 0 firmware/{ => stepper_nano_zero}/.project | 0 .../.settings/language.settings.xml | 15 +++ firmware/{ => stepper_nano_zero}/A4954.cpp | 8 ++ firmware/{ => stepper_nano_zero}/A4954.h | 2 + firmware/{ => stepper_nano_zero}/Flash.cpp | 0 firmware/{ => stepper_nano_zero}/Flash.h | 0 firmware/{ => stepper_nano_zero}/angle.h | 0 firmware/{ => stepper_nano_zero}/as5047d.cpp | 0 firmware/{ => stepper_nano_zero}/as5047d.h | 0 firmware/{ => stepper_nano_zero}/board.h | 31 +++-- .../{ => stepper_nano_zero}/calibration.cpp | 0 .../{ => stepper_nano_zero}/calibration.h | 0 firmware/{ => stepper_nano_zero}/command.cpp | 0 firmware/{ => stepper_nano_zero}/command.h | 0 firmware/{ => stepper_nano_zero}/commands.cpp | 88 ++++++-------- firmware/{ => stepper_nano_zero}/commands.h | 0 .../{ => stepper_nano_zero}/nonvolatile.cpp | 3 +- .../{ => stepper_nano_zero}/nonvolatile.h | 0 .../stepper_controller.cpp | 113 ++++++++++++++---- .../stepper_controller.h | 14 ++- .../stepper_nano_zero.ino | 41 ++++--- firmware/{ => stepper_nano_zero}/syslog.cpp | 0 firmware/{ => stepper_nano_zero}/syslog.h | 0 24 files changed, 215 insertions(+), 100 deletions(-) rename firmware/{ => stepper_nano_zero}/.cproject (100%) rename firmware/{ => stepper_nano_zero}/.project (100%) create mode 100644 firmware/stepper_nano_zero/.settings/language.settings.xml rename firmware/{ => stepper_nano_zero}/A4954.cpp (99%) rename firmware/{ => stepper_nano_zero}/A4954.h (95%) rename firmware/{ => stepper_nano_zero}/Flash.cpp (100%) rename firmware/{ => stepper_nano_zero}/Flash.h (100%) rename firmware/{ => stepper_nano_zero}/angle.h (100%) rename firmware/{ => stepper_nano_zero}/as5047d.cpp (100%) rename firmware/{ => stepper_nano_zero}/as5047d.h (100%) rename firmware/{ => stepper_nano_zero}/board.h (77%) rename firmware/{ => stepper_nano_zero}/calibration.cpp (100%) rename firmware/{ => stepper_nano_zero}/calibration.h (100%) rename firmware/{ => stepper_nano_zero}/command.cpp (100%) rename firmware/{ => stepper_nano_zero}/command.h (100%) rename firmware/{ => stepper_nano_zero}/commands.cpp (83%) rename firmware/{ => stepper_nano_zero}/commands.h (100%) rename firmware/{ => stepper_nano_zero}/nonvolatile.cpp (53%) rename firmware/{ => stepper_nano_zero}/nonvolatile.h (100%) rename firmware/{ => stepper_nano_zero}/stepper_controller.cpp (92%) rename firmware/{ => stepper_nano_zero}/stepper_controller.h (93%) rename firmware/{ => stepper_nano_zero}/stepper_nano_zero.ino (70%) rename firmware/{ => stepper_nano_zero}/syslog.cpp (100%) rename firmware/{ => stepper_nano_zero}/syslog.h (100%) diff --git a/firmware/.cproject b/firmware/stepper_nano_zero/.cproject similarity index 100% rename from firmware/.cproject rename to firmware/stepper_nano_zero/.cproject diff --git a/firmware/.project b/firmware/stepper_nano_zero/.project similarity index 100% rename from firmware/.project rename to firmware/stepper_nano_zero/.project diff --git a/firmware/stepper_nano_zero/.settings/language.settings.xml b/firmware/stepper_nano_zero/.settings/language.settings.xml new file mode 100644 index 0000000..c3aa351 --- /dev/null +++ b/firmware/stepper_nano_zero/.settings/language.settings.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/firmware/A4954.cpp b/firmware/stepper_nano_zero/A4954.cpp similarity index 99% rename from firmware/A4954.cpp rename to firmware/stepper_nano_zero/A4954.cpp index 4ec2258..232da24 100644 --- a/firmware/A4954.cpp +++ b/firmware/stepper_nano_zero/A4954.cpp @@ -416,6 +416,14 @@ int32_t A4954::move(int32_t stepAngle, uint32_t mA) int32_t cos,sin; int32_t dacSin,dacCos; + + if (enabled == false) + { + setDAC(0,0); //turn current off + bridge1(3); //tri state bridge outputs + bridge2(3); //tri state bridge outputs + } + //WARNING("move %d %d",stepAngle,mA); //handle roll overs, could do with modulo operator stepAngle=stepAngle%SINE_STEPS; diff --git a/firmware/A4954.h b/firmware/stepper_nano_zero/A4954.h similarity index 95% rename from firmware/A4954.h rename to firmware/stepper_nano_zero/A4954.h index 413c687..03f497a 100644 --- a/firmware/A4954.h +++ b/firmware/stepper_nano_zero/A4954.h @@ -28,6 +28,7 @@ class A4954 private: uint32_t lastStepMicros; // time in microseconds that last step happened bool forwardRotation=true; + volatile bool enabled=true; public: void begin(void); @@ -38,6 +39,7 @@ class A4954 uint32_t microsSinceStep(void) {return micros()-lastStepMicros;}; void setRotationDirection(bool forward) {forwardRotation=forward;}; + void enable(bool enable); void limitCurrent(uint8_t percent); //higher more current }; diff --git a/firmware/Flash.cpp b/firmware/stepper_nano_zero/Flash.cpp similarity index 100% rename from firmware/Flash.cpp rename to firmware/stepper_nano_zero/Flash.cpp diff --git a/firmware/Flash.h b/firmware/stepper_nano_zero/Flash.h similarity index 100% rename from firmware/Flash.h rename to firmware/stepper_nano_zero/Flash.h diff --git a/firmware/angle.h b/firmware/stepper_nano_zero/angle.h similarity index 100% rename from firmware/angle.h rename to firmware/stepper_nano_zero/angle.h diff --git a/firmware/as5047d.cpp b/firmware/stepper_nano_zero/as5047d.cpp similarity index 100% rename from firmware/as5047d.cpp rename to firmware/stepper_nano_zero/as5047d.cpp diff --git a/firmware/as5047d.h b/firmware/stepper_nano_zero/as5047d.h similarity index 100% rename from firmware/as5047d.h rename to firmware/stepper_nano_zero/as5047d.h diff --git a/firmware/board.h b/firmware/stepper_nano_zero/board.h similarity index 77% rename from firmware/board.h rename to firmware/stepper_nano_zero/board.h index bd4cee3..9bb30a8 100644 --- a/firmware/board.h +++ b/firmware/stepper_nano_zero/board.h @@ -6,22 +6,34 @@ #define NZS_FAST_CAL // define this to use 32k of flash for fast calibration table #define NZS_FAST_SINE //uses 2048 extra bytes to implement faster sine tables -/* does not work at moment +/* DOES NOT WORK at moment //#define NZS_AS5047_PIPELINE //does a pipeline read of encoder, which is slightly faster - * DOES NOT WORK + * DOES NOT WORK, not sure why yet, but might be SPI bus remains enabled? */ #define NZS_CONTROL_LOOP_HZ (4000) //update rate of control loop, this should be limited to less than 5k + +#define NZS_LCD_ABSOULTE_ANGLE //define this to show angle from zero in positive and negative direction + // for example 2 rotations from start will be angle of 720 degrees + +#define VERSION "FW: 0.04" //this is what prints on LCD during splash screen + +/* change log + * 0.02 added fixes for 0.9 degree motor + * 0.03 added code for using error pin as an enable pin, enable feedback by default + */ + +//#define USE_ENABLE_PIN //define this to use the error pin as enable + +// ******** TIMER USAGE ************ //TCC0 is used for DAC PWM to the A4954 //TCC1 can be used as PWM for the input pins on the A4954 - -#define VERSION "FW: 0.02" +//TC5 is use for timing the control loop #define PIN_STEP_INPUT (0) #define PIN_DIR_INPUT (1) -#define PIN_ERROR_INPUT (10) -#define PIN_ERROR_OUTPUT (12) +#define PIN_ERROR (10) #define PIN_AS5047D_CS (16)//analogInputToDigitalPin(PIN_A2)) #define PIN_AS5047D_PWR (11) //pull low to power on AS5047D @@ -58,8 +70,11 @@ static void boardSetupPins(void) pinMode(PIN_STEP_INPUT, INPUT_PULLDOWN); pinMode(PIN_DIR_INPUT, INPUT_PULLDOWN); - pinMode(PIN_ERROR_INPUT, INPUT_PULLDOWN); - pinMode(PIN_ERROR_OUTPUT, OUTPUT); +#ifdef USE_ENABLE_PIN + pinMode(PIN_ERROR, INPUT_PULLDOWN); +#else + pinMode(PIN_ERROR, OUTPUT); +#endif pinMode(PIN_AS5047D_CS,OUTPUT); digitalWrite(PIN_AS5047D_CS,LOW); //pull CS LOW by default (chip powered off) diff --git a/firmware/calibration.cpp b/firmware/stepper_nano_zero/calibration.cpp similarity index 100% rename from firmware/calibration.cpp rename to firmware/stepper_nano_zero/calibration.cpp diff --git a/firmware/calibration.h b/firmware/stepper_nano_zero/calibration.h similarity index 100% rename from firmware/calibration.h rename to firmware/stepper_nano_zero/calibration.h diff --git a/firmware/command.cpp b/firmware/stepper_nano_zero/command.cpp similarity index 100% rename from firmware/command.cpp rename to firmware/stepper_nano_zero/command.cpp diff --git a/firmware/command.h b/firmware/stepper_nano_zero/command.h similarity index 100% rename from firmware/command.h rename to firmware/stepper_nano_zero/command.h diff --git a/firmware/commands.cpp b/firmware/stepper_nano_zero/commands.cpp similarity index 83% rename from firmware/commands.cpp rename to firmware/stepper_nano_zero/commands.cpp index 7744014..09792c1 100644 --- a/firmware/commands.cpp +++ b/firmware/stepper_nano_zero/commands.cpp @@ -21,57 +21,44 @@ CMD_STR(readpos, "reads the current angle as 16bit number, applies calibration i CMD_STR(encoderdiag, "Prints encoder diagnositc") CMD_STR(pid, "with no arguments prints PID parameters, with arguments sets PID 'PID Kp Ki Kd Threshold' " "Where Kp,Ki,Kd,Threshold are int32_t numbers") -CMD_STR(testringing ,"Steps motor at various currents and measures encoder") -CMD_STR(microsteperror ,"test error on microstepping") -CMD_STR(params, "with no arguments read parameters, will set 'params currentMa holdCurrentMa errorLimit'") -CMD_STR(boot, "Enters the bootloader") -CMD_STR(move, "moves encoder x degrees at y ma 'move x y'") -CMD_STR(test, "test") -//List of supported commands -sCommand Cmds[] = -{ - COMMAND(help), - COMMAND(calibrate), - COMMAND(getcal), - COMMAND(testcal), - COMMAND(microstep), - COMMAND(step), - COMMAND(feedback), - COMMAND(readpos), - COMMAND(encoderdiag), - COMMAND(pid), - COMMAND(testringing), - COMMAND(microsteperror), - COMMAND(params), - COMMAND(boot), - COMMAND(move), - COMMAND(test), - {"",0,""}, //End of list signal -}; - -static int test_cmd(sCmdUart *ptrUart,int argc, char * argv[]) -{ - int32_t x=0;; - int32_t i; + CMD_STR(testringing ,"Steps motor at various currents and measures encoder") + CMD_STR(microsteperror ,"test error on microstepping") + CMD_STR(params, "with no arguments read parameters, will set 'params currentMa holdCurrentMa errorLimit'") + CMD_STR(boot, "Enters the bootloader") + CMD_STR(move, "moves encoder to absolute angle in degrees 'move 400.1'") + + //List of supported commands + sCommand Cmds[] = + { + COMMAND(help), + COMMAND(calibrate), + COMMAND(getcal), + COMMAND(testcal), + COMMAND(microstep), + COMMAND(step), + COMMAND(feedback), + COMMAND(readpos), + COMMAND(encoderdiag), + COMMAND(pid), + COMMAND(testringing), + COMMAND(microsteperror), + COMMAND(params), + COMMAND(boot), + COMMAND(move), + + {"",0,""}, //End of list signal + }; - for (i=0; i<100; i++) - { - x=stepperCtrl.getCurrentLocation()+300; - LOG("x is %d, %d",x,stepperCtrl.getCurrentLocation()); - stepperCtrl.moveToAngle(x,2000); - delay(100); - stepperCtrl.process(); - } - return 0; -} static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { int32_t x,ma; CommandPrintf(ptrUart, "Move %d",argc); - if (2 == argc) + + if (1 == argc) { float f; + f=atof(argv[0]); // if (f>1.8) // f=1.8; @@ -79,10 +66,9 @@ static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[]) // f=-1.8; x=ANGLE_FROM_DEGREES(f); LOG("moving %d", x); - ma=atoi(argv[1]); - - stepperCtrl.moveToAngle(x,ma); + stepperCtrl.moveToAbsAngle(x); } + return 0; } @@ -187,11 +173,13 @@ static int encoderdiag_cmd(sCmdUart *ptrUart,int argc, char * argv[]) static int readpos_cmd(sCmdUart *ptrUart,int argc, char * argv[]) { - uint16_t pos,a; - int32_t x; + float pos; + int32_t x,y; - pos=stepperCtrl.measureMeanEncoder(); - CommandPrintf(ptrUart,"encoder %d",pos); + pos=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[]) diff --git a/firmware/commands.h b/firmware/stepper_nano_zero/commands.h similarity index 100% rename from firmware/commands.h rename to firmware/stepper_nano_zero/commands.h diff --git a/firmware/nonvolatile.cpp b/firmware/stepper_nano_zero/nonvolatile.cpp similarity index 53% rename from firmware/nonvolatile.cpp rename to firmware/stepper_nano_zero/nonvolatile.cpp index 2f61b8f..096bb38 100644 --- a/firmware/nonvolatile.cpp +++ b/firmware/stepper_nano_zero/nonvolatile.cpp @@ -12,7 +12,8 @@ __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[16640]={ #else __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[256]={ //allocates 512 bytes #endif - 40504,40841,41170,41504,41834,42169,42497,42833,43162,43498,43825,44162,44490,44826,45153,45490,45818,46154,46481,46816,47144,47478,47805,48139,48 3,49454,49777,50107,50430,50758,51079,51407,51726,52052,52369,52695,53012,53337,53653,53978,54292,54617,54931,55255,55569,55894,56210,56535,56849, 816,58132,58458,58776,59104,59422,59752,60069,60399,60719,61049,61371,61701,62023,62355,62679,63011,63334,63668,63991,64325,64650,64985,65310,111, 433,1762,2097,2426,2760,3088,3423,3753,4087,4417,4753,5082,5418,5749,6083,6414,6750,7081,7415,7747,8081,8412,8745,9077,9410,9740,10073,10403,10738 1728,12061,12388,12720,13047,13380,13706,14035,14360,14692,15015,15344,15669,15997,16319,16649,16970,17299,17618,17949,18269,18598,18918,19248,195 ,20546,20865,21194,21514,21842,22160,22488,22804,23133,23450,23778,24093,24420,24737,25063,25378,25706,26021,26349,26664,26991,27307,27635,27952,2 28,29246,29577,29896,30227,30548,30881,31203,31537,31860,32197,32521,32857,33183,33520,33847,34185,34512,34850,35178,35516,35845,36182,36511,36847 7842,38179,38508,38844,39174,39510,39839,40174, + + 25125,25451,25805,26119,26469,26783,27123,27428,27769,28076,28410,28710,29046,29349,29680,29980,30316,30624,30962,31267,31611,31925,32268,32579,32929,33248,33596,33911,34263,34580,34925,35234,35578,35890,36227,36531,36869,37178,37516,37821,38163,38476,38818,39128,39476,39792,40137,40450,40802,41120,41469,41784,42135,42453,42801,43114,43464,43778,44123,44433,44779,45089,45428,45734,46073,46381,46720,47029,47374,47690,48038,48353,48708,49029,49379,49695,50044,50361,50700,51004,51341,51643,51971,52268,52602,52906,53241,53546,53891,54203,54547,54859,55209,55527,55877,56191,56544,56863,57212,57528,57883,58206,58561,58882,59239,59562,59911,60220,60563,60873,61208,61509,61846,62152,62490,62797,63140,63454,63797,64108,64451,64763,65098,65398,198,499,826,1122,1453,1758,2092,2398,2739,3053,3398,3710,4060,4379,4729,5046,5401,5725,6081,6405,6766,7096,7456,7777,8135,8458,8807,9119,9465,9779,10119,10424,10768,11077,11413,11719,12061,12369,12705,13008,13346,13650,13980,14277,14608,14907,15233,15528,15857,16155,16484,16781,17114,17421,17756,18061,18404,18718,19057,19367,19714,20028,20372,20683,21031,21347,21694,22010,22368,22692,23050,23376,23742,24074,24438,24763, 0xFFFF //for the valid flag }; diff --git a/firmware/nonvolatile.h b/firmware/stepper_nano_zero/nonvolatile.h similarity index 100% rename from firmware/nonvolatile.h rename to firmware/stepper_nano_zero/nonvolatile.h diff --git a/firmware/stepper_controller.cpp b/firmware/stepper_nano_zero/stepper_controller.cpp similarity index 92% rename from firmware/stepper_controller.cpp rename to firmware/stepper_nano_zero/stepper_controller.cpp index 897ddae..0e9914d 100644 --- a/firmware/stepper_controller.cpp +++ b/firmware/stepper_nano_zero/stepper_controller.cpp @@ -8,6 +8,7 @@ #define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY); volatile bool TC5_ISR_Enabled=false; + void setupTCInterrupts() { @@ -55,6 +56,7 @@ 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 } @@ -62,7 +64,8 @@ void enableTCInterrupts() { void disableTCInterrupts() { TC5_ISR_Enabled=false; - NVIC_DisableIRQ(TC5_IRQn); + //NVIC_DisableIRQ(TC5_IRQn); + TC5->COUNT16.INTENCLR.bit.OVF = 1; } @@ -137,7 +140,12 @@ void StepperCtrl::setLocationFromEncoder(void) void StepperCtrl::encoderDiagnostics(char *ptrStr) { + bool state=TC5_ISR_Enabled; + disableTCInterrupts(); + encoder.diagnostics(ptrStr); + + if (state) enableTCInterrupts(); } @@ -213,8 +221,11 @@ int32_t StepperCtrl::measureError(void) bool StepperCtrl::changeMicrostep(uint16_t microSteps) { + bool state=TC5_ISR_Enabled; + disableTCInterrupts(); numMicroSteps=microSteps; motorReset(); + if (state) enableTCInterrupts(); return true; } @@ -377,6 +388,8 @@ void StepperCtrl::UpdateLcd(void) static int32_t RPM=0; int32_t lasttime=0; int32_t d; + bool state; + display.clearDisplay(); @@ -385,17 +398,19 @@ void StepperCtrl::UpdateLcd(void) display.setTextSize(2); display.setTextColor(WHITE); + state=TC5_ISR_Enabled; disableTCInterrupts(); - lastAngle=getCurrentLocation(); + lastAngle=getDesiredLocation(); lasttime=millis(); - enableTCInterrupts(); + if (state) enableTCInterrupts(); delay(100); + state=TC5_ISR_Enabled; disableTCInterrupts(); - deg=getCurrentLocation(); + deg=getDesiredLocation(); err=measureError(); y=millis()-lasttime; - enableTCInterrupts(); + if (state) enableTCInterrupts(); @@ -439,6 +454,7 @@ void StepperCtrl::UpdateLcd(void) //LOG("error is %d",err); + err=(err*360*100)/(int32_t)ANGLE_STEPS; x=err/100; y=abs(err-x*100); @@ -451,13 +467,16 @@ void StepperCtrl::UpdateLcd(void) // sprintf(str,"%01d.%02d Amps", NVM->SystemParams.currentMa/1000, NVM->SystemParams.currentMa%1000); // display.setCursor(0,20); // display.println(str); +#ifndef NZS_LCD_ABSOULTE_ANGLE + deg=deg & ANGLE_MAX; //limit to 360 degrees +#endif - deg=deg & ANGLE_MAX; - deg=(deg*360 *100)/ANGLE_STEPS; - x=deg/100; - y=abs(deg-x*100); + deg=(deg*360*10)/(int32_t)ANGLE_STEPS; + x=(deg)/10; + y=abs(deg-(x*10)); - sprintf(str,"%01d.%02d deg", x,y); + LOG("deg is %d, %d, %d",deg, x, y); + sprintf(str,"%03d.%01ddeg", x,y); display.setCursor(0,40); display.println(str); @@ -641,7 +660,7 @@ int StepperCtrl::begin(void) int i; int32_t x; - enableFeedback=false; + enableFeedback=true; forwardWiring=true; //assume we are forward wiring to start with degreesPerFullStep=0; @@ -708,6 +727,7 @@ int StepperCtrl::begin(void) //turn microstepping on changeMicrostep(16); motorReset(); + enableFeedback=true; setupTCInterrupts(); enableTCInterrupts(); @@ -758,6 +778,8 @@ void StepperCtrl::feedback(bool enable) void StepperCtrl::updateStep(int dir, uint16_t steps) { + bool state=TC5_ISR_Enabled; + disableTCInterrupts(); if (dir) { numSteps-=steps; @@ -765,6 +787,7 @@ void StepperCtrl::updateStep(int dir, uint16_t steps) { numSteps+=steps; } + if (state) enableTCInterrupts(); } void StepperCtrl::requestStep(int dir, uint16_t steps) @@ -787,19 +810,13 @@ void StepperCtrl::move(int dir, uint16_t steps) int64_t ret; int32_t n; - n=numMicroSteps; - if (dir) - { - numSteps-=steps; - }else - { - numSteps+=steps; - } + updateStep(dir,steps); if (false == enableFeedback) { + n=numMicroSteps; ret=((int64_t)numSteps * A4954_NUM_MICROSTEPS+(n/2))/n; n=A4954_NUM_MICROSTEPS*fullStepsPerRotation; while(ret>n) @@ -824,9 +841,11 @@ int64_t StepperCtrl::getDesiredLocation(void) { int64_t ret; int32_t n; - + bool state=TC5_ISR_Enabled; + disableTCInterrupts(); n=fullStepsPerRotation * numMicroSteps; - ret=((int64_t)numSteps * ANGLE_STEPS+(n/2))/n; + ret=((int64_t)numSteps * (int64_t)ANGLE_STEPS+(n/2))/n; + if (state) enableTCInterrupts(); return ret; } @@ -893,6 +912,19 @@ int StepperCtrl::measure(void) + +void StepperCtrl::moveToAbsAngle(int32_t a) +{ + + int64_t ret; + int32_t n; + + n=fullStepsPerRotation * numMicroSteps; + + ret=((int64_t)a*n)/(int32_t)ANGLE_STEPS; + numSteps=ret; +} + void StepperCtrl::moveToAngle(int32_t a, uint32_t ma) { //we need to convert 'Angle' to A4954 steps @@ -909,7 +941,8 @@ int32_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); @@ -922,9 +955,19 @@ int32_t StepperCtrl::getCurrentLocation(void) currentLocation += ANGLE_STEPS; } currentLocation=(currentLocation & 0xFFFF0000UL) | (uint16_t)a; + if (state) enableTCInterrupts(); return currentLocation; } + +float StepperCtrl::getCurrentAngle(void) +{ + int32_t x; + float f; + x=getCurrentLocation(); + f=(x*360.0)/(float)ANGLE_STEPS; + return f; +} //Since we are doing fixed point math our // threshold needs to be large. // We need a large threshold when we have fast update @@ -1004,7 +1047,7 @@ bool StepperCtrl::process(void) } - moveToAngle(y,U); + //moveToAngle(y,U); calls++; @@ -1118,6 +1161,30 @@ bool StepperCtrl::process2(void) return 0; } + +void StepperCtrl::enable(bool enable) +{ + bool state=TC5_ISR_Enabled; + disableTCInterrupts(); + uint16_t microSteps=numMicroSteps; + bool feedback=enableFeedback; + + enabled=enable; + stepperDriver.enable(enabled); //enable or disable the stepper driver as needed + + if (enabled==false && enable==true) //if we are enabling previous disabled motor + { + enableFeedback=false; + numMicroSteps=1; + LOG("reset motor"); + motorReset(); + } + + numMicroSteps=microSteps; + enableFeedback=feedback; + if (state) enableTCInterrupts(); +} + void StepperCtrl::testRinging(void) { uint16_t c; diff --git a/firmware/stepper_controller.h b/firmware/stepper_nano_zero/stepper_controller.h similarity index 93% rename from firmware/stepper_controller.h rename to firmware/stepper_nano_zero/stepper_controller.h index d89dce8..28f0abb 100644 --- a/firmware/stepper_controller.h +++ b/firmware/stepper_nano_zero/stepper_controller.h @@ -19,9 +19,10 @@ class StepperCtrl A4954 stepperDriver; Adafruit_SSD1306 display; bool currentLimit; + volatile bool enabled; - int32_t numMicroSteps=16; - int32_t fullStepsPerRotation=200; + volatile int32_t numMicroSteps=16; + volatile int32_t fullStepsPerRotation=200; volatile int64_t numSteps; //this is the number of steps we have taken from our start angle @@ -59,6 +60,8 @@ class StepperCtrl void showSplash(void); void menu(void); void showCalError(void); + int32_t measureMeanEncoder(void); + int32_t getCurrentLocation(void); public: CalibrationTable calTable; @@ -66,6 +69,7 @@ class StepperCtrl bool calibrateEncoder(void); //do manual calibration of the encoder Angle maxCalibrationError(void); //measures the maximum calibration error as an angle + void moveToAbsAngle(int32_t a); void moveToAngle(int32_t a, uint32_t ma); int begin(void); @@ -75,9 +79,9 @@ class StepperCtrl int measure(void); - int32_t measureMeanEncoder(void); + bool changeMicrostep(uint16_t microSteps); void feedback(bool enable); bool getFeedback(void) {return enableFeedback;} @@ -86,10 +90,12 @@ class StepperCtrl int32_t getMicroSteps(void) {return numMicroSteps;} int32_t measureError(void); void testRinging(void); - int32_t getCurrentLocation(void); + + float getCurrentAngle(void); void move(int dir, uint16_t steps); //forces motor to move even if feedback controller is turned off. void UpdateLcd(void); //this can be called from outside class + void enable(bool enable); }; #endif //__STEPPER_CONTROLLER_H__ diff --git a/firmware/stepper_nano_zero.ino b/firmware/stepper_nano_zero/stepper_nano_zero.ino similarity index 70% rename from firmware/stepper_nano_zero.ino rename to firmware/stepper_nano_zero/stepper_nano_zero.ino index dd2e3bc..2744c1d 100644 --- a/firmware/stepper_nano_zero.ino +++ b/firmware/stepper_nano_zero/stepper_nano_zero.ino @@ -15,18 +15,18 @@ static void stepInput(void) dir = digitalRead(PIN_DIR_INPUT); stepperCtrl.requestStep(dir,1); -/* - if (dir==0) - { - stepperCtrl.step(STEPPER_FORWARD); - }else - { - stepperCtrl.step(STEPPER_REVERSE); - } - */ } - +#ifdef USE_ENABLE_PIN +//this function is called when error pin chagnes as enable signal +static void enableInput(void) +{ + static int enable; + //read our enable pin + enable = digitalRead(PIN_ERROR); + stepperCtrl.enable(enable); +} +#endif @@ -56,6 +56,10 @@ void setup() { commandsInit(); //setup command handler system attachInterrupt(digitalPinToInterrupt(PIN_STEP_INPUT), stepInput, RISING); + + #ifdef USE_ENABLE_PIN //if we use enable pin setup interrupt to handle it + attachInterrupt(digitalPinToInterrupt(PIN_ERROR), enableInput, CHANGE); + #endif LOG("SETUP DONE!"); } @@ -65,11 +69,20 @@ void TC5_Handler() { if (TC5->COUNT16.INTFLAG.bit.OVF == 1) { -// static int i=0; -// YELLOW_LED(i); -// i=(i+1) & 0x01; + int error; - YELLOW_LED(stepperCtrl.process2()); //handle the control loop + error=(stepperCtrl.process2()); //handle the control loop + YELLOW_LED(error); +#ifndef USE_ENABLE_PIN + if (error) + { //assume high is inactive and low is active on error pin + digitalWrite(PIN_ERROR,LOW); + }else + { + digitalWrite(PIN_ERROR,HIGH); + } +#endif + TC5->COUNT16.INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag } diff --git a/firmware/syslog.cpp b/firmware/stepper_nano_zero/syslog.cpp similarity index 100% rename from firmware/syslog.cpp rename to firmware/stepper_nano_zero/syslog.cpp diff --git a/firmware/syslog.h b/firmware/stepper_nano_zero/syslog.h similarity index 100% rename from firmware/syslog.h rename to firmware/stepper_nano_zero/syslog.h From c0dcae2c6d30f868de4a8a7e7e6d33332ff2d939 Mon Sep 17 00:00:00 2001 From: trampas Date: Sun, 27 Nov 2016 14:39:49 -0500 Subject: [PATCH 2/2] FW 0.04 fixed merges --- .../.settings/language.settings.xml | 15 --------------- firmware/stepper_nano_zero/board.h | 5 +---- firmware/stepper_nano_zero/nonvolatile.cpp | 6 ++---- 3 files changed, 3 insertions(+), 23 deletions(-) delete mode 100644 firmware/stepper_nano_zero/.settings/language.settings.xml diff --git a/firmware/stepper_nano_zero/.settings/language.settings.xml b/firmware/stepper_nano_zero/.settings/language.settings.xml deleted file mode 100644 index c3aa351..0000000 --- a/firmware/stepper_nano_zero/.settings/language.settings.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/firmware/stepper_nano_zero/board.h b/firmware/stepper_nano_zero/board.h index 0265f1c..9c68718 100644 --- a/firmware/stepper_nano_zero/board.h +++ b/firmware/stepper_nano_zero/board.h @@ -13,14 +13,11 @@ #define NZS_CONTROL_LOOP_HZ (4000) //update rate of control loop, this should be limited to less than 5k -<<<<<<< HEAD:firmware/stepper_nano_zero/board.h #define NZS_LCD_ABSOULTE_ANGLE //define this to show angle from zero in positive and negative direction // for example 2 rotations from start will be angle of 720 degrees #define VERSION "FW: 0.04" //this is what prints on LCD during splash screen -======= -#define VERSION "FW: 0.03" //this is what prints on LCD during splash screen ->>>>>>> 2226a588d97e2a1798ec7a1ea41cc1382f29fba3:firmware/board.h + /* change log * 0.02 added fixes for 0.9 degree motor diff --git a/firmware/stepper_nano_zero/nonvolatile.cpp b/firmware/stepper_nano_zero/nonvolatile.cpp index 95712f3..59236a2 100644 --- a/firmware/stepper_nano_zero/nonvolatile.cpp +++ b/firmware/stepper_nano_zero/nonvolatile.cpp @@ -12,11 +12,9 @@ __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[16640]={ #else __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[256]={ //allocates 512 bytes #endif -<<<<<<< HEAD:firmware/stepper_nano_zero/nonvolatile.cpp - 25125,25451,25805,26119,26469,26783,27123,27428,27769,28076,28410,28710,29046,29349,29680,29980,30316,30624,30962,31267,31611,31925,32268,32579,32929,33248,33596,33911,34263,34580,34925,35234,35578,35890,36227,36531,36869,37178,37516,37821,38163,38476,38818,39128,39476,39792,40137,40450,40802,41120,41469,41784,42135,42453,42801,43114,43464,43778,44123,44433,44779,45089,45428,45734,46073,46381,46720,47029,47374,47690,48038,48353,48708,49029,49379,49695,50044,50361,50700,51004,51341,51643,51971,52268,52602,52906,53241,53546,53891,54203,54547,54859,55209,55527,55877,56191,56544,56863,57212,57528,57883,58206,58561,58882,59239,59562,59911,60220,60563,60873,61208,61509,61846,62152,62490,62797,63140,63454,63797,64108,64451,64763,65098,65398,198,499,826,1122,1453,1758,2092,2398,2739,3053,3398,3710,4060,4379,4729,5046,5401,5725,6081,6405,6766,7096,7456,7777,8135,8458,8807,9119,9465,9779,10119,10424,10768,11077,11413,11719,12061,12369,12705,13008,13346,13650,13980,14277,14608,14907,15233,15528,15857,16155,16484,16781,17114,17421,17756,18061,18404,18718,19057,19367,19714,20028,20372,20683,21031,21347,21694,22010,22368,22692,23050,23376,23742,24074,24438,24763, -======= ->>>>>>> 2226a588d97e2a1798ec7a1ea41cc1382f29fba3:firmware/nonvolatile.cpp + //25125,25451,25805,26119,26469,26783,27123,27428,27769,28076,28410,28710,29046,29349,29680,29980,30316,30624,30962,31267,31611,31925,32268,32579,32929,33248,33596,33911,34263,34580,34925,35234,35578,35890,36227,36531,36869,37178,37516,37821,38163,38476,38818,39128,39476,39792,40137,40450,40802,41120,41469,41784,42135,42453,42801,43114,43464,43778,44123,44433,44779,45089,45428,45734,46073,46381,46720,47029,47374,47690,48038,48353,48708,49029,49379,49695,50044,50361,50700,51004,51341,51643,51971,52268,52602,52906,53241,53546,53891,54203,54547,54859,55209,55527,55877,56191,56544,56863,57212,57528,57883,58206,58561,58882,59239,59562,59911,60220,60563,60873,61208,61509,61846,62152,62490,62797,63140,63454,63797,64108,64451,64763,65098,65398,198,499,826,1122,1453,1758,2092,2398,2739,3053,3398,3710,4060,4379,4729,5046,5401,5725,6081,6405,6766,7096,7456,7777,8135,8458,8807,9119,9465,9779,10119,10424,10768,11077,11413,11719,12061,12369,12705,13008,13346,13650,13980,14277,14608,14907,15233,15528,15857,16155,16484,16781,17114,17421,17756,18061,18404,18718,19057,19367,19714,20028,20372,20683,21031,21347,21694,22010,22368,22692,23050,23376,23742,24074,24438,24763, + 0xFFFF //for the valid flag };