diff --git a/F0:F030,F042,F072/3steppersLB/Makefile b/F0:F030,F042,F072/3steppersLB/Makefile index ed0fd91..57ff91c 100644 --- a/F0:F030,F042,F072/3steppersLB/Makefile +++ b/F0:F030,F042,F072/3steppersLB/Makefile @@ -65,12 +65,12 @@ CFLAGS += -fno-common -ffunction-sections -fdata-sections ############################################################################### # Linker flags -LDFLAGS += --static -nostartfiles +LDFLAGS += --static -nostartfiles -nostdlib +LDFLAGS += -Wl,--gc-sections -Wl,--print-memory-usage #--specs=nano.specs LDFLAGS += -L$(LIB_DIR) LDFLAGS += -T$(LDSCRIPT) LDFLAGS += -Wl,-Map=$(OBJDIR)/$(BINARY).map -LDFLAGS += -Wl,--gc-sections ############################################################################### # Used libraries @@ -113,7 +113,7 @@ $(VERSION_FILE): *.[ch] $(OBJDIR)/strfunct.o: strfunct.c $(VERSION_FILE) $(OBJDIR)/%.o: %.c - @make $(VERSION_FILE) +# @make $(VERSION_FILE) @echo " CC $<" $(CC) $(CFLAGS) $(DEFS) $(INCLUDE) $(ARCH_FLAGS) -o $@ -c $< diff --git a/F0:F030,F042,F072/3steppersLB/Readme.md b/F0:F030,F042,F072/3steppersLB/Readme.md index 9383309..7f457d9 100644 --- a/F0:F030,F042,F072/3steppersLB/Readme.md +++ b/F0:F030,F042,F072/3steppersLB/Readme.md @@ -255,3 +255,66 @@ Commands list: 31 - find zero position & refresh counters 32 - get motor state 33 - set/get encoder's position + +dumpconf +Find known command: dumpconf +flashsize=64*2048=131072 +userconf_addr=0x08006800 +userconf_idx=-1 // "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) +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 +eswreaction0=0 // end-switches reaction (esw_react) +microsteps1=32 +accel1=500 +maxspeed1=2000 +minspeed1=20 +maxsteps1=500000 +encperrev1=4000 +encperstepmin1=17 +encperstepmax1=23 +motflags1=0x3c +eswreaction1=0 +microsteps2=32 +accel2=500 +maxspeed2=2000 +minspeed2=20 +maxsteps2=500000 +encperrev2=4000 +encperstepmin2=17 +encperstepmax2=23 +motflags2=0x3c +eswreaction2=0 + +Motor flags: +bit0 - reversing motor rotation +bit1 - reversing encoder rotation +bit2 - have encoder +bit3 - clear power @ stop (don't hold motor when stopped) +bit4 - inverse end-switches (Work @ high level when this flag activated) +bit5 - keep current position (as servo motor) + +Stepper states: +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 + +ESW reaction: + +ESW_IGNORE, // 0 - don't stop @ end-switch +ESW_ANYSTOP, // 1 - stop @ esw in any moving direction +ESW_STOPMINUS, // 2 - stop only in negative moving + diff --git a/F0:F030,F042,F072/3steppersLB/commonproto.c b/F0:F030,F042,F072/3steppersLB/commonproto.c index 4793f2a..da8b3ac 100644 --- a/F0:F030,F042,F072/3steppersLB/commonproto.c +++ b/F0:F030,F042,F072/3steppersLB/commonproto.c @@ -140,10 +140,14 @@ static errcodes extparser(uint8_t par, int32_t *val){ #error "change the code!!!" #endif uint8_t n = PARBASE(par); +#ifdef EBUG SEND("par="); printu(par); SEND(", n="); bufputchar('0'+n); newline(); +#endif if(n > EXTNO-1){ // all +#ifdef EBUG SEND("ALL\n"); +#endif uint8_t *arr = (uint8_t*)val; if(ISSETTER(par)){ for(int i = 0; i < EXTNO; ++i) @@ -310,9 +314,11 @@ static errcodes reinitmparser(uint8_t _U_ par, int32_t _U_ *val){ 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 = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); emstopmotor(n); return ERR_OK; } @@ -324,43 +330,37 @@ static errcodes emstopallparser(uint8_t _U_ par, int32_t _U_ *val){ } static errcodes stopparser(uint8_t par, int32_t _U_ *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); stopmotor(n); return ERR_OK; } static errcodes curposparser(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)) return motor_absmove(n, *val); return getpos(n, val); } static errcodes relstepsparser(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)) return motor_relmove(n, *val); return getremainsteps(n, val); } static errcodes relslowparser(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)) return motor_relslow(n, *val); return getremainsteps(n, val); } static errcodes motstateparser(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 = getmotstate(n); return ERR_OK; } static errcodes encposparser(uint8_t par, int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); errcodes ret = ERR_OK; if(ISSETTER(par)){ if(!setencpos(n, *val)) ret = ERR_CANTRUN; @@ -369,11 +369,22 @@ static errcodes encposparser(uint8_t par, int32_t *val){ return ret; } +static errcodes setposparser(uint8_t par, int32_t *val){ + uint8_t n; CHECKN(n, par); + errcodes ret = ERR_OK; + if(ISSETTER(par)){ + ret = setmotpos(n, *val); + } + getpos(n, val); + return ret; +} + static errcodes gotozeroparser(uint8_t par, _U_ int32_t *val){ - uint8_t n = PARBASE(par); - if(n > MOTORSNO-1) return ERR_BADPAR; + uint8_t n; CHECKN(n, par); return motor_goto0(n); } + +#undef CHECKN /******************* END of motors' parsers *******************/ /* @@ -419,6 +430,7 @@ const fpointer cmdlist[CMD_AMOUNT] = { [CMD_REINITMOTORS] = reinitmparser, [CMD_MOTORSTATE] = motstateparser, [CMD_ENCPOS] = encposparser, + [CMD_SETPOS] = setposparser, [CMD_GOTOZERO] = gotozeroparser, }; diff --git a/F0:F030,F042,F072/3steppersLB/commonproto.h b/F0:F030,F042,F072/3steppersLB/commonproto.h index 07e1c26..578cd4b 100644 --- a/F0:F030,F042,F072/3steppersLB/commonproto.h +++ b/F0:F030,F042,F072/3steppersLB/commonproto.h @@ -52,7 +52,8 @@ typedef enum{ typedef errcodes (*fpointer)(uint8_t par, int32_t *val); enum{ - CMD_PING // ping device + CMD_NONE // omit zero + ,CMD_PING // ping device ,CMD_RELAY // relay on/off ,CMD_BUZZER // buzzer on/off ,CMD_ADC // ADC ch# @@ -86,6 +87,7 @@ enum{ ,CMD_GOTOZERO // go to zero's ESW ,CMD_MOTORSTATE // motor state ,CMD_ENCPOS // position of encoder (independing on settings) + ,CMD_SETPOS // set motor position //,CMD_STOPDECEL //,CMD_FINDZERO // should be the last: diff --git a/F0:F030,F042,F072/3steppersLB/custom_buttons.c b/F0:F030,F042,F072/3steppersLB/custom_buttons.c index 75766a0..10059ac 100644 --- a/F0:F030,F042,F072/3steppersLB/custom_buttons.c +++ b/F0:F030,F042,F072/3steppersLB/custom_buttons.c @@ -22,6 +22,43 @@ #include "buttons.h" #include "custom_buttons.h" #include "hardware.h" +#include "steppers.h" +#include "strfunct.h" + +/** + * @brief custom_buttons_process - check four buttons and if find hold: + * 0..2 - move motor i by +maxsteps while hold + * 3 (or pressed) - switch moving direction to - + * if pressed: + * 0..2 - stop motor i (if moving) or move by +10 steps + */ +void custom_buttons_process(){ + static uint32_t lastT = 0; + static keyevent lastevent[3] = {EVT_NONE, EVT_NONE, EVT_NONE}; + if(lastUnsleep == lastT) return; // no buttons activity + lastT = lastUnsleep; + int32_t dir = 1; + if(keyevt(3) == EVT_HOLD || keyevt(3) == EVT_PRESS) dir = -1; // button 3: change direction to `-` + for(int i = 0; i < 3; ++i){ + keyevent e = keyevt(i); + if(e == EVT_RELEASE){ // move by 10 steps or emergency stop @ release after shot press + if(lastevent[i] == EVT_PRESS){ + if(getmotstate(i) == STP_RELAX) motor_relslow(i, dir*10); + else emstopmotor(i); + }else stopmotor(i); // stop motor when key was released after long hold + }else if(e == EVT_HOLD){ // move by `maxsteps` steps + if(getmotstate(i) == STP_RELAX){ + if(ERR_OK != motor_absmove(i, dir*the_conf.maxsteps[i])){ + // here we can do BEEP +#ifdef EBUG + SEND("can't move\n"); +#endif + } + } + } + lastevent[i] = e; + } +} /* @@ -30,7 +67,7 @@ * 2 - switch buzzer * 3 - work with PWM out 0 (when btn3 pressed, btn1 increased & btn2 decreased PWM width) * press once btn2/3 to change PWM @1, hold to change @25 (repeat as many times as need) - */ + * void custom_buttons_process(){ static uint32_t lastT = 0; static uint8_t pwmval = 127; @@ -65,3 +102,4 @@ void custom_buttons_process(){ TGL(BUZZER); } } +*/ diff --git a/F0:F030,F042,F072/3steppersLB/hardware.c b/F0:F030,F042,F072/3steppersLB/hardware.c index a27179e..486ac86 100644 --- a/F0:F030,F042,F072/3steppersLB/hardware.c +++ b/F0:F030,F042,F072/3steppersLB/hardware.c @@ -217,7 +217,6 @@ void tim16_isr(){ TIM16->SR = 0; } - void tim1_brk_up_trg_com_isr(){ encoders_UPD(0); } diff --git a/F0:F030,F042,F072/3steppersLB/main.c b/F0:F030,F042,F072/3steppersLB/main.c index 9ac0c20..5212cc0 100644 --- a/F0:F030,F042,F072/3steppersLB/main.c +++ b/F0:F030,F042,F072/3steppersLB/main.c @@ -70,7 +70,7 @@ static char *get_USB(){ int main(void){ uint8_t ctr, len; CAN_message *can_mesg; - //uint32_t oS = 0; + uint32_t oS = 0; char *txt; sysreset(); SysTick_Config(6000, 1); @@ -83,19 +83,18 @@ int main(void){ RCC->CSR |= RCC_CSR_RMVF; // remove reset flags iwdg_setup(); - while (1){ + while(1){ IWDG->KR = IWDG_REFRESH; // refresh watchdog - /* if(Tms - oS > 1999){ + if(Tms - oS > 99){ // refresh USB buffer each 100ms oS = Tms; - SEND("2s"); NL(); - }*/ + sendbuf(); + } process_keys(); custom_buttons_process(); IWDG->KR = IWDG_REFRESH; can_proc(); if(CAN_get_status() == CAN_FIFO_OVERRUN){ SEND("CAN bus fifo overrun occured!\n"); - sendbuf(); } IWDG->KR = IWDG_REFRESH; usb_proc(); @@ -112,7 +111,7 @@ int main(void){ SEND(" "); printuhex(can_mesg->data[ctr]); } - newline(); sendbuf(); + newline(); } } IWDG->KR = IWDG_REFRESH; diff --git a/F0:F030,F042,F072/3steppersLB/steppers.bin b/F0:F030,F042,F072/3steppersLB/steppers.bin old mode 100644 new mode 100755 index 0aaace9..e50adaf 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 4f8b211..98348cb 100644 --- a/F0:F030,F042,F072/3steppersLB/steppers.c +++ b/F0:F030,F042,F072/3steppersLB/steppers.c @@ -38,6 +38,8 @@ static t_stalled stallflags[MOTORSNO]; // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) static int8_t motdir[MOTORSNO]; +// direction of moving when stalled (forbid moving in that direction before go out of position) +static int8_t stalleddir[MOTORSNO] = {0}; // current position (in steps) by STP counter static volatile int32_t stppos[MOTORSNO] = {0}; // previous position when check (set to current in start of moving) @@ -91,8 +93,9 @@ void init_steppers(){ timers_setup(); // reinit timers & stop them // init variables for(int i = 0; i < MOTORSNO; ++i){ + stalleddir[i] = 0; // clear old stall direction stopflag[i] = 0; - motdir[i] = 1; + motdir[i] = 0; curspeed[i] = 0; accdecsteps[i] = (the_conf.maxspd[i] * the_conf.maxspd[i]) / the_conf.accel[i] / 2; state[i] = STP_RELAX; @@ -119,11 +122,26 @@ int setencpos(uint8_t i, int32_t position){ if(remain < 0) remain += the_conf.encrev[i]; enctimers[i]->CNT = remain; prevencpos[i] = encpos[i] = position - remain; + int32_t curstppos; + getpos(i, &curstppos); + stppos[i] = targstppos[i] = curstppos; +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(", position="); printi(position); SEND(", remain="); printi(remain); SEND(", CNT="); printu(enctimers[i]->CNT); SEND(", pos="); printi(encpos[i]); NL(); +#endif return TRUE; } +// set absolute position of motor `i` +errcodes setmotpos(uint8_t i, int32_t position){ + if(state[i] != STP_RELAX) return ERR_CANTRUN; + if(position > (int32_t)the_conf.maxsteps[i] || position < -(int32_t)the_conf.maxsteps[i]) + return ERR_BADVAL; // too big position or zero + if(position == stppos[i]) return ERR_OK; + setencpos(i, position * encperstep[i]); + return ERR_OK; +} + // get current position errcodes getpos(uint8_t i, int32_t *position){ if(the_conf.motflags[i].haveencoder){ @@ -151,7 +169,6 @@ static void calcacceleration(uint8_t i){ }else{ // triangle speed profile decelstartpos[i] = stppos[i] + delta/2; } - motdir[i] = 1; if(the_conf.motflags[i].reverse) MOTOR_CCW(i); else MOTOR_CW(i); }else{ // negative direction @@ -161,7 +178,6 @@ static void calcacceleration(uint8_t i){ }else{ // triangle speed profile decelstartpos[i] = stppos[i] - delta/2; } - motdir[i] = -1; if(the_conf.motflags[i].reverse) MOTOR_CW(i); else MOTOR_CCW(i); } @@ -171,30 +187,65 @@ static void calcacceleration(uint8_t i){ recalcARR(i); } +// check if end-switch is blocking the moving of i'th motor +// @return TRUE if motor can't move +static int esw_block(uint8_t i){ + int ret = FALSE; + if(ESW_state(i)){ // ESW active + switch(ESW_reaction[i]){ + case ESW_ANYSTOP: // stop motor in any direction + ret = TRUE; + break; + case ESW_STOPMINUS: // stop only @ minus + if(motdir[i] == -1) ret = TRUE; + break; + default: // ESW_IGNORE + break; + } + } + return ret; +} + // move to absolute position errcodes motor_absmove(uint8_t i, int32_t newpos){ //if(i >= MOTORSNO) return ERR_BADPAR; // bad motor number + int8_t dir = (newpos > stppos[i]) ? 1 : -1; switch(state[i]){ case STP_ERR: - case STP_STALL: case STP_RELAX: break; + case STP_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"); return ERR_CANTRUN; } - if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]) + 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 - Nstalled[i] = (state[i] == STP_STALL) ? -(NSTALLEDMAX*5) : 0; // give some more chances to go out of stall state + } + motdir[i] = dir; + if(esw_block(i)){ + DBG("Block by ESW"); + return ERR_CANTRUN; // on end-switch + } + Nstalled[i] = (state[i] == STP_STALL) ? -(NSTALLEDMAX*4) : 0; // give some more chances to go out of stall state stopflag[i] = 0; targstppos[i] = newpos; prevencpos[i] = encoder_position(i); prevstppos[i] = stppos[i]; curspeed[i] = the_conf.minspd[i]; calcacceleration(i); +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" targstppos="); printi(targstppos[i]); SEND(", decelstart="); printi(decelstartpos[i]); SEND(", accdecsteps="); printu(accdecsteps[i]); NL(); +#endif MOTOR_EN(i); mottimers[i]->CR1 |= TIM_CR1_CEN; // start timer return ERR_OK; @@ -235,24 +286,13 @@ stp_state getmotstate(uint8_t i){ // count steps @tim 14/15/16 void addmicrostep(uint8_t i){ static volatile uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position - if(ESW_state(i)){ // ESW active - switch(ESW_reaction[i]){ - case ESW_ANYSTOP: // stop motor in any direction - stopflag[i] = 1; - break; - case ESW_STOPMINUS: // stop only @ minus - if(motdir[i] == -1) stopflag[i] = 1; - break; - default: // ESW_IGNORE - break; - } - } + if(esw_block(i)) stopflag[i] = 1; // turn on stop flag if end-switch was active if(++microsteps[i] == the_conf.microsteps[i]){ microsteps[i] = 0; stppos[i] += motdir[i]; uint8_t stop_at_pos = 0; if(motdir[i] > 0){ - if(stppos[i] >= targstppos[i]){ // reached start of deceleration + if(stppos[i] >= targstppos[i]){ // reached stop position stop_at_pos = 1; } }else{ @@ -271,7 +311,9 @@ void addmicrostep(uint8_t i){ state[i] = STP_STALL; }else state[i] = STP_RELAX; +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline(); +#endif } } } @@ -298,7 +340,7 @@ static t_stalled chkSTALL(uint8_t i){ Dstp = -Dstp; difsign = -difsign; } - if(Dstp < 10){ // didn't move even @ 10 steps + if(Dstp < STALLEDSTEPS){ // didn't move even @ `STALLEDSTEPS` steps stallflags[i] = STALL_NO; return STALL_NO; } @@ -315,20 +357,32 @@ static t_stalled chkSTALL(uint8_t i){ stppos[i] = curstppos; prevstppos[i] = curstppos; if(Denc < the_conf.encperstepmin[i]*Dstp || the_conf.encperstepmax[i]*Dstp < Denc){ // stall? +#ifdef EBUG 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; Nstalled[i] = 0; - SEND(" --- STALL!"); NL(); +#ifdef EBUG + SEND(" --- STALL!"); +#else + SEND("ERRCODE="); bufputchar(ERR_CANTRUN+'0'); + SEND("\nstate"); bufputchar(i+'0'); bufputchar('='); + bufputchar(STP_STALL); +#endif + NL(); stallflags[i] = STALL_STOP; + stalleddir[i] = motdir[i]; return STALL_STOP; }else{ uint16_t spd = curspeed[i] >> 1; // speed / 2 curspeed[i] = (spd > the_conf.minspd[i]) ? spd : the_conf.minspd[i]; // now recalculate acc/dec parameters calcacceleration(i); +#ifdef EBUG SEND(" --- pre-stall, newspeed="); printu(curspeed[i]); NL(); +#endif stallflags[i] = STALL_ONCE; return STALL_ONCE; } @@ -337,33 +391,46 @@ static t_stalled chkSTALL(uint8_t i){ return STALL_NO; } +#ifdef EBUG #define TODECEL() do{state[i] = STP_DECEL; \ startspeed[i] = curspeed[i]; \ Taccel[i] = Tms; \ SEND("MOTOR"); bufputchar('0'+i); \ SEND(" -> DECEL@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL(); \ }while(0) +#else +#define TODECEL() do{state[i] = STP_DECEL; \ + startspeed[i] = curspeed[i]; \ + Taccel[i] = Tms; \ + }while(0) +#endif // check state of i`th stepper static void chkstepper(int i){ - int32_t newspeed; + int32_t i32; + static uint8_t stopctr[3] = {0}; // counters for encoders/position zeroing after stopping @ esw switch(state[i]){ case STP_RELAX: // check if need to keep current position if(the_conf.motflags[i].haveencoder){ - getpos(i, &newspeed); - int32_t diff = stppos[i] - newspeed; // correct `curpos` counter by encoder + getpos(i, &i32); + int32_t diff = stppos[i] - i32; // correct `curpos` counter by encoder if(diff){ // correct current stppos by encoder +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" diff="); printi(diff); - SEND(", change stppos from "); printi(stppos[i]); SEND(" to "); printi(newspeed); NL(); - stppos[i] = newspeed; + SEND(", change stppos from "); printi(stppos[i]); SEND(" to "); printi(i32); NL(); +#endif + stppos[i] = i32; } if(the_conf.motflags[i].keeppos){ // keep old position - diff = targstppos[i] - newspeed; // check whether we need to change position + diff = targstppos[i] - i32; // check whether we need to change position if(diff){ // try to correct position +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); - SEND(" curpos="); printi(newspeed); SEND(", need="); printi(targstppos[i]); NL(); - motor_absmove(i, targstppos[i]); + SEND(" curpos="); printi(i32); SEND(", need="); printi(targstppos[i]); NL(); +#endif + if(ERR_OK != motor_absmove(i, targstppos[i])) + targstppos[i] = i32; // can't move to target position - forget it } } } @@ -371,14 +438,16 @@ static void chkstepper(int i){ case STP_ACCEL: // acceleration to max speed if(STALL_NO == chkSTALL(i)){ //newspeed = curspeed[i] + dV[i]; - newspeed = the_conf.minspd[i] + (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; - if(newspeed >= the_conf.maxspd[i]){ // max speed reached -> move with it + 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 curspeed[i] = the_conf.maxspd[i]; state[i] = STP_MOVE; +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" -> MOVE@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL(); +#endif }else{ // increase speed - curspeed[i] = newspeed; + curspeed[i] = i32; } recalcARR(i); } @@ -410,14 +479,16 @@ static void chkstepper(int i){ case STP_DECEL: if(STALL_NO == chkSTALL(i)){ //newspeed = curspeed[i] - dV[i]; - newspeed = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; - if(newspeed > the_conf.minspd[i]){ - curspeed[i] = newspeed; + i32 = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000; + if(i32 > the_conf.minspd[i]){ + curspeed[i] = i32; }else{ curspeed[i] = the_conf.minspd[i]; state[i] = STP_MVSLOW; +#ifdef EBUG SEND("MOTOR"); bufputchar('0'+i); SEND(" -> MVSLOW@"); printi(stppos[i]); NL(); +#endif } recalcARR(i); //SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline(); @@ -432,25 +503,33 @@ static void chkstepper(int i){ switch(mvzerostate[i]){ case M0FAST: if(state[i] == STP_RELAX || state[i] == STP_STALL){ // stopped -> move to + +#ifdef EBUG SEND("M0FAST: motor stopped\n"); +#endif if(ERR_OK != motor_relslow(i, 1000)){ +#ifdef EBUG SEND("Can't move\n"); +#endif state[i] = STP_ERR; mvzerostate[i] = M0RELAX; ESW_reaction[i] = the_conf.ESW_reaction[i]; - }else + }else{ mvzerostate[i] = M0SLOW; + stopctr[i] = 0; + } } break; case M0SLOW: if(0 == ESW_state(i)){ // moved out of limit switch - can stop emstopmotor(i); } - if(state[i] == STP_RELAX || state[i] == STP_STALL){ - SEND("M0SLOW: motor stopped @ 0\n"); NL(); + 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(); +#endif ESW_reaction[i] = the_conf.ESW_reaction[i]; prevencpos[i] = encpos[i] = 0; - stppos[i] = 0; + targstppos[i] = stppos[i] = 0; enctimers[i]->CNT = 0; // set encoder counter to zero mvzerostate[i] = M0RELAX; } diff --git a/F0:F030,F042,F072/3steppersLB/steppers.h b/F0:F030,F042,F072/3steppersLB/steppers.h index d9aa116..c6391ab 100644 --- a/F0:F030,F042,F072/3steppersLB/steppers.h +++ b/F0:F030,F042,F072/3steppersLB/steppers.h @@ -25,6 +25,8 @@ // amount of tries to detect motor stall #define NSTALLEDMAX (5) +// amount of steps to detect stalled state +#define STALLEDSTEPS (15) // stepper states typedef enum{ @@ -53,6 +55,7 @@ void encoders_UPD(uint8_t i); void init_steppers(); int32_t encoder_position(uint8_t i); int setencpos(uint8_t i, int32_t position); +errcodes setmotpos(uint8_t i, int32_t position); errcodes getpos(uint8_t i, int32_t *position); errcodes getremainsteps(uint8_t i, int32_t *position); diff --git a/F0:F030,F042,F072/3steppersLB/strfunct.c b/F0:F030,F042,F072/3steppersLB/strfunct.c index 5476950..11b7d4d 100644 --- a/F0:F030,F042,F072/3steppersLB/strfunct.c +++ b/F0:F030,F042,F072/3steppersLB/strfunct.c @@ -76,7 +76,7 @@ int cmpstr(const char *s1, const char *s2){ } /** - * @brief getchr - analog of strchr + * @brief getchr - analog of strchr; this function saves 200 bytes comparing to strchr * @param str - string to search * @param symbol - searching symbol * @return pointer to symbol found or NULL @@ -126,7 +126,6 @@ static CAN_message *parseCANmsg(char *txt){ return NULL; } SEND("Message parsed OK\n"); - sendbuf(); canmsg.length = (uint8_t) ctr; return &canmsg; } @@ -422,7 +421,6 @@ void dumperrcodes(_U_ char *txt){ newline(); ++c; } - sendbuf(); } typedef void(*specfpointer)(char *arg); @@ -479,7 +477,7 @@ typedef struct{ // the main commands list, index is CAN command code static const commands textcommands[] = { // different commands - {0, "", "Different commands:"}, // DELIMETERS + {CMD_NONE, "", "Different commands:"}, // DELIMETERS {CMD_ADC, "adc", "get ADC values"}, {CMD_BUTTONS, "button", "get buttons state"}, {CMD_BUZZER, "buzzer", "change buzzer state (1/0)"}, @@ -493,7 +491,7 @@ static const commands textcommands[] = { {CMD_RESET, "reset", "reset MCU"}, {CMD_TIMEFROMSTART, "time", "get time from start"}, // configuration - {0, "", "Confuguration:"}, + {CMD_NONE, "", "Confuguration:"}, {CMD_ACCEL, "accel", "set/get accel/decel (steps/s^2)"}, {CMD_ENCREV, "encrev", "set/get max encoder's pulses per revolution"}, {CMD_ENCSTEPMAX, "encstepmax", "maximal encoder ticks per step"}, @@ -507,8 +505,8 @@ static const commands textcommands[] = { {CMD_SAVECONF, "saveconf", "save current configuration"}, {CMD_SPEEDLIMIT, "speedlimit", "get limiting speed for current microsteps"}, // motors' commands - {0, "", "Motors' commands:"}, - {CMD_ABSPOS, "abspos", "set/get position (in steps)"}, + {CMD_NONE, "", "Motors' commands:"}, + {CMD_ABSPOS, "abspos", "move to/get absolute position (in steps)"}, {CMD_EMERGSTOPALL, "emerg", "emergency stop all motors"}, {CMD_EMERGSTOP, "emstop", "emergency stop motor (right now)"}, {CMD_ENCPOS, "encpos", "set/get encoder's position"}, @@ -516,10 +514,11 @@ static const commands textcommands[] = { {CMD_REINITMOTORS, "motreinit", "re-init motors after configuration changed"}, {CMD_RELPOS, "relpos", "set relative steps, get remaining"}, {CMD_RELSLOW, "relslow", "set relative steps @ lowest speed"}, + {CMD_SETPOS, "setpos", "set/get absolute position (in steps)"}, {CMD_MOTORSTATE, "state", "get motor state"}, {CMD_STOP, "stop", "smooth motor stopping"}, // USB-only commands - {0, "", "USB-only commands:"}, + {CMD_NONE, "", "USB-only commands:"}, {-SCMD_CANID, "canid", "get/set CAN ID"}, {-SCMD_CANSPEED, "canspeed", "CAN bus speed"}, {-SCMD_DELIGNLIST, "delignlist", "delete ignore list"}, @@ -540,8 +539,9 @@ static const commands textcommands[] = { {0, NULL, NULL} }; +// dump base commands codes (for CAN protocol) void dumpcmdcodes(_U_ char *txt){ - SEND("Commands list:\n"); + SEND("CANbus commands list:\n"); for(uint16_t i = 0; i < CMD_AMOUNT; ++i){ printu(i); SEND(" - "); @@ -554,7 +554,6 @@ void dumpcmdcodes(_U_ char *txt){ } newline(); } - sendbuf(); } /* @@ -582,22 +581,17 @@ static void showHelp(){ while(cmd->command){ if(*cmd->command){ bufputchar('\t'); - SEND(cmd->command); /*SEND(" ("); - if(cmd->cmd_code < 0) bufputchar('u'); - else bufputchar('c'); - SEND(") - ");*/ + SEND(cmd->command); SEND(" - "); } SEND(cmd->help); newline(); ++cmd; } - sendbuf(); } /** * @brief cmd_parser - command parsing * @param txt - buffer with commands & data (will be broken by this function!) - * @param isUSB - == 1 if data got from USB * Common commands format: command [[N]=I], where * command - one of `command` from `cmdlist` * N - optional parameter (0..255) @@ -605,7 +599,6 @@ static void showHelp(){ * Special commands format: s_command [text], where * s_command - one of `spec_cmdlist` * text - optional list of arguments - * The space after command name is mandatory (it will be substituted by \0) */ void cmd_parser(char *txt){ char cmd[32], *pcmd = cmd; diff --git a/F0:F030,F042,F072/3steppersLB/strfunct.h b/F0:F030,F042,F072/3steppersLB/strfunct.h index e90cc66..9d59925 100644 --- a/F0:F030,F042,F072/3steppersLB/strfunct.h +++ b/F0:F030,F042,F072/3steppersLB/strfunct.h @@ -36,7 +36,7 @@ #define newline() do{bufputchar('\n');}while(0) // newline & send buffer -#define NL() do{bufputchar('\n'); sendbuf();}while(0) +#define NL() do{bufputchar('\n');}while(0) #define IGN_SIZE 10 extern uint16_t Ignore_IDs[IGN_SIZE]; diff --git a/F0:F030,F042,F072/3steppersLB/version.inc b/F0:F030,F042,F072/3steppersLB/version.inc index eb844db..5e39d96 100644 --- a/F0:F030,F042,F072/3steppersLB/version.inc +++ b/F0:F030,F042,F072/3steppersLB/version.inc @@ -1,2 +1,2 @@ -#define BUILD_NUMBER "139" -#define BUILD_DATE "2021-12-02" +#define BUILD_NUMBER "156" +#define BUILD_DATE "2022-06-10"