diff --git a/F0:F030,F042,F072/3steppersLB/Makefile b/F0:F030,F042,F072/3steppersLB/Makefile index 57ff91c..0427056 100644 --- a/F0:F030,F042,F072/3steppersLB/Makefile +++ b/F0:F030,F042,F072/3steppersLB/Makefile @@ -5,7 +5,7 @@ BOOTSPEED ?= 57600 FAMILY ?= F0 # MCU code MCU ?= F072xB -DEFS += -DEBUG +#DEFS += -DEBUG # change this linking script depending on particular MCU model, # for example, if you have STM32F103VBT6, you should write: LDSCRIPT ?= stm32f072B.ld diff --git a/F0:F030,F042,F072/3steppersLB/Readme.md b/F0:F030,F042,F072/3steppersLB/Readme.md index 7f457d9..022f240 100644 --- a/F0:F030,F042,F072/3steppersLB/Readme.md +++ b/F0:F030,F042,F072/3steppersLB/Readme.md @@ -142,10 +142,10 @@ Common commands format is cmd[ N[ = val]] where N is command argument (0..127), val is its value Different commands: adc - get ADC values - button - get buttons state + button - get buttons state (return time of last event & ERRCODE == buttonstate) buzzer - change buzzer state (1/0) - esw - get end switches state - ext - external outputs + esw - get end switches state (without number - all, by bytes) + ext - external outputs (without number - all, by bytes; value= 0-off, 1-on, other-toggle) mcut - get MCU T mcuvdd - get MCU Vdd ping - echo given command back @@ -175,6 +175,7 @@ Motors' commands: motreinit - re-init motors after configuration changed relpos - set relative steps, get remaining relslow - set relative steps @ lowest speed + setpos - set/get absolute position (in steps) state - get motor state stop - smooth motor stopping USB-only commands: @@ -185,6 +186,7 @@ USB-only commands: dumperr - dump error codes dumpcmd - dump command codes dumpconf - dump current configuration + eraseflash - erase flash data storage filter - add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]] getctr - get TIM1/2/3 counters ignbuf - print ignore buffer @@ -208,7 +210,6 @@ bytes descr 7 Hdata dumperr -Find known command: dumperr Error codes: 0 - all OK 1 - wrong parameter's value @@ -219,82 +220,82 @@ Error codes: dumpcmd -Find known command: dumpcmd Commands list: -0 - Different commands: -1 - change relay state (1/0) -2 - change buzzer state (1/0) -3 - get ADC values -4 - get buttons state -5 - get end switches state -6 - get MCU T -7 - get MCU Vdd -8 - reset MCU -9 - get time from start -10 - pwm value -11 - external outputs -12 - save current configuration -13 - minimal encoder ticks per step -14 - maximal encoder ticks per step -15 - set/get microsteps settings -16 - set/get accel/decel (steps/s^2) -17 - set/get max speed (steps per sec) -18 - set/get min speed (steps per sec) -19 - get limiting speed for current microsteps -20 - set/get max steps (from zero) -21 - set/get max encoder's pulses per revolution -22 - set/get motorN flags -23 - end-switches reaction -24 - re-init motors after configuration changed -25 - set/get position (in steps) -26 - set relative steps, get remaining -27 - set relative steps @ lowest speed -28 - emergency stop motor (right now) -29 - smooth motor stopping -30 - emergency stop all motors -31 - find zero position & refresh counters -32 - get motor state -33 - set/get encoder's position +1 - echo given command back +2 - change relay state (1/0) +3 - change buzzer state (1/0) +4 - get ADC values +5 - get buttons state +6 - get end switches state +7 - get MCU T +8 - get MCU Vdd +9 - reset MCU +10 - get time from start +11 - pwm value +12 - external outputs +13 - save current configuration +14 - minimal encoder ticks per step +15 - maximal encoder ticks per step +16 - set/get microsteps settings +17 - set/get accel/decel (steps/s^2) +18 - set/get max speed (steps per sec) +19 - set/get min speed (steps per sec) +20 - get limiting speed for current microsteps +21 - set/get max steps (from zero) +22 - set/get max encoder's pulses per revolution +23 - set/get motorN flags +24 - end-switches reaction +25 - re-init motors after configuration changed +26 - move to/get absolute position (in steps) +27 - set relative steps, get remaining +28 - set relative steps @ lowest speed +29 - emergency stop motor (right now) +30 - smooth motor stopping +31 - emergency stop all motors +32 - find zero position & refresh counters +33 - get motor state +34 - set/get encoder's position +35 - set/get absolute position (in steps) + dumpconf -Find known command: dumpconf -flashsize=64*2048=131072 -userconf_addr=0x08006800 -userconf_idx=-1 // "index of stored conf" +userconf_addr=0x08006000// address from which userconf started +userconf_idx=5 // "index of stored conf" userconf_sz=68 // "magick number" canspeed=100 // default CAN speed canid=170 // identifier (0xaa) microsteps0=32 // microsteps amount per step -accel0=500 // acceleration/deceleration (steps/s^2) -maxspeed0=2000 // max motor speed (steps per second) +accel0=1500 // acceleration/deceleration (steps/s^2) +maxspeed0=1501 // max motor speed (steps per second) minspeed0=20 // min motor speed (steps per second) maxsteps0=500000 // maximal amount of steps encperrev0=4000 // encoders' counts per revolution encperstepmin0=17 // min amount of encoder ticks per one step encperstepmax0=23 // max amount of encoder ticks per one step -motflags0=0x3c // motor's flags +motflags0=0x2f // motor's flags eswreaction0=0 // end-switches reaction (esw_react) microsteps1=32 -accel1=500 +accel1=1500 maxspeed1=2000 minspeed1=20 maxsteps1=500000 encperrev1=4000 encperstepmin1=17 encperstepmax1=23 -motflags1=0x3c +motflags1=0x2f eswreaction1=0 microsteps2=32 -accel2=500 -maxspeed2=2000 +accel2=1500 +maxspeed2=2500 minspeed2=20 maxsteps2=500000 encperrev2=4000 encperstepmin2=17 encperstepmax2=23 -motflags2=0x3c +motflags2=0x2f eswreaction2=0 + Motor flags: bit0 - reversing motor rotation bit1 - reversing encoder rotation diff --git a/F0:F030,F042,F072/3steppersLB/commonproto.c b/F0:F030,F042,F072/3steppersLB/commonproto.c index da8b3ac..76cbb05 100644 --- a/F0:F030,F042,F072/3steppersLB/commonproto.c +++ b/F0:F030,F042,F072/3steppersLB/commonproto.c @@ -27,6 +27,11 @@ #include "strfunct.h" #endif +#define NOPARCHK() do{uint8_t n = PARBASE(par); if(n != CANMESG_NOPAR) return ERR_BADPAR;}while(0) + +#define CHECKN(val, par) do{val = PARBASE(par); \ + if(val > MOTORSNO-1) return ERR_BADPAR;}while(0) + /******* All functions from cmdlist[i].function *******/ static errcodes pingparser(uint8_t _U_ par, int32_t _U_ *val){ @@ -34,6 +39,7 @@ static errcodes pingparser(uint8_t _U_ par, int32_t _U_ *val){ } static errcodes relayparser(uint8_t par, int32_t *val){ + NOPARCHK(); if(ISSETTER(par)){ if(*val) ON(RELAY); else OFF(RELAY); @@ -43,6 +49,7 @@ static errcodes relayparser(uint8_t par, int32_t *val){ } static errcodes buzzerparser(uint8_t par, int32_t *val){ + NOPARCHK(); if(ISSETTER(par)){ if(*val) ON(BUZZER); else OFF(BUZZER); @@ -63,7 +70,7 @@ static errcodes adcparser(uint8_t par, int32_t *val){ static errcodes buttonsparser(uint8_t par, int32_t *val){ uint8_t n = PARBASE(par); if(n > BTNSNO-1){ - par = CANMESG_NOPAR; // the only chance to understand error + *val = CANMESG_NOPAR; // the only chance to understand error return ERR_BADPAR; } return (uint8_t) keystate(n, (uint32_t*)val); @@ -75,39 +82,43 @@ static errcodes eswparser(uint8_t par, int32_t *val){ #error "change the code!!!" #endif uint8_t n = PARBASE(par); - if(n > ESWNO-1){ // all + if(n == CANMESG_NOPAR){ // all *val = 0; uint8_t *arr = (uint8_t*)val; for(int i = 0; i < ESWNO; ++i) *arr++ = ESW_state(i); return ERR_OK; - } + }else if(n > ESWNO - 1) return ERR_BADPAR; *val = (int32_t)ESW_state(n); return ERR_OK; } static errcodes mcutparser(uint8_t _U_ par, int32_t *val){ + NOPARCHK(); *val = getMCUtemp(); return ERR_OK; } static errcodes mcuvddparser(uint8_t _U_ par, int32_t *val){ + NOPARCHK(); *val = getVdd(); return ERR_OK; } static errcodes resetparser(uint8_t _U_ par, int32_t _U_ *val){ + NOPARCHK(); NVIC_SystemReset(); return ERR_OK; } static errcodes timeparser(uint8_t _U_ par, int32_t *val){ + NOPARCHK(); *val = Tms; return ERR_OK; } static errcodes pwmparser(uint8_t par, int32_t *val){ - if(PARBASE(par) > PWMCHMAX && par != CANMESG_NOPAR) return ERR_BADPAR; + NOPARCHK(); #if PWMCHMAX != 0 #error "change the code!!!" #endif @@ -144,7 +155,7 @@ static errcodes extparser(uint8_t par, int32_t *val){ SEND("par="); printu(par); SEND(", n="); bufputchar('0'+n); newline(); #endif - if(n > EXTNO-1){ // all + if(n == CANMESG_NOPAR){ // all #ifdef EBUG SEND("ALL\n"); #endif @@ -157,7 +168,7 @@ static errcodes extparser(uint8_t par, int32_t *val){ arr[i] = EXT_CHK(i); } return ERR_OK; - } + }else if(n > EXTNO-1) return ERR_BADPAR; if(ISSETTER(par)) setextpar((uint8_t)*val, n); *val = (int32_t) EXT_CHK(n); @@ -166,8 +177,7 @@ static errcodes extparser(uint8_t par, int32_t *val){ /******************* START of config parsers *******************/ static errcodes ustepsparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ #if MICROSTEPSMAX > 512 #error "Change the code anywhere!" @@ -184,8 +194,7 @@ static errcodes ustepsparser(uint8_t par, int32_t *val){ } static errcodes encstepsminparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val < 1 || *val > MAXENCTICKSPERSTEP - 1) return ERR_BADVAL; the_conf.encperstepmin[n] = *val; @@ -195,8 +204,7 @@ static errcodes encstepsminparser(uint8_t par, int32_t *val){ } static errcodes encstepsmaxparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val < 1 || *val > MAXENCTICKSPERSTEP) return ERR_BADVAL; the_conf.encperstepmax[n] = *val; @@ -206,8 +214,7 @@ static errcodes encstepsmaxparser(uint8_t par, int32_t *val){ } static errcodes accparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL; the_conf.accel[n] = *val; @@ -227,8 +234,7 @@ static uint16_t getSPD(uint8_t n, int32_t speed){ } static errcodes maxspdparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val <= the_conf.minspd[n]) return ERR_BADVAL; the_conf.maxspd[n] = getSPD(n, *val); @@ -238,8 +244,7 @@ static errcodes maxspdparser(uint8_t par, int32_t *val){ } static errcodes minspdparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val >= the_conf.maxspd[n]) return ERR_BADVAL; the_conf.minspd[n] = getSPD(n, *val); @@ -249,15 +254,13 @@ static errcodes minspdparser(uint8_t par, int32_t *val){ } static errcodes spdlimparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); *val = getSPD(n, 0xffff); return ERR_OK; } static errcodes maxstepsparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val < 1) return ERR_BADVAL; the_conf.maxsteps[n] = *val; @@ -267,8 +270,7 @@ static errcodes maxstepsparser(uint8_t par, int32_t *val){ } static errcodes encrevparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val < 1 || *val > MAXENCREV) return ERR_BADVAL; the_conf.encrev[n] = *val; @@ -279,8 +281,7 @@ static errcodes encrevparser(uint8_t par, int32_t *val){ } static errcodes motflagsparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ the_conf.motflags[n] = *((motflags_t*)val); } @@ -290,8 +291,7 @@ static errcodes motflagsparser(uint8_t par, int32_t *val){ // setter of GLOBAL reaction, getter of LOCAL! static errcodes eswreactparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); if(ISSETTER(par)){ if(*val < 0 || *val > ESW_AMOUNT-1) return ERR_BADVAL; the_conf.ESW_reaction[n] = *val; @@ -302,6 +302,7 @@ static errcodes eswreactparser(uint8_t par, int32_t *val){ } static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){ + NOPARCHK(); if(store_userconf()) return ERR_CANTRUN; return ERR_OK; } @@ -310,13 +311,11 @@ static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){ /******************* START of motors' parsers *******************/ static errcodes reinitmparser(uint8_t _U_ par, int32_t _U_ *val){ + NOPARCHK(); init_steppers(); return ERR_OK; } -#define CHECKN(val, par) do{val = PARBASE(par); \ - if(val > MOTORSNO-1) return ERR_BADPAR;}while(0) - static errcodes emstopparser(uint8_t par, int32_t _U_ *val){ uint8_t n; CHECKN(n, par); emstopmotor(n); @@ -324,6 +323,7 @@ static errcodes emstopparser(uint8_t par, int32_t _U_ *val){ } static errcodes emstopallparser(uint8_t _U_ par, int32_t _U_ *val){ + NOPARCHK(); for(int i = 0; i < MOTORSNO; ++i) emstopmotor(i); return ERR_OK; @@ -384,7 +384,6 @@ static errcodes gotozeroparser(uint8_t par, _U_ int32_t *val){ return motor_goto0(n); } -#undef CHECKN /******************* END of motors' parsers *******************/ /* diff --git a/F0:F030,F042,F072/3steppersLB/flash.c b/F0:F030,F042,F072/3steppersLB/flash.c index 84de13b..9f958cf 100644 --- a/F0:F030,F042,F072/3steppersLB/flash.c +++ b/F0:F030,F042,F072/3steppersLB/flash.c @@ -112,7 +112,7 @@ int store_userconf(){ // for binarySearch() checking that there's nothing more after it! if(currentconfidx > (int)maxCnum - 3){ // there's no more place currentconfidx = 0; - if(erase_flash(Flash_Data, (&__varsstart))) return 1; + if(erase_flash(Flash_Data, NULL)) return 1; }else ++currentconfidx; // take next data position (0 - within first run after firmware flashing) return write2flash((const void*)&Flash_Data[currentconfidx], &the_conf, sizeof(the_conf)); } @@ -241,3 +241,7 @@ void dump_userconf(_U_ char *txt){ } NL(); } + +int erase_storage(){ + return erase_flash(Flash_Data, NULL); +} diff --git a/F0:F030,F042,F072/3steppersLB/flash.h b/F0:F030,F042,F072/3steppersLB/flash.h index 5794a20..07dfd1f 100644 --- a/F0:F030,F042,F072/3steppersLB/flash.h +++ b/F0:F030,F042,F072/3steppersLB/flash.h @@ -79,5 +79,6 @@ extern user_conf the_conf; // global user config (read from FLASH to RAM) void flashstorage_init(); int store_userconf(); void dump_userconf(_U_ char *txt); +int erase_storage(); #endif // __FLASH_H__ diff --git a/F0:F030,F042,F072/3steppersLB/steppers.bin b/F0:F030,F042,F072/3steppersLB/steppers.bin index e50adaf..9eb447e 100755 Binary files a/F0:F030,F042,F072/3steppersLB/steppers.bin and b/F0:F030,F042,F072/3steppersLB/steppers.bin differ diff --git a/F0:F030,F042,F072/3steppersLB/steppers.c b/F0:F030,F042,F072/3steppersLB/steppers.c index 98348cb..6c7ab24 100644 --- a/F0:F030,F042,F072/3steppersLB/steppers.c +++ b/F0:F030,F042,F072/3steppersLB/steppers.c @@ -34,6 +34,10 @@ typedef enum{ STALL_STOP // Nstalled >= limit } t_stalled; +#ifdef EBUG +static uint8_t stp[MOTORSNO] = {0}; +#endif + static t_stalled stallflags[MOTORSNO]; // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) @@ -162,6 +166,15 @@ errcodes getremainsteps(uint8_t i, int32_t *position){ // calculate acceleration/deceleration parameters for motor i static void calcacceleration(uint8_t i){ + switch(state[i]){ // do nothing in case of error/stopping + case STP_ERR: + case STP_RELAX: + case STP_STALL: + return; + break; + default: + break; + } int32_t delta = targstppos[i] - stppos[i]; if(delta > 0){ // positive direction if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid @@ -181,7 +194,10 @@ static void calcacceleration(uint8_t i){ if(the_conf.motflags[i].reverse) MOTOR_CW(i); else MOTOR_CCW(i); } - if(state[i] != STP_MVSLOW) state[i] = STP_ACCEL; + if(state[i] != STP_MVSLOW){ + DBG("->accel"); + state[i] = STP_ACCEL; + } startspeed[i] = curspeed[i]; Taccel[i] = Tms; recalcARR(i); @@ -215,20 +231,21 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){ case STP_RELAX: break; case STP_STALL: + DBG("Move from STALL"); if(dir == stalleddir[i]){ DBG("Move to stalled direction!"); return ERR_CANTRUN; // can't run into stalled direction } break; default: // moving state - DBG("Always moving"); + DBG("Is moving"); return ERR_CANTRUN; } if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]){ DBG("Too much steps"); return ERR_BADVAL; // too big position or zero } - motdir[i] = dir; + motdir[i] = dir; // should be before limit switch check if(esw_block(i)){ DBG("Block by ESW"); return ERR_CANTRUN; // on end-switch @@ -239,6 +256,7 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){ prevencpos[i] = encoder_position(i); prevstppos[i] = stppos[i]; curspeed[i] = the_conf.minspd[i]; + state[i] = STP_ACCEL; calcacceleration(i); #ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); @@ -259,6 +277,7 @@ errcodes motor_relmove(uint8_t i, int32_t relsteps){ errcodes motor_relslow(uint8_t i, int32_t relsteps){ errcodes e = motor_absmove(i, stppos[i] + relsteps); if(ERR_OK == e){ + DBG("-> MVSLOW"); state[i] = STP_MVSLOW; } return e; @@ -269,6 +288,7 @@ void emstopmotor(uint8_t i){ switch(state[i]){ case STP_ERR: // clear error state case STP_STALL: + DBG("-> RELAX"); state[i] = STP_RELAX; // fallthrough case STP_RELAX: // do nothing in stopping state @@ -301,18 +321,19 @@ void addmicrostep(uint8_t i){ } } if(stopflag[i] || stop_at_pos){ // stop NOW + mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer if(stopflag[i]) targstppos[i] = stppos[i]; // keep position (for keep flag) stopflag[i] = 0; - mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer if(the_conf.motflags[i].donthold) MOTOR_DIS(i); // turn off power if(stallflags[i] == STALL_STOP){ stallflags[i] = STALL_NO; state[i] = STP_STALL; - }else + }else{ state[i] = STP_RELAX; + } #ifdef EBUG - SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline(); + stp[i] = 1; #endif } } @@ -361,15 +382,14 @@ static t_stalled chkSTALL(uint8_t i){ SEND("MOTOR"); bufputchar('0'+i); SEND(" Denc="); printi(Denc); SEND(", Dstp="); printu(Dstp); SEND(", speed="); printu(curspeed[i]); #endif - if(++Nstalled[i] > NSTALLEDMAX){ - stopflag[i] = 1; + if(++Nstalled[i] >= NSTALLEDMAX){ Nstalled[i] = 0; #ifdef EBUG SEND(" --- STALL!"); #else SEND("ERRCODE="); bufputchar(ERR_CANTRUN+'0'); SEND("\nstate"); bufputchar(i+'0'); bufputchar('='); - bufputchar(STP_STALL); + bufputchar(STP_STALL + '0'); #endif NL(); stallflags[i] = STALL_STOP; @@ -408,7 +428,19 @@ static t_stalled chkSTALL(uint8_t i){ // check state of i`th stepper static void chkstepper(int i){ int32_t i32; + t_stalled s = chkSTALL(i); + static int32_t oldtargpos[3] = {0}; // target position of previous `keep` call + static uint8_t keepposctr[3] = {0}; // counter of tries to keep current position static uint8_t stopctr[3] = {0}; // counters for encoders/position zeroing after stopping @ esw +#ifdef EBUG + if(stp[i]){ + stp[i] = 0; + // motor state could be changed outside of interrupt, so return it to relax or leave in STALL + if(state[i] != STP_STALL) state[i] = STP_RELAX; + SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); + SEND(", curstate="); printu(state[i]); newline(); + } +#endif switch(state[i]){ case STP_RELAX: // check if need to keep current position if(the_conf.motflags[i].haveencoder){ @@ -425,18 +457,30 @@ static void chkstepper(int i){ if(the_conf.motflags[i].keeppos){ // keep old position diff = targstppos[i] - i32; // check whether we need to change position if(diff){ // try to correct position + if(oldtargpos[i] == targstppos[i]){ + if(++keepposctr[i] >= KEEPPOSMAX){ + DBG("Can't keep position"); + targstppos[i] = i32; + return; // can't keep position for + } + }else{ + oldtargpos[i] = targstppos[i]; + keepposctr[i] = 0; + } #ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" curpos="); printi(i32); SEND(", need="); printi(targstppos[i]); NL(); #endif - if(ERR_OK != motor_absmove(i, targstppos[i])) + if(ERR_OK != motor_absmove(i, targstppos[i])){ + DBG("Can't move to target position for keeping"); targstppos[i] = i32; // can't move to target position - forget it + } } } } break; case STP_ACCEL: // acceleration to max speed - if(STALL_NO == chkSTALL(i)){ + if(s == STALL_NO){ //newspeed = curspeed[i] + dV[i]; i32 = the_conf.minspd[i] + (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; if(i32 >= the_conf.maxspd[i]){ // max speed reached -> move with it @@ -450,20 +494,22 @@ static void chkstepper(int i){ curspeed[i] = i32; } recalcARR(i); - } - // check position for triangle profile - if(motdir[i] > 0){ - if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration - TODECEL(); - } - }else{ - if(stppos[i] <= decelstartpos[i]){ - TODECEL(); + // check position for triangle profile + if(motdir[i] > 0){ + if(stppos[i] >= decelstartpos[i]){ // reached end of acceleration + TODECEL(); + } + }else{ + if(stppos[i] <= decelstartpos[i]){ + TODECEL(); + } } + }else if(s == STALL_STOP){ + stopflag[i] = 1; } break; case STP_MOVE: // move @ constant speed until need to decelerate - if(STALL_NO == chkSTALL(i)){ + if(s == STALL_NO){ // check position if(motdir[i] > 0){ if(stppos[i] >= decelstartpos[i]){ // reached start of deceleration @@ -474,10 +520,12 @@ static void chkstepper(int i){ TODECEL(); } } + }else if(s == STALL_STOP){ + stopflag[i] = 1; } break; case STP_DECEL: - if(STALL_NO == chkSTALL(i)){ + if(s == STALL_NO){ //newspeed = curspeed[i] - dV[i]; i32 = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; if(i32 > the_conf.minspd[i]){ @@ -492,10 +540,12 @@ static void chkstepper(int i){ } recalcARR(i); //SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline(); + }else if(s == STALL_STOP){ + stopflag[i] = 1; } break; case STP_MVSLOW: - chkSTALL(i); + if(s == STALL_STOP) stopflag[i] = 1; break; default: // STALL, ERR -> do nothing, check mvzerostate break; @@ -504,12 +554,13 @@ static void chkstepper(int i){ case M0FAST: if(state[i] == STP_RELAX || state[i] == STP_STALL){ // stopped -> move to + #ifdef EBUG - SEND("M0FAST: motor stopped\n"); + bufputchar('M'); bufputchar('0'+i); SEND("FAST: motor stopped\n"); #endif if(ERR_OK != motor_relslow(i, 1000)){ #ifdef EBUG SEND("Can't move\n"); #endif + DBG("->ERR"); state[i] = STP_ERR; mvzerostate[i] = M0RELAX; ESW_reaction[i] = the_conf.ESW_reaction[i]; @@ -525,11 +576,11 @@ static void chkstepper(int i){ } if((state[i] == STP_RELAX || state[i] == STP_STALL) && ++stopctr[i] > 5){ // wait at least 50ms #ifdef EBUG - SEND("M0SLOW: set position to 0\n"); NL(); + bufputchar('M'); bufputchar('0'+i); SEND("SLOW: motor stopped\n"); #endif ESW_reaction[i] = the_conf.ESW_reaction[i]; prevencpos[i] = encpos[i] = 0; - targstppos[i] = stppos[i] = 0; + prevstppos[i] = targstppos[i] = stppos[i] = 0; enctimers[i]->CNT = 0; // set encoder counter to zero mvzerostate[i] = M0RELAX; } @@ -541,8 +592,9 @@ static void chkstepper(int i){ errcodes motor_goto0(uint8_t i){ errcodes e = motor_absmove(i, -the_conf.maxsteps[i]); - if(ERR_OK != e) return e; - ESW_reaction[i] = ESW_STOPMINUS; + if(ERR_OK != e){ + if(!ESW_state(i)) return e; // not @ limit switch -> error + }else ESW_reaction[i] = ESW_STOPMINUS; mvzerostate[i] = M0FAST; return e; } diff --git a/F0:F030,F042,F072/3steppersLB/steppers.h b/F0:F030,F042,F072/3steppersLB/steppers.h index c6391ab..00c5e22 100644 --- a/F0:F030,F042,F072/3steppersLB/steppers.h +++ b/F0:F030,F042,F072/3steppersLB/steppers.h @@ -27,16 +27,18 @@ #define NSTALLEDMAX (5) // amount of steps to detect stalled state #define STALLEDSTEPS (15) +// amount of tries to keep current position (need for states near problem places) +#define KEEPPOSMAX (10) // stepper states typedef enum{ - STP_RELAX, // no moving - STP_ACCEL, // start moving with acceleration - STP_MOVE, // moving with constant speed - STP_MVSLOW, // moving with slowest constant speed (end of moving) - STP_DECEL, // moving with deceleration - STP_STALL, // stalled - STP_ERR // wrong/error state + STP_RELAX, // 0 - no moving + STP_ACCEL, // 1 - start moving with acceleration + STP_MOVE, // 2 - moving with constant speed + STP_MVSLOW, // 3 - moving with slowest constant speed (end of moving) + STP_DECEL, // 4 - moving with deceleration + STP_STALL, // 5 - stalled + STP_ERR // 6 - wrong/error state } stp_state; // end-switches reaction diff --git a/F0:F030,F042,F072/3steppersLB/strfunct.c b/F0:F030,F042,F072/3steppersLB/strfunct.c index 11b7d4d..fee64b4 100644 --- a/F0:F030,F042,F072/3steppersLB/strfunct.c +++ b/F0:F030,F042,F072/3steppersLB/strfunct.c @@ -423,6 +423,14 @@ void dumperrcodes(_U_ char *txt){ } } +static void eraseflash(_U_ char *txt){ + SEND("Start at "); printu(Tms); + int e = erase_storage(); + SEND("\nStop at "); printu(Tms); newline(); + if(e) SEND("Error\n"); + else SEND("OK\n"); +} + typedef void(*specfpointer)(char *arg); enum{ @@ -443,7 +451,7 @@ enum{ SCMD_WD, SCMD_DUMPERR, SCMD_DUMPCMD, - //SCMD_ST, + SCMD_ERASEFLASH, SCMD_AMOUNT }; @@ -466,7 +474,7 @@ static specfpointer speccmdlist[SCMD_AMOUNT] = { [SCMD_WD] = wdcheck, [SCMD_DUMPCMD] = dumpcmdcodes, [SCMD_DUMPERR] = dumperrcodes, - //[SCMD_ST] = stp_check, + [SCMD_ERASEFLASH] = eraseflash, }; typedef struct{ @@ -526,6 +534,7 @@ static const commands textcommands[] = { {-SCMD_DUMPERR, "dumperr", "dump error codes"}, {-SCMD_DUMPCMD, "dumpcmd", "dump command codes"}, {-SCMD_DUMPCONF, "dumpconf", "dump current configuration"}, + {-SCMD_ERASEFLASH, "eraseflash", "erase flash data storage"}, {-SCMD_FILTER, "filter", "add/modify filter, format: bank# FIFO# mode(M/I) num0 [num1 [num2 [num3]]]"}, {-SCMD_GETCTR, "getctr", "get TIM1/2/3 counters"}, {-SCMD_IGNBUF, "ignbuf", "print ignore buffer"}, @@ -542,7 +551,7 @@ static const commands textcommands[] = { // dump base commands codes (for CAN protocol) void dumpcmdcodes(_U_ char *txt){ SEND("CANbus commands list:\n"); - for(uint16_t i = 0; i < CMD_AMOUNT; ++i){ + for(uint16_t i = 1; i < CMD_AMOUNT; ++i){ printu(i); SEND(" - "); const commands *c = textcommands; @@ -656,7 +665,9 @@ void cmd_parser(char *txt){ par &= 0x7f; if(par != CANMESG_NOPAR) printu(par); bufputchar('='); printi(val); +#ifdef EBUG SEND(" ("); printuhex((uint32_t)val); bufputchar(')'); +#endif if(ERR_OK != retcode){ SEND("\nERRCODE="); printu(retcode); diff --git a/F0:F030,F042,F072/3steppersLB/version.inc b/F0:F030,F042,F072/3steppersLB/version.inc index 5e39d96..a2f289a 100644 --- a/F0:F030,F042,F072/3steppersLB/version.inc +++ b/F0:F030,F042,F072/3steppersLB/version.inc @@ -1,2 +1,2 @@ -#define BUILD_NUMBER "156" -#define BUILD_DATE "2022-06-10" +#define BUILD_NUMBER "168" +#define BUILD_DATE "2022-06-17"