From 16f72f68010e558628ddb5a429d5054aabfb9849 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 10 Jun 2022 20:07:41 +0300 Subject: [PATCH] fix some bugs in 3steppers --- F0:F030,F042,F072/3steppersLB/Makefile | 6 +- F0:F030,F042,F072/3steppersLB/Readme.md | 63 +++++++ F0:F030,F042,F072/3steppersLB/commonproto.c | 44 +++-- F0:F030,F042,F072/3steppersLB/commonproto.h | 4 +- .../3steppersLB/custom_buttons.c | 40 ++++- F0:F030,F042,F072/3steppersLB/hardware.c | 1 - F0:F030,F042,F072/3steppersLB/main.c | 13 +- F0:F030,F042,F072/3steppersLB/steppers.bin | Bin 24988 -> 25296 bytes F0:F030,F042,F072/3steppersLB/steppers.c | 157 +++++++++++++----- F0:F030,F042,F072/3steppersLB/steppers.h | 3 + F0:F030,F042,F072/3steppersLB/strfunct.c | 27 ++- F0:F030,F042,F072/3steppersLB/strfunct.h | 2 +- F0:F030,F042,F072/3steppersLB/version.inc | 4 +- 13 files changed, 276 insertions(+), 88 deletions(-) mode change 100644 => 100755 F0:F030,F042,F072/3steppersLB/steppers.bin 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 0aaace9e3e09753aab987dc37d7ddd370557e691..e50adaf4660a7e70fc004397ff2ac3ffb5670c01 GIT binary patch delta 11511 zcmb7qdw5e-*7x2gDNQfXlyXTEXiu90DJdnr0EL32X*sk#^iHK0mx^Pw>FBG?%jsiLCJj0}@-aS8 z?F9YB_u7~bz5-bNy}a+*P5Spvx?_oE$*X-TO3$2ln=&H3#~JBU(r28Rekn~gzyWiq*!%!CQGVJs(wy9O-`?DT}t?%PstN??P?SC&~$xk>(bLgzO-7s zIBFqE&yikIZ={b%iBZ3ws6|6O;Ym15!lfP1$aUh)m##&XISa(Gq}x#&HHLKba(tpq zj@4;A}lQ#vkOF8+Ipowtu?O+!8%fV7e|1mNX{DmNy z73(G$rm6zS&ju~4Xg()j9XV?I*8r2?yIm@ZHbl3H^k|RJCbdKx z=|<`2_->JoM9-!T(m?dQI1yt~E92fPrD-PqFm{B;D1H(!K1}D(xzF0Z84!dDfsL#! zkoGXiO!N#JUF5JZm8iUQ5t$Y3PJ1|CemH|1*7zgW*U7>dPB3#IS1N@j`>X(Inz8_jRNr1z)IjEX4cwUyv*nz=CK}|IN)$a z8$4Fgu-+A&;)%{!RB>ee6kD37wQnKjP^>cUSg4hZ6f{`v?Qt3wk>|^+*Z#?L&;Q?PF=#AkEzSBYh)6!aEeHt1$#c# znOFfTpYU5qYadTy98_L0G=_Zbqtm}dYu}aE_MeU!;~~>o=|}x0`*-C(lwM9qA zjtegf&%>pw`0!wZpE^wgZfZm`d0oPQFRvZOZccVnc?tF@c=^5|V*1!~WU7mZC09C= zch?Cq>crv#(pXI8Vm~pt-3n>E(&LCbBY2#>x?%vl0ujYNet{Go0Hi9>(I`3Yt2p{AF+^mTp7v zp=G1?>kBSIdJSZDOvj4@g38UoR+K*%+#>26RQ`D6exyDK-mj!S9NCQ2>EI@@OVD+) z{EbMz9xN6bkUAZ7AoVIzT}Yh_{zzOZxSYZ|oK`*fUO2B?$S1M9CI8do*cCyhH&>8t zqjY5$slnT*_Mgfil7rQdKA}jH6sb0NcsG@wQlzmsX{r1ZMXFY$H0anvA$u5GBi% zk{+exxq#yr6o)s{(;ox*kQOS^cNFPc0n?LIzDJRs3}i!^737_m-w8#0EN~m**^2m; zzzm3|DwPi@(q9D%+M$&Z*0c);4VA|!(q|OufdHNBhg=)v=FoYnY~pV6v67DmzHidZ zU|L~c%s_U4GGn;s2{6nu4uUcRJPP_ca2hxZTm-zp=YStjWB*P7G5|}}iC^o^(`nD6 z9<#oE$YbHs(@3+ncJ3rZg~lmI$P?1D`k&Js(lov?D(Y8j z`(U3|<`{|G^Ie{mB9BAfzVGr%SRN}kE~M>a{)4rniSVo}r|?{Pl7r?Y*;&BHVn&)K zH{5j|2NdUVpft8vZ*Ow!I>uR>?0UyjrJPj{`lLde9KQff;G67MHauEd&I}PFk$xoJSK$)w z6Ys0)!Z+J1q$khbEEpUveT!M_4-Hz-UT$EXJv3Mp3Jty+P&-Y;^SF1Oos+*FB648} zoY&k=%!QuN;F|$;ir-T#%(LqpoSZkjQy`{G?qWgb5QQ_#shRVr+MVW&ZccuCs2H7y zB~2;g+hb=gb=!m_d!0~@yo35WVTzq(oxEBsl$7g{yEt8MUwn69PByX=m)RJJMCDKX zzwz0zKfeqR)0^%lbZvZC>!6VQU}z3(xoU_>?ZebX%vv`wb_+(bOz0M7Lsm2-3c6MB zp2IYs_S`()WBa(b?wU#Q6w z4ao(0hV28<`ImCUD~&ngK%+q|t+t5zRjQfPjeALbcEwsJ$!yLRvrCifvPHckTf9<} zEg}$;Ul^ezr>Mh7=d-9R`NGVync}q4fyT>gXbvaGjL23~oxiV6pNy`O?{@WP#G#YR z2O7^e_N@N1uO;)-^DSvNdbsR!Oo(CI#Fv})t@v8d~ey<7R{C5@(eX&_|6duDXo|tY|>KyJfw-yN)4Z56%*2RZ(e{ zNHSakzNhyHS>mjUZtz_~Az6VV#Vk7Zq|}?mmv^Vs&lIi2DfQHq(!t5$V6#Bdty-OZ zx_G-&UmqS^Ir7J}f)2+oIQhPyC^$Qg>+Jf74h+#L7wZJQgKY%1R>&_mtm5V*p)uUq z@~EUX9u&BFH%qRUcuQ`SsIuPmsE&Q-b>+S~!G^ly9X+ePm=M!YD>T70$TY(POp6WB zpdEZWt{KXVnL4W=xGk;b&GA{0V$8EjYtt-roAgZD48>BJh-I^u*xJawONDbnN3U5_ z6T`{`vg;TzS9myL5+~%=5|?gOX93A1oSZo_E@kmRhWHZc7XH~_EdRlpHw;Ir_X%m` z^ktqLsSXbw3R72Vd1~pA>eTWh)kMx!BD~YbiTwM}lu{zkbcR_dGuy96A|q@8pXt|u zPYH`?vS9;}r-dgXJtO_tsUZNujPFPrv=q zV|CFFUaA&%3L5lm2PnUc*Kk{wIL>NM!-$vk49+v43YBs`Y5k{ zbb$J2pjqyi_tYPRSB1lZ-1oSCT(;`k8sR)f`%525H;4q~zW_H$;cokA8oWhmay>C7 z)f0rRtmT(SxOA<^q2zctDxm(N)5jL-j^(k1S`IKBvc>uw_y!09@yPoen^;#(G;6`b zzIhFJD=4#fwb()4X;6#rHW1#Wk5i6m)(%^EzhcP^(0iBA+}Hu%kw9qL8#XfFzaP1qj_I+x{g04)YU#~BW`~mF=kiR)n=E!MA{n1uehLV zt06NNUNcuc(D-m;d*g?b&nN7}5)T|tD^FY%SA5G&GO4?*@sS#0q-ja@=}f&&$6n~S zMbIS|SXA2$(dtW6#gy{q##)@IQ?VzwnJI3zoAmVtXm&dS^U)n13NUZ@K(H0d_mHCR zZv(&c#aBJpxE)a`pAsd~C7>09ym59xInRx&Ta=SB*BB8b^OVkpV;A%AE05(o- zCIMY{XLa#@yz!m#&k(Iz!wO=~y+&OJg%{zK!@9!V!`N{4lBD8YHhPU0yK2j*ixI=V8r~M0 z8+vSM18SqWwXl=+yy+}W>KF|olI#Spj8R>q*3~_H5;}F;6 zXj)mS!o_8Am=cvM`EEQ;&cEfow~Xt#S6p7|YG5%rm0t|0T>aL9j$9uW*kkiSx9(D3 znOORLZTBzsReH-Y-jv}nPz-Sz^IDg}sVo(5EYHQDMx)2!G?%_y*tdH(X}|--k1@pL zz0G@ybkX~}aUOo{%@%hbXZFN~nWqdhw^WT`ejcJk{k>s6cwrU>IZHI(9b`>NjX|c0 z1s#eVUUQzfHEM_k#SqE0i??3A#ZH#L4zWFV?+`B<%k-;w438LJAa&JzSzYzGvc1)f zLYzINY)?bon!MJJZjIiygK#tK62NvG+l6e`-3hsv`#FXkH|pa?FBI63PR*UIxRn(~ z0Tw5-8^xVl#3AGGc@uled_CMZe4} z?9x!J_ySQ$>H|Fu(JqfZYJZQd%~M%EKDi)w{C4MoG2~+NttJ5>yN!76y;X*}*A8(m zOI|;sw!irWq|1k*yNFyhTmrsis7cT|h+I0%^z(tgN#ME?9pS;{%&t6hZ#4_iJ$ifY ztvL}pE{NDMxn{bx|5jm#%4jLYy372KV!uvw@{*s$1-cbT`1Kd@_`T!WaE)wwIOlZ}3OER6XkMy6XGjVVQTm<5`(E zwPHDz0W1`-gDwY{SJeSLPypOH$k`F3y!znBxML_!*$Yr>>o_HGN6;gU&7Pe3C%;5{eC2!yewJzOZL)v8l_ZEB=?q&_1ez{UyFDKIQ0XfIS9&b zvji*^}u~se5+n`sqqL)sc2G@I)FO{4gBLrz`m-k^BzhKopa_ z3CUDNGA|<0D>aTIKUv8)NAeTJh7IAtUn75llAkK2S(2RN6n=c9j7BN*3zWf26Ep+8 zVZ5Fd&g`21xYImO(fU)Nj9DLfI!?WgyUaIe`OZR^mMe>qq8h-@wu2$O`<2*cVd6gy z{mUogy@2ksL{xacbRs85tj8TJCTrZ~6_)T|6`t6>@bNLvWywe{w1x)-MJo4OAhkmJ zu`j-k#RcyRq*q^6#uviiGTb_478YrCZgTBQp%P(%qN0?@crExL^o4L?JDtPLJz|T! z#+E@Fy6uZmYQ;rLjn3q>&;#={p${D%l@2nqu9JFl3kp9T8C?epCe;+m;x{OjFGO;G zW?ZX87O8MjPk3|C!Tb2tc8yZzZE0+tfu4{u^QO_~rK-Hi$xn~$$2oknq3ig-7|vsj z2@g&T^QhA)IrAn@-7&IRL+3gPnYHikZ`u|70;MgeEN^z|y^&t`8_NY2&#-o^?kql2 zDYI0f`G&FcM;`E{EGtv?RGyTXKXqnCq}D#Amb-z(zdJ_dX8lecdCkX0@ob2tv?Ee$ zzD{2N9aw=ZyWxxEh>1eYtC?(-@6Q zG+t=1i@E~Y?IfGM06Wc`+bZB9h9617gHHr#S9mZQMpUGz^s&IGN?EREBoieb36R;b z?pS6RPWXeZkpk=oH5maXd_^Gc?!yi-#2cNLM=VzKR(2Cn#a3ZgH)yxHc$DdB49T6zuvTbH0}S9y!usYO~|kl_?U zkJ;kf9Xym+%;q1b%$UtLnr!!3g>QULBaeAs1nm+$Fb(E0UV}-T1=wE;f}jN6qaU;# zGP##d>zI*Wxf$1`UB|ZVTueC_h^n9Fv!xjFj7}jo^t^4Vd!uuT#vMIt&QlHa~?cI@V|}I$ zNE3hcRr0Y)&NGpm_c3!nHcvce%7UoJK(l~Sz_DC9H$UAejXY+{a?`FH6Mt_eNw6xV!U+P{M;1!>VJJzGeLA87f5N-}tzS zPkpb);SC*Kvi*4$vXghEau=M6gaf?!&p}=FdMQ&3$bsgyJ zu@$*H(#KQDX1-Fy&unjws+p46nqF2*n#ZOutE*)P=NbQZJIRVDYV?>t8Ob%Zcr}z1R7-lTPB| zJF!+v6~&Lx>Jxt|{(#o9HlKQn5LWyxP^U8-u11~=ECX1DQ*3yUQxzeHRuhd1vJlbUZ!V?oqKTxHUXxIzz%^0HjZ{Q!mE#L-l7#JQSg~YjO zFQCZtz(Jr3cp5kWJPGUp9zRjPEQuyBhaXxAtOXoEJJ1O{A?>U%;7leYEskiW0X4wo z@kH}BFa*2`{0jI9@}B`-0=6d-%{t%`_{4aW)1n^eOTfCZM3XiSJn$K?C;@Fj-V5vp zHUniqGLQpMEt#tM0);%lv%qGcW%d0tw(^^|JhU#BoXDs=L!E6~*R{5;d$29L1+F|d zXMW}az5-%?>lVIk>*`IL)~)5&u77Y{%hvT|uTlme}S{!I}U2EHSWpt5_^YT#0wrNYtMt;q9eo5U5$yfDWZri%8eCw9B z^{jI?B1U9Zo=JHlP*OHq%%QAOXjzy}id23Qp z<(h46e9M;2n^)hzwvFGkzHRFQ=peOL=4k#2ZT(S|nW+W6%)Sr3u3po&Wz)8;>%QyW zwEq6l!6VinC;rwjht9(AFgnb4-PZqir4unt2`cBQRahA`a0V1VtdpytuzlhO#f5~# z;~a|uodODHfA>?fh-j{76HVi6toj0?$pq%iC7Stpuz*~m`4eQ9fI0J#H;HI=T8QQg zq>FN3WdLgkk+xI01F?>Fz=w1Ith0TfT>u-%QP9_bG2q>x7XkbMg?tVw1JU5sE071U zR&}5}z$TRinh)TpAj?3j0XE5%plg8`f*sKTrW3$YlBYor0$|zbG{?XrWFzdq3#Y+P z0`CRA4ImyQ6B^JNFb=#GbQz!nzX`M*7!RKP=V2fCiQw7)9$o_vM@ra#A}%_SNJ4`B zDMAK68GJ?+uDp#zGZuUnXaO(*{Blq`09F~?6X5mWp9eh*q=LT)dKH)o-VaKeh$bC; z251(*gD(ZG29m)$+rc~r7?3yy`Z_QL{8i9T0V8-#Ge!)s?RE$%F94qcz7(_?0ILkh z0e%=haR=yQKmh#Hpw9y#@W((;0VCkupck95{$C++8wr^Kym}?p68Hu@51Iu8!Iy$o z17Cwb2zm(cgFgj&1^^rG2lWC&;MFi-Ex??#3Dg4o3;Z(B<-oV#x2}R;+JVIXAaNM< h=qhQF{T=D5{jlyZ9uyX1V(fK*5-TC^FpJWV{|jM{z^(uQ delta 11288 zcmb7qdw5e-*7rUqDNPH7^a7+w3#UyBX;W-^0a{=rB;}Cy^j<1$17o3}K|rE1V|AQK zMbs8`274H%H0a=n4mkJ@f*tjhI(58^7kuAD(XUX@pbkMtXO1&y(j-m3-#$%|`R99{ zug~+#T6?X%_gQ=Gz1Lo6pL*V84!+K)iJ5S3t|6+|fMMX#RGo{^0*U`o{x#jCl2-Wt z!Q=6EM?r_K>0=}Kp8$Q=$bRTI{XZY_d@@m`JU^^pn72>9#!w=@$?BOq#4lMR^PxCf z@#&l+4B<>bIiLpG2;ro9F77$Z6EBz_LL7ya+ zsM6V3NaR(V{!ElLT=A^t2mSiqQX3t7SJ;f-?Q&P2hOB&!*r&>8Y~mkPKXFyU(QqeM z4r8g4&9n?&1LOe*>xjxZ#MkN_#F*k;EsQzbiSxZyLATYNnCnfl|JQ zb1#Lcxm!Z4)E?p-tn`&!-hgsbi0Jj5i{D71Vi0a<2zw3K;3% zW(rAkx&8J5h75?6vx*C7$vNc8OCd@#TcjQxNt)&*3pm0^n}dc<6LAi6B+0=@cZ#pg z%DM4)h+3c?9^qYj^^n5MaIBFX)u8s1VfCx;10h=bhq?;6?$2cD zwS7#=-Qp^B=8gNu)x-3OwV@EK55jr{adGM)jhQv5M#qv^FC*=a*D?J+>TC~A_H+06 zR6np!+A6-RE=b=Sog4>)XvlIQ*+bu{7c*BxV`@nX^$EF+CA9u4;?`8X?(b1}R=N_s z-o{9Oi*m$dVx+EUGDg@b{yw#d9g2#f)PC09-2X7f%4##<0i`) zSIY`=PcDPuX%RwyVbYjmfg><{guh4c9Lgp$NP<~mVLeQ~0aKwU)0m@yT`{gR;CDf< zF{cpLF#Rmilf3DjKGz8%X(EPJrd&}J^wO0=Bz45lIqqL6-~o=6LZ)M{CC#JYw4JfQ zBSF?+@Q~bP0>f#YvB3Q(t352@K@T|MQ4jazmRF}KY+Dt|bh9dVw>91R(8hoJncN<( zxZ2DLTL-N=rNW$UWx0no#sXV{F4pjc_X~e4(1{Lac^B5CcV`;sjuL&kGe^*NlS0m` zueNO?h8#~#*Z;!tv49X{3`yQ>)Fj5QJFW{9eM;#{w(RPpWq|w0|>Zj%Jpn zJJ0#k&{LYw(e+3FteUj$nl5^P9hGb6FGXkXdXrIo!uZxGgX*vQV|e%o=9eCghk(NX zQ4$-;AT}kP=OV@|Dl=|cIwv$u&~$#_SJfoj(mShivH8Zo^q&sI=Fh7~fI(HH1RM@R z%n6#&{J??OEk2p?InyWJKl{Gfv<}tLdW2)m#6-*)V)mScuDk1ghJ|x$#L$b46e+~A z`6_&8?c_Gw8Tk>)pM?#*omNJ2;J{?0vG8rEe?!D6XdI06i;0~mHAHsGrEL?pqGXHg z5C(WnFO_dc*&MO*Z7A6yTT!Y+X#k~Fk(-4zyxYa!gi~-3zblp=;LAxeXDa{V1S1^@ zxABamkI>!zyD&B5x8ciZwm>%GK)4aw3|TA6+I`_8eT+0$*6t14piPjqdt~jsVaHxZ zQpwt%g==BP_hH)ePr@$R_3dH9LyR$3xQ=)_ z{B$tW!i*FONvB@nPw+?iL&$*WqRFer62VF^fm-N_FoU71qY;z_m`>2`z%HN%=mquz zhk#?iOTgQ}DDXx7$zNzbVDb;69bJJ{h%Gb~8$g-%b??1|DDV)5CB!WLMEf9P5+}69 zt{&(#s7vdphZv4Xmm?&X5wsiC!&swc8nB7<$qzC!0~rh1nIB|TR7UIdPtdqD+0Vmq zp4NRp=GmHb2ew=%O(tnR%H&9GLp1NI=U4L&3O>Z22W`n#ZD)t$zT>R9qf_hHV`I%) z(EDWC;n)p2i|gn-cgt?;xlP(_E-LRLxr3b@+xA*Jy6&k_m!zWo&qPI5@q9-uWGFF} z$)GcGyBvs8#;|uJXKT$Z!YyJ`R;KISa4evTveLZqO+r^gTW>VLM52M|AtH5+_vkm> zVppbHyEerFh0*oGhPnZMgWv`Rfdjn2-%z9Nj0SSihbAoW^TAg{qk$K~O2bb*w9HCx zj}z&w37Pj95yFbR(ZGL&r_Bp`sm$u)H4avq7E;a|^;p4AYh{X9ucF&!T<>9}$HuL^ z&Y{cfn3vX_T)4zz;nO=?Aj#6T@L8Rt=+*O9zJu37wlY`S`J&&6&37nF4DZViUo4d0 zjp(o++bmdyrRon-y*fmSrX!IyLGnp(JMw5LF~pQJgP)%mv|Jgb>$Nq=NY%kV_^<8U z&g;69jSqNIj7qPaw}U5Mmp@Br!zdWZ5V_NzjSl~WmPNtc{?g{D)HGZ*Pv zIOy>A&nt4#h4$>Tgd7Jw#sXLfFMy|^kS+=b79-8iZJ^tM;Lu`WaovLm$miNGx2J1~ zvFbu;^*il0GGM73`>Nfpc58}jK5Sxgiv?|Kv7pPWDAV0FmT3EEk#MekkucV-Q;Hjl z1nmYzA>&~hbG5C7b(=|ny+A0mWwsOwT1TO9uBlK!Mkn1r!H|+wJ$j~+rgt+w%&%S` zaJI4bvzwR_R{A<3nGG$$;TG*%{;q;=Kj_N&r(Z43nm^Yy9R|d5lVL#eRRjL?kxEFa z8EgNbeQ@KS{ChCeZa@iWfGU8x!a;|QVLDM(0}g=XT_MyJSAcye)BQ~Mb@1wD@eRcS z!H7{Ps(156LXoRs5QO9%parbaGE&dKjD0o86FrktP~+H}-fE~ZTT@#*_@jO4J;<;p zh(5o@tWIyO=)Bw;3p_UQc8;mX@d$nAvhqDAG@aV`Ky-=OXIglzL!b*0p414LHM$M# z;`CNle~@R(KCe2+Bb`t#WtaG>E>?Y3r6_vCdu^y|;cL(|%`v#~qMwd%7e+$8Fax8e zqss%RCw75SuQ-7gl)n~-IHSuH*U~r^9Qq%A4c{}gKu{9h`k90nS9@8KCuEd16SwBZ zUK6P#taR%{%Dkmx^M&7&1AMq`Ud@J0cj#Ve+{b6vWZPb8B+`-?P0mJ;2%U^-ZA7{* zW+IFv1U29dF`C4!4$c9e8x#0&dt2KrL@J1-L!gaW`CuC(EeR$gr2RTdq`oM_O}*2; zpZTVrZ~;FxpALr_OpUJvoEU01u!jyA7y!ht@jjj{X6G!Fw*;+9n^14ibs`Nl@E`L$ zKQgqoK}nWtD$2BX?LOX;_>+&Cg!_rTy+3i=nTDF?_U2pMn)>PnTTT1Dc<~+7?&Irf z%WA=v#Wm?0m@@l`^wtF9 zGOUGAAw9_QPfDao?%`=AY+}@Y3)D z+NA3g7dG)9U>v{plU%JpQ2!fnb7bzapUIgc%(Z2i3v_gXfLHCy&tI?k8ZJWXzB z|D~nXld$~9mOEBy7T7H(t;O|atz<#5Ft4?^zF4@eiOH-eNxIA4-j}%gOi^`pd-W}L zjj_79U1%~29hj>bn5)-13WQAv58mcjAb(;Ze0*?XYQ8cj@=)GC;cO=^>G^0b3csAl z5!%{`^lX$v>A?w3F8ykPP6!K^eIH?Rvf$Q^cAa3Cqoh{oXis-cx2AUzAB%T`1ibqs zO&X)6Z=O?YTV9~>8ub?S$kgJU^t634l2KP*27-VFl~j#zD=6LnbnnxMh8IL(H~Fxw zT@wqm#?-^K<7u))J=CsIl31W7MhrL)b6Lbz=*6Z53tjZFjG)0))uc(+#zorZpCMwH z=4ljk8yFl?_xf1ljh?#dO{C7&(s93rDf_0^eWIoLaZ7^d+Bik@EzLO5YXuKTbm@J~- z#4|aFsshREXsN0JSCxcRc zq!{HxkrtlL-6?stbdhKMoS<9Bl)1f};L_C{%lksTI%Q{7y7fK_b0J3jID9tr_4`9Xdzfal1CjNJd2;%?L?edgpt> z>AC~_Y2kD|Bc2xCM+)#hQd5CH%e#4_t6OO_kG@;u#^(Ah|M>|GNf2X!?g%juZ~nzM z1%>z&VZ}Fu&u<_`a=z^%JE(BG+m<;*Oa7QrZ*D8VY)kL4!pk}bB7!tGoG>uy zjs2k)G5lF!g_|85Xd_Z#QMLHUPSKZM#Q2lP> zlhyCFb!;ki($s@-e_q9wF`|;DAe z2}YlHA0GF7Xpi7Kp(RfdZ(B(3CI|VARk6Su)I**=p8`H$mX|r-^;5Gigx!AXLl!># zKS;0ldw&}DTA)TT;lS5t(!jTzn1MwbguYUnnQ7x6Rr|6#b0EB3n@m z>K;;S6ZZ{Ri0gdvMLU1*iOe24&o|)M_^_kZrVtsaJH`-&JM(IiyZOV$YId+usJFS> zXqwAN7orOHs2OwFljFzg@Rs-!U=Z(L?9vOtMubn-W$nf>p|Etjhk- z2&;^O;i^^rQDxk!xs$%X@XVzjRxP>k-lfBrx(W^;DgO=0w+L4DLmork-rAHk-=4g0(81jAK3G(%+ z9DM{by5m1T;nI-Aqe`rpy`gpT8e#hf+Y;SgZNj627OYNROHEp4MQPey{nN;q9hc}V z^tCCdr@S{^s)wgHjVs-zvQZ?ep9Te#JH`_Sh;(PD8vNREcCf0tuT4c1j}mEzymd|o zi8K;q2d}SI4)nFfT!GH8e>El!3U|(| z_tS=cYJ-Vx)Hu-6Xh)!(3a)$LdOCIM8;MKhz+Bt|H~|`2%YhZZT3{-Fpa+k9-+@nD z$IzIecOP+nO8yw+Bm9KlIYjaj>Za8j9VU|HWF+0VQn;=&%jv|O$4gib$5HP&fMzwi z?&&nz!~%aqTJxM(P`ogKs$LRXi}fW;R6Xp25me$pKABU?l*a=96XOsZ$G!7wttMK2 zD_+j3%@U6muURu)t~D5!Uh=E(kjZD<9-}5)p+jpTs#Cze4Y(cxois;D2zpNN&|dEr z7n<^!o5ecQT;@@6i)p^AK-EMF0q_vptGsOD8@I9XF1*JR5r zPC%X|%k^=2hR}9PEbt8Eb7lG5YkH!Z1JI<_GA+$VEI?w@uMY4$95%vx zFd%vvI|}NfypotNgGxXUpo3ivcg~R?=Zj9dGovn}J{IVTl@pGm4mcB+Z+B3Af@cR6 zz8e=NbE?I$!0XX%_^8Hs4Bemkr(HE-FiO2lnsHfnjd!J?w|FZ@hUUo+eJ0)y^6LBn zURR&hU_hRefA!&yiT_(tyb-Ti@=W8d1y8#(n!D&@?+>%t6zw_n5?+SXc+%87u4uVB zm-7^3_}Ut{1ea7<7Qs>LqTlY(>4;P+B4^#>-q%(af{!OtC<5l<2?l zb4kA~l2Be@js+ITT4~e-EeGufWJBlo8k-z&|W@kq3aUnaXS zIw4A>+9GMMDfkC{4G5b`*2?S+Q^L8r?FmhQY?@Y z<4}9o#G{aZ-ZpSzEH0ld%N?@(+WJZI0$V?^L&Yp{5wd9C)i2X^+%oxj@L+i{vqW^2 zXEO`M$IEqVGbe8I&#SGLcV>Fr;(huWo@Q9|iJ6|F_&QhqhZYeGb}Bs7!a(E&KehMq zC@o15amHeewibR`3#)D#WAUd)hV|#-`;yr+c@r`mMNZ{C=-Y$E_PCEZ_=}<)XI)mVS@!FWaD0DaZ`_QoX;(Sk zQjBN?cz-Vu44)!~WfT5wghx;!wpR`=aO`%XCiVq!qF26d_K_?wtc*IBpuQTgPq(tKg(S-EKGq_>y3CNJg;$C9v`=vmC!j}HH0y$i+bfUr zy>XFUo|pR4xIsQsV)`sIJs*~dOP7~1kBQrsH!*({hnN4MU~c?j^h9!xbDT+TJ|iw& z2{%nHE`|8$3LX1hQ2ehI*Rvl5#eb|Qa=jXyH1yj`5g1s$7KiUu={~KvEhDTpt&C{4^rd zDWV6PvlXiaAOPJQYe9>EidJ#i_07yF@saE2GmQA$_4b5QXi|PMW#ud;sa%QRpujb_ zm~Za7ru3-!57||aCtOcd8-Z28R9)I!DYma#oyI27KUh~gbj2iO3m8j@5zZN&FxsjR~#P>Ee7&wFeC|_~1&h}SDt|}4#p94gu5!Ls=zkq)LUjfGfHc?zs zyCmmXR5=111`YuSfkEIgpda|<$wz9_nanK2s63zuFac(u5m+m}T&HtYPA94#0b7CB z(unFPa1l5F^Z~zy{5fD4*qBOGb-*{^3ud668tp)bfuE-kRpm_Z05glIT7jd`KMvdl zv;rjn17reULjTb$lB9YW%>BSgXZ;(v#A$q^bjQwJJ8$Ri*tOH?+_IU&Y$g+8-FWN5 zUEGeHKi!f{ZZ9ONj}5qE162ci3&p=QZJAY8iMpkx(vn3brHf3Z;*#b+&)m6V$HrSX z-@$F)ddIG1$tS0`NG!AXWTf?&BJqwbyNYk#vPqQ`_{umOvwN! zz7mt;pqO}a8We93#0Q!HDuE(sUj0k@5#-mw4ZvN?h{{_D7cR!|OEFyFn?*!54!mDM zR2$~t-yDmHDiw9_Hxbo$0PP6*txJA!p%2`L4YVJi4?PTe44?*$fW8e-BfbE=3?O6@ zas%oD)4*y#IUo^y5okF;tC&G;0Q^ZhK{o?*uujl!Kt)`H><4oYNP^-B=y3onwNCX0 zcr|$XA^r^b+2F5$MgV+@B8GK%7?1*fEodh&m(~Y;7)S+AKj0q*kLW?@NBpzXx<5FaiEB=n>#s@TWlE=)n9-D2$@u1FnF-0!r*~ zG59RdJmBBp@m)#P4FtjO104Xs#*Tr$0E~kl1^oh`Q7Hnd#G?HkJO^q3z5{OqZ3O-W r{+{(p{FB9cv3}!U6j%x3tW8HX$8a~W5`&}f^$al+@)x>dROJ5xy<3ji 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"