From 8741e2e428f4471524c5c6ab4ff45ea193bd4083 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Thu, 4 Nov 2021 02:03:33 +0300 Subject: [PATCH] add some code to 3steppersLB --- F0-nolib/3steppersLB/commonproto.c | 71 +++++-- F0-nolib/3steppersLB/commonproto.h | 14 +- F0-nolib/3steppersLB/flash.c | 4 + F0-nolib/3steppersLB/flash.h | 14 +- F0-nolib/3steppersLB/hardware.c | 13 +- F0-nolib/3steppersLB/hardware.h | 19 ++ F0-nolib/3steppersLB/main.c | 6 +- F0-nolib/3steppersLB/steppers.bin | Bin 18800 -> 20812 bytes F0-nolib/3steppersLB/steppers.c | 326 +++++++++++++++++++++++++---- F0-nolib/3steppersLB/steppers.h | 39 +++- 10 files changed, 435 insertions(+), 71 deletions(-) diff --git a/F0-nolib/3steppersLB/commonproto.c b/F0-nolib/3steppersLB/commonproto.c index 950d87b..3e56758 100644 --- a/F0-nolib/3steppersLB/commonproto.c +++ b/F0-nolib/3steppersLB/commonproto.c @@ -160,29 +160,45 @@ static errcodes extparser(uint8_t par, int32_t *val){ } /******************* START of config parsers *******************/ -static const uint8_t bval[] = {0,1,2,2,3,3,3,3,4,4,4,4,4,4,4,4}; static errcodes ustepsparser(uint8_t par, int32_t *val){ uint8_t n = PARBASE(par); if(n > MOTORSNO-1) return ERR_BADPAR; if(ISSETTER(par)){ -#if MICROSTEPSMAX > 32768 -#error "Change the code here!" +#if MICROSTEPSMAX > 512 +#error "Change the code anywhere!" #endif uint16_t m = (uint16_t)*val; if(m < 1 || m > MICROSTEPSMAX) return ERR_BADVAL; // find most significant bit - uint8_t r = 0; - uint16_t x = m; - if(x & 0xff00){r += 8; x >>= 8;} - if(x & 0x00f0){r += 4; x >>= 4;} - uint8_t mostbit = (uint8_t)r + bval[x]; - if(m != 1< MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val < 1 || *val > MAXENCTICKSPERSTEP - 1) return ERR_BADVAL; + the_conf.encperstepmin[n] = *val; + } + *val = the_conf.encperstepmin[n]; + return ERR_OK; +} + +static errcodes encstepsmaxparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val < 1 || *val > MAXENCTICKSPERSTEP) return ERR_BADVAL; + the_conf.encperstepmax[n] = *val; + } + *val = the_conf.encperstepmax[n]; + return ERR_OK; +} + static errcodes accparser(uint8_t par, int32_t *val){ uint8_t n = PARBASE(par); if(n > MOTORSNO-1) return ERR_BADPAR; @@ -205,6 +221,13 @@ static errcodes maxspdparser(uint8_t par, int32_t *val){ return ERR_OK; } +static errcodes spdlimparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + *val = MOTORFREQ / the_conf.microsteps[n]; + return ERR_OK; +} + static errcodes maxstepsparser(uint8_t par, int32_t *val){ uint8_t n = PARBASE(par); if(n > MOTORSNO-1) return ERR_BADPAR; @@ -236,6 +259,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){ + init_steppers(); + return ERR_OK; +} + static errcodes mstopparser(uint8_t par, int32_t _U_ *val){ uint8_t n = PARBASE(par); if(n > MOTORSNO-1) return ERR_BADPAR; @@ -246,9 +274,16 @@ static errcodes mstopparser(uint8_t par, int32_t _U_ *val){ static errcodes curposparser(uint8_t par, int32_t *val){ uint8_t n = PARBASE(par); if(n > MOTORSNO-1) return ERR_BADPAR; - if(ISSETTER(par)) return setpos(n, *val); + 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; + if(ISSETTER(par)) return motor_relmove(n, *val); + return getremainsteps(n, val); +} /******************* END of motors' parsers *******************/ /* @@ -259,6 +294,7 @@ static errcodes parser(uint8_t _U_ par, int32_t _U_ *val){ // the main commands list, index is CAN command code const commands cmdlist[CMD_AMOUNT] = { + // different commands [CMD_PING] = {"ping", pingparser, "echo given command back"}, [CMD_RELAY] = {"relay", relayparser, "change relay state (1/0)"}, [CMD_BUZZER] = {"buzzer", buzzerparser, "change buzzer state (1/0)"}, @@ -271,13 +307,20 @@ const commands cmdlist[CMD_AMOUNT] = { [CMD_TIMEFROMSTART] = {"time", timeparser, "get time from start"}, [CMD_PWM] = {"pwm", pwmparser, "pwm value"}, [CMD_EXT] = {"ext", extparser, "external outputs"}, + // configuration [CMD_SAVECONF] = {"saveconf", saveconfparser, "save current configuration"}, - [CMD_CURPOS] = {"position", curposparser, "set/get position (in steps)"}, + [CMD_ENCSTEPMIN] = {"encstepmin", encstepsminparser, "minimal encoder ticks per step"}, + [CMD_ENCSTEPMAX] = {"encstepmax", encstepsmaxparser, "maximal encoder ticks per step"}, [CMD_MICROSTEPS] = {"microsteps", ustepsparser, "set/get microsteps settings"}, - [CMD_STOPMOTOR] = {"stop", mstopparser, "stop motor now"}, - [CMD_ACCEL] = {"accel", accparser, "set/get accel/decel (usteps/s per 10ms)"}, - [CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (usteps per sec)"}, + [CMD_ACCEL] = {"accel", accparser, "set/get accel/decel (steps/s^2)"}, + [CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (steps per sec)"}, + [CMD_SPEEDLIMIT] = {"speedlimit", spdlimparser, "get limiting speed for current microsteps"}, [CMD_MAXSTEPS] = {"maxsteps", maxstepsparser, "set/get max steps (from zero)"}, [CMD_ENCREV] = {"encrev", encrevparser, "set/get max encoder's pulses per revolution"}, + // motor's commands + [CMD_ABSPOS] = {"abspos", curposparser, "set/get position (in steps)"}, + [CMD_RELPOS] = {"relpos", relstepsparser, "set relative steps, get remaining"}, + [CMD_STOPMOTOR] = {"stop", mstopparser, "stop motor now"}, + [CMD_REINITMOTORS] = {"motreinit", reinitmparser, "re-init motors after configuration changed"}, }; diff --git a/F0-nolib/3steppersLB/commonproto.h b/F0-nolib/3steppersLB/commonproto.h index 67562b1..0f928f8 100644 --- a/F0-nolib/3steppersLB/commonproto.h +++ b/F0-nolib/3steppersLB/commonproto.h @@ -71,13 +71,23 @@ enum{ ,CMD_PWM // PWM value ,CMD_EXT // value on EXTx outputs ,CMD_SAVECONF // save configuration + ,CMD_ENCSTEPMIN // min ticks of encoder per one step + ,CMD_ENCSTEPMAX // max ticks of encoder per one step ,CMD_MICROSTEPS // get/set microsteps - ,CMD_CURPOS // current position (set/get) - ,CMD_STOPMOTOR // stop moving NOW ,CMD_ACCEL // set/get acceleration/deceleration ,CMD_MAXSPEED // set/get maximal speed + ,CMD_SPEEDLIMIT // get limit of speed for current microsteps settings ,CMD_MAXSTEPS // max steps (-max..+max) ,CMD_ENCREV // encoder's pulses per revolution + ,CMD_REINITMOTORS // re-init motors after configuration changing + ,CMD_ABSPOS // current position (set/get) + ,CMD_RELPOS // set relative steps or get steps left + ,CMD_STOPMOTOR // stop moving NOW + //,CMD_STOPDECEL + //,CMD_RELMOVE + //,CMD_ABSMOVE + //,CMD_MOVE // @max speed infty + //,CMD_FINDZERO // should be the last: ,CMD_AMOUNT // amount of common commands }; diff --git a/F0-nolib/3steppersLB/flash.c b/F0-nolib/3steppersLB/flash.c index 9636339..a36bc58 100644 --- a/F0-nolib/3steppersLB/flash.c +++ b/F0-nolib/3steppersLB/flash.c @@ -24,6 +24,7 @@ #include #include // memcpy #include "flash.h" +#include "steppers.h" #include "strfunct.h" extern const uint32_t __varsstart, _BLOCKSIZE; @@ -44,7 +45,10 @@ static uint32_t maxCnum = 1024 / sizeof(user_conf); // can't use blocksize here ,.maxspd = {1000, 1000, 1000} \ ,.maxsteps = {50000, 50000, 50000} \ ,.encrev = {800,800,800} \ + ,.encperstepmin = {3,3,3} \ + ,.encperstepmax = {5,5,5} \ ,.motflags = {DEFMF,DEFMF,DEFMF} \ + ,.ESW_reaction = {ESW_ANYSTOP, ESW_ANYSTOP, ESW_ANYSTOP} \ } static int erase_flash(const void*, const void*); static int write2flash(const void*, const void*, uint32_t); diff --git a/F0-nolib/3steppersLB/flash.h b/F0-nolib/3steppersLB/flash.h index b840c06..065209a 100644 --- a/F0-nolib/3steppersLB/flash.h +++ b/F0-nolib/3steppersLB/flash.h @@ -32,8 +32,8 @@ // limiting values #define MICROSTEPSMAX (512) -// (STEPS per second per 10ms) -#define ACCELMAXSTEPS (100) +// (STEPS per second^2) +#define ACCELMAXSTEPS (1000) // max speed IN STEPS! #define MAXMAXSPD (10000) // max encoder steps per rev @@ -49,8 +49,9 @@ blocksizeASH_SIZE_REG ((uint32_t)0x1FFFF7CC) // motor flags typedef struct{ uint8_t reverse : 1; // reversing motor rotation - uint8_t encreverse : 1; // reversing encoder rotation + uint8_t encreverse : 1; // reversing encoder rotation TODO: configure encoder's timer to downcounting uint8_t haveencoder : 1; // have encoder + uint8_t donthold : 1; // clear power @ stop (don't hold motor when stopped) } motflags_t; /* @@ -61,11 +62,14 @@ typedef struct __attribute__((packed, aligned(4))){ uint16_t CANspeed; // default CAN speed uint16_t CANID; // identifier uint16_t microsteps[MOTORSNO]; // microsteps amount per step - uint16_t accel[MOTORSNO]; // acceleration/deceleration (dv microsteps/s per 10ms) - uint16_t maxspd[MOTORSNO]; // max motor speed (microsteps per second) + uint16_t accel[MOTORSNO]; // acceleration/deceleration (steps/s^2) + uint16_t maxspd[MOTORSNO]; // max motor speed (steps per second) uint32_t maxsteps[MOTORSNO]; // maximal amount of steps uint16_t encrev[MOTORSNO]; // encoders' counts per revolution + uint16_t encperstepmin[MOTORSNO]; // min amount of encoder ticks per one step + uint16_t encperstepmax[MOTORSNO]; // max amount of encoder ticks per one step motflags_t motflags[MOTORSNO]; // motor's flags + uint8_t ESW_reaction[MOTORSNO]; // end-switches reaction (esw_react) } user_conf; extern user_conf the_conf; // global user config (read from FLASH to RAM) diff --git a/F0-nolib/3steppersLB/hardware.c b/F0-nolib/3steppersLB/hardware.c index e842095..b81b2d3 100644 --- a/F0-nolib/3steppersLB/hardware.c +++ b/F0-nolib/3steppersLB/hardware.c @@ -85,8 +85,8 @@ static IRQn_Type motirqs[MOTORSNO] = {TIM15_IRQn, TIM14_IRQn, TIM16_IRQn}; // motor's PWM static void setup_mpwm(int i){ TIM_TypeDef *TIM = mottimers[i]; - TIM->PSC = 999; // 48kHz - TIM->ARR = 1000; // starting ARR value + TIM->CR1 = 0; + TIM->PSC = MOTORTIM_PSC - 1; // 64kHz // PWM mode 1 (OCxM = 110), preload enable TIM->CCMR1 = TIM_CCMR1_OC1M_2; // Force inactive TIM->CCER = TIM_CCER_CC1E; // turn it on, active high @@ -178,6 +178,15 @@ void Jump2Boot(){ // for STM32F072 SysMemBootJump(); } +// calculate MSB position of value `val` +// 0 1 2 3 4 5 6 7 8 9 a b c d e f +static const uint8_t bval[] = {0,0,1,1,2,2,2,2,3,3,3,3,3,3,3,3}; +uint8_t MSB(uint16_t val){ + register uint8_t r = 0; + if(val & 0xff00){r += 8; val >>= 8;} + if(val & 0x00f0){r += 4; val >>= 4;} + return ((uint8_t)r + bval[val]); +} void tim14_isr(){ addmicrostep(0); diff --git a/F0-nolib/3steppersLB/hardware.h b/F0-nolib/3steppersLB/hardware.h index 17aa16d..c40f5fa 100644 --- a/F0-nolib/3steppersLB/hardware.h +++ b/F0-nolib/3steppersLB/hardware.h @@ -22,6 +22,17 @@ #include +// PCLK frequency +#ifndef PCLK +#define PCLK (48000000) +#endif +// motor frequency in mictosteps - 96kHz +#ifndef MOTORFREQ +#define MOTORFREQ (96000) +#endif +// motors' timer PSC +#define MOTORTIM_PSC (PCLK/MOTORFREQ) + // default CAN bus speed in kbaud #define DEFAULT_CAN_SPEED (250) @@ -170,6 +181,12 @@ extern volatile GPIO_TypeDef *DIRports[MOTORSNO]; extern const uint32_t DIRpins[MOTORSNO]; #define MOTOR_CW(x) do{ pin_set(DIRports[x], DIRpins[x]); }while(0) #define MOTOR_CCW(x) do{ pin_clear(DIRports[x], DIRpins[x]); }while(0) +// minimal motor speed - steps per second +#define MOTORMINSPEED (10) +// interval of velocity checking (10ms) +#define MOTCHKINTERVAL (10) +// maximal ticks of encoder per step +#define MAXENCTICKSPERSTEP (100) extern volatile uint32_t Tms; @@ -184,4 +201,6 @@ void timers_setup(); void pause_ms(uint32_t pause); void Jump2Boot(); +uint8_t MSB(uint16_t val); + #endif // __HARDWARE_H__ diff --git a/F0-nolib/3steppersLB/main.c b/F0-nolib/3steppersLB/main.c index 293cfd2..7dddc7e 100644 --- a/F0-nolib/3steppersLB/main.c +++ b/F0-nolib/3steppersLB/main.c @@ -87,11 +87,14 @@ int main(void){ process_keys(); custom_buttons_process(); can_proc(); - usb_proc(); if(CAN_get_status() == CAN_FIFO_OVERRUN){ SEND("CAN bus fifo overrun occured!\n"); sendbuf(); } + IWDG->KR = IWDG_REFRESH; + usb_proc(); + process_steppers(); + IWDG->KR = IWDG_REFRESH; while((can_mesg = CAN_messagebuf_pop())){ if(can_mesg && isgood(can_mesg->ID)){ if(ShowMsgs){ // new data in buff @@ -108,6 +111,7 @@ int main(void){ } } } + IWDG->KR = IWDG_REFRESH; if((txt = get_USB())){ IWDG->KR = IWDG_REFRESH; cmd_parser(txt); diff --git a/F0-nolib/3steppersLB/steppers.bin b/F0-nolib/3steppersLB/steppers.bin index e1b82178e0ea8106351d9d6ad3cfedfd4878dd5e..0f6a3d3524aef05c431aceb15ea9402975120a9e 100755 GIT binary patch delta 6829 zcmbVRdw5e-wqN@sZPWA>+5%}>%4yQpq-jfNS^;^51P)D3U-B|-ft$8eQ!F$BVnH+= zfw`^DD00;41C5Ov+q&}{LT(4i^YszcBs7TcDaqv^pLYi~ePFuxqzHk1x z<@?32QdTj_>@d9Pyr@>D1UVNcqdi(|KM>P z|MX2ZHX*+QvG=C7pY5jqa*|&uL2=K5S6*WX85($<$dCyC&FGltBEnZp0V9hD(`27c zJ4k@lBMK1;L=6DzD;JXG#Re+M^s-@wAA+H^p5?%XgV8o#-7eP?KhbJDp5;TFNobZY zj+u+vvxTGbtxVULkR9_eF$q7$v@ME>=z)mSB8BBdl*YVG~Bt7CzDf z$wc&qi&1h@iJ|g+VI#_}uy8y+U9mcxkHU(u@NRroEI||B+z`HvFI}4-{zot!d$i$? zgK0>!!|w%$xKirQ223v#SYCV)z=Ll625}s525}itgBU?{baViy7>guHW34D>q1y@s z*n|g_55<_!RH+b?;3Wrzy$NMrnT0W?4-du1`4b%J=6d)*UDpY$(&bsMbk}4z>J`K3 z=9uz&*x&!Ax~}7Dkg!*S4Er5AMc2ivqvKO?e5_w>RyyjTx9;cBP5QWDwOL-izrP+d zE566E-{V+uV(iGEH#a#4gnZ3fk#+L8#2K7Kfcw}4vxdZ)ZxCiac8E?`xs2xcR73+^Mrg~TrJzNm|;L79cH zj8QhM1|k|psOAvo)zNmwsKmKtel>u&OH!SRcT=6Dkx_Iu6#Q461rs_TZk5J$RD&Q6 zM#gPML;5MASi3)k-vC(|pW}*_xkgSUxf30K?7u&|uV1jH49~?W_lAkCNM4k<+wuo% ztY0404t4}_@*u9%=h0v!0w5et%FQO?wNZL-;;z#)6GvGvnTYt}X!K}|3RjY>aw5Jx zDikJfDEwk6C`7~_TmiWqGXr_mU?NDNvlDAI#Er`@jtVD}H5r!cKQBrm5LRxK<8n!% zWZ}DHdu%(J87UVQrA*7rxX$SHKG5FA6ISDqLQ_yy`50CTB?@gRWr=`qG$d-Dx6QrV$O^TBPoS>uZ$A?S-;Ye);?x@dzhHhJZ}aiU7BYIvx3%}Zo~n> zu3DnXAn$0pphcb7xuEcS|HF?7y(+C{L5^!FBl;q-yxR4CP~lLPr@0nr8S&nT@Udzp zaSGQ|`3en5aDAZZ10i>cmMjpKPx&pg9Q&vI9{V3m&GJ4f={rGuRw`gYcAf(AofKToZQFt zaEG`*a3gqr-VI4JTg$Po;O*9q6bajjG!bbxQhE-LAU%ONNht@4w~)%0gQ9i0ke_~@ zEE34nOfpxPF*VZwx?b)Fon-FePRy6)cXNy8AK;33$RXN3uCQtqr+9C!CCgR89T3({ z%`s_l`(*~kSI?bU^J#FVgS9`-G1_F<$7MTmZw1{G9AO8z^op$Nvuabx{vE=xsoI69 zl~Y}*j?WhMu9X%RSYqHwOT1&Mi(1XXAN%L6W;;|4ulszHMM z(g~hHe$h?shi__sCdxC8G&i0Qti?mzK|B{EXJqgn+#M^pDsCCq#Rmp8?yhpBP2;L_ zGnP7+#{KJZ#-c&GH%jZ=dyuBEb*}RpAF`cq)ZG0m+TH}(URT}S58LXR9(1T?Pr=a- z2u0Ipk&VK-=@~Jtj#hq?aR2lSZ)I2t&5jVUHgcVfgi<4skR}Xb>u86rabvwa&DPW? zg;qz_^Xseoxb=Jwq8HK4*|_x%jVltGJtl>oz9EGSBazVG#^ieN?+VzsJ1VkVM2wL@ zG~cjsS*Z8sm^{1C=fae&kU)P8H#M^zjNf~GHm(8FGNYc3`T$5~hlgFcjlU zA&=J3BVpG%QU>A=;eYl&WBqZMChS^tv^*RMJ%y&<_uIJJ&<@Y3-9*fgVzQ6L)(EQTP2Sh(w3+$r+DGCReYkbwka{*>Qr*L4*3PSaQ*(eb;Z|kV z)@$sX;&z+Q0O{V@cDP+_@2s^oEQ`Hp05!@h>Q>sCmgSz$$AV2T!P7i)W67|tycJ~i z^$JZ^yit3OS^0QBTh53tU7yKk)~lDy}OPBL<}s{!wa^U=F79ao4R zNfXiqh#JIYT$Qg8cE?qj^r2}N4nBhn>%*HklNs|fPwGmpR&5!I_KQ=oGxI*mq+V4bq8p3 z`7b&(tEJGN$9V2cj=JN>Zy52}Q5)CMug09wxYc|nmh`?InQ_xUobT>!WON5O#{9`r z{5?Og^i!myl)05Dlh65VsaIC``b~vsT5eXSJc{UH5B?>{F_c-`!osB0!7=`BE-SBxsnPP z)U5^MdjD+4?iqd1=X}Jv64@82OgzjdDE4At+)NzV6EUgaLkgjso)@+-frA$fWBq*YK*9nDOs{L)(^(j=YSDtu@155Wh{TD26q2j7p>4$%@`v&yraZkxfe8~je9^@@XuEn4doUw*|c zm6x60ICER_tVzDS%Dk$jaHxJ(1}Tv`_xB%K`8(7BT|$F+D!Z~!J+CykofH=Nn8L68 zSrxf!Na+=SW`%auXL!^Ot<0#H?zrMiU9&&q)0(M{X=^cR{}t!dip$OytQY-|o4!iv zCZ&gLMQWwjWh#4dM{1n>qhv=)#rw|BoI(R=lhDsI!8k|ys=o$lRO3&<@%x=c?`hOD zipkZX$w?!Rt{TSO=kyF_w7e6S_EBoZ40n#&G`nj@SKK=(-^3<;1coFZ0oClpN;@>D zy~mtgon0A6Ye1`M@SFm}RiDZfTWJASebm{}8Lv57a}`ekebm$KpxYqQ`*7lX-*4aL zNN}s%yN_pT;*IOiCAc9s&DGAgSN3r2{En(_ZU?`kx)*=CwiTvvZaM=RF3g(IyF=;67C>oVP{ zCBu!7)8g2%<~itff}!rxUG{pXU}Y|VZUvX-G2=vc_+&WQ{Q-ivgs~k2pm#<c#w*mvP&~S*K^N@~7gX9aJ0Gfxuj^Bj zeUDf-UO>k?7@g5WZ!&8+2t@n>-<_oQgF5vR=o@adVL<}jjWuALd1;~jIiQXnyCBCA zuLkGY>%Ax*zC=C#{e+({`%4|qL60-gzhRyfnic`Q*)KzNv*%#2Xu<$o`fZS!_+5~? z_g`VqgKrPsfs<36z4&Diqc(!?*FmlQ5Ui}3X;hy-sx|uklhIztRXlX5LM5godos z%;gF}JELDirvK?oLq~Ha>3vXy&laGb6BElY>tKeNUC&d9wPP3;H6p{oGMoPUsjC2SQs1%ZO z*WkEE#{_q7YpNR`8uX~`K}z*%>;qSFKV}L{4*c?hpx-3+AbkuTgLIr*k6=+ALOg-E z>=?Lmi;@&mAE_Pc0}XA`4?vwn#6Glt8F3PE#x7(VOk|c&Z7>_(<{`g_n}LTh&8=?# zl@)xi2f!fnxj33;F9%NZryU%takc1P6J9np$PkUu#=Z25zU*H(I$bil!RYgk3icucxgu;Wx<%G>rA2tUXa)%g ze=VvdCxofRGs!ujthg=p1g7I;0E?_CkSHJcQ*knhPm}{YjDcJf{&!Z>P0~EmA7*69 zfL&6GpI;FVASQLGt&0MgJwJxFoEum&JC4N4k(QWuu5c}Q|;^+JadLV3^Q$oozb1YfM!VxU1 z@C^ipP!nBce}fh&uaXa3pL1TO@!-z4BRUX!5KkZuBaYYxzMa34c$a}9J^?=mAY~Cr z$m@|VMU)|5fz*w7G6mnUNOvI~KzZp16dA3%Bnu?hK;NY5bFA@4(a5z&BlE+Z8Y zJCGkinuwnlx{&v(kjX;alZ*}^Ek&$D-hy;3;xDM=M%sxOMg9q-^xH%X>R{xGi-@vB zoB%2PR?&sL3TY|g6XY$??-v+Y60QUC-H2t#A4K}@axf}JQ5Z%+j*p;^kWWOaN4$c( z3F#_C5Ativ2No^bPS!MUZrZx7*|Vu-GXVZ-+0@X=Vq4pW<}K{DmZrP6vL5893c#dp z1HQsGHMIX5U7>$*fskdH5l;thowap+iQus0ERRa#2Ai7thwy;h2ZrRejc^kW_rD;oB)Bp%OEscvdwQbwdj1J>S*wS1?ITqX6 zwrph^?%akxH??iPv*oUPw=`^PY1_;;xf?d$)w}_qp>eCHZL4r)akgzsb1Smw1I70y#njKqgpF2vtio zw1F`ABb)dca4$I{^U~VXP>efUg*%IQG6#1S=_-Ul3koOFRz!ml6x)yvA-W21hml@H z^yo1|kp6&Z$;Db&4GJ~lX&q)A(o)0&XmbHlC!z~^7t#*ILIb2II*~bnSdxaus|Q|N zHdB`9q_?spO~C*b2w#>P=E{Kni;4b|%jJa7*vfwaIm-V6A>Ur2K8)|4#i&c4Cj|fR K0N$XZ74U!myo7B4 delta 4936 zcmbVPeRNY*wm;`4ZPU#y%bXn5e~v1TQ3J6N@z{K#I|4il9L&h`60VSIKQ5omxoYGN{~sJj z|JSdJu@~|aT)(dEr?}ZS4zf#6bl)|cd_$mgc;vsR2E@(^#X`>%`$DJ?&PG^{=2Fg1 zN<=N713o20TngBlSemw`oU0m(W?YcR$k;kh3lXD%c$;kO(^{yXnoS;0%b1kHdbCRt zvJoJa9n)?T!~74y=?#fL%m3q%@L}sjj-nucjSyMBDzW73a}CA?;*DR3)$SHOG~~E7w;fu zQfcrVlTJu8Ng1*BQDt64M1*HyqYZT4Z|&cHC`oM+kNRYoDi4N>c93$>XCBC6t8_<9 zsYo2pOgyoW=(53lBCleY_C}VOJU8>Uu;`}BrZDfGf$l$q*saO+W%I&T%%U!Qw3nn& zr8!&!t%_>`*B6E>p_hc&iR5fuZnz9ePMDof&QClWqRPn-`$uy9x-UW>2eM)LEcAXL z3w$YbAuuKxcsd(VURGjz@kK(uMOp@=Qx7Z$+`zkN+X?RP?hK|$MLwCxA> zEMgDncPFGElz}D8@Y0>^p&1QcGRL&kBOHTB6lAGiqRq1GI`_=BJ*T>7 zJ$C9vd;YjuDrl>P%oK|~%dd!{eWtH8`O1RS75Ch3%cZ4bryjCBVNdoaIl9Q3#)yBrZ}~NG7w`&Q*L2tsSn$>Ab_{nfTXD?RBFbJgeYrbn9}Jn>1w-BsYf)8 zpIdY>!|7x$EOO(%I z?9;T3Ew;r(=Kx$7cwG}J0J32;g{Z-@i0HaH@KrtWM_og9nhm2=G}$%&2dqY$8XRVi zrr$(2u@}-?=dVVjc0^ht8%9m;C33ENpFPv35lt>Ncuz#ISbW5Mi;UhLHFy_vgHI6E zpqtG$G^Dga|J46!Kn*$~%x$oUbrEVw_7^y_`bgy!A6=N``8<$||IaX6HCPg1Ck$0W zYJ`1kFeUTXc{|X@XhxIaHuAPHnf4k}2CP-z`1OylpJ$j&)*{zZLHWm2qHJ{iHK23q z9a%1`Sy0#%dnBWfwzF3<%5S`v+i70;tYojv! ze-m}eMmJF|MO&dg8f}ehe~CJv4MbbyLCH8klqaJS^!{kG)G^>C%7JJz)L%gzgmG`Q zNtUD`$tJf*hw%TDR7Lcnwf@snL@AASNJQBf|K5OBze(MUfd# zC_UN^-xYD+O&lol@>mTF{b9+Ik8td?{a z7TAWnmrD~YH>b#2jGEDu3%)Mt+^v@aqO+*=pd^^n$N{O)Su&3lKQ2*mM9N-Xu<{+F zW$t5rY*UUoE30j$E6e#wT-VgIFfWLJ_@m-Sz$nRX&9=fjmo471Ev)r^U%f*i@e(KSp zpXzqEJc;*nw}Y=jr(3Xfx=e1bL$H~^55&3CeZNB}v!#oju8W&?H(%^FZQj8R8;JRk zt8??d=FScGISsWL=zcHDFwUVHSd%d~p~u-H-^uPU=6W|o)nH4ED!-nJh_T?FSS+|d zN|f`V{^IUUUD~YX4c%&Rb8M}=cIBY77S+2}J_HO&UD8^o$rTI!=MQRdWJ(PV;Q(oh z#)9)>TFc$OSgGM`L_^5JuirtwztNWP=S*pNztE44nlWQ;_OJzT5C30S$zVZ=YYNf_sBzHj` zKz4b~Sq_tnD%GJ~0sdM%fkBsq_XUjdpq-50hX`~5imWIHC*!uMtpRo=x5|stRpeKI zJt&U>UM|-TN&Y+!poIFt9`NHpXtYJTAZ0lFM-NH{Cn-LFe_HyuL`ul)v=!;C^n&1i zu~~3ArT<-Rmt4|&s}B)cm(stdU8vPme&tt#ulb21s`4PnMxhI@dLkcWI)Q9?H}bZQ z=Jvjwl>J`j@&b2}(Nep&Z*S7u>0c(My+_K^e3YcN8ETJqw30p2dgI=K^-GVn6SHZ( z^)xA0dn zbX!Tjhbp@wTDiuR@9|)d*pGy@yfZsnR7@%Ryl74Wwi9tbD=;@^nSbCuKh{t)(NT7n zc{43!ADGVwRm<3+;!`F*#I#HPGNStv%I^x=w97{BEDp#HTb*Gu>4!>E6=bOtg|%-<%7&`t()BYb>PqH&}^SpcGUAm-~Z?HX}@I@%%x+`e6fFB2r0xts=(i5MktP*o|$UjeB zhG*QclILkxo29B7Qn{JkUzb;18BeqMU(ytBNHbThbdyS-e~O*0E2o#)S9P;BJ)do6 zx%CC~5}RLt8$G}d))&$>>}37c%mXOW=?N^hr;`EA$lQiBnmnK(;%ZE!gSF1vaGf?f z@1=r4w4bdex}q9P7MS+su{zlO^A{%YmNg@%=O@v`eu#$!_RfL}Igjxd0yht`j@U#i z*hOqGy>jyo;jl1Lamxj|f`@+e3?W{xs`8#^AP=MMsw)v~XnP!C_y81_oaGLp(|}!I z5AZzT1GWGqjuC0mELwCJ{kgvy#7Es%SL5EB%H;&wr=10OY!cl_YwA?G^^>JZQFZ1+vaQ&Jw3k^Ypr)}D<^lY@7w0->Dd_X z(zAWezx8Yv(I&S8_L;ww(kUw}BI0^YM_*LNZ5*$uoT?^4=K+Xo207YgzXYeB6>jEq|*aehAc7t~U z35c@?d>F`pd=z{RIBO>9Iv<26us0LI9V7R)6>3uO^@sXYXChH6d$zqi8>CNT -static int32_t stepsleft[MOTORSNO]; // steps left to stop (full steps or encoder's counters) +typedef enum{ + STALL_NO, // moving OK + STALL_ONCE, // Nstalled < limit + STALL_STOP // Nstalled >= limit +} t_stalled; -static uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position -static uint8_t stopflag[MOTORSNO]; // ==1 to stop @ nearest step +// motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) +static int8_t motdir[MOTORSNO]; +// 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) +static int32_t prevstppos[MOTORSNO]; +// target stepper position +static int32_t targstppos[MOTORSNO]; +// position to start deceleration +static int32_t decelstartpos[MOTORSNO]; +// current encoder position (4 per ticks) (without TIM->CNT) +static volatile int32_t encpos[MOTORSNO] = {0}; +// previous encoder position +static int32_t prevencpos[MOTORSNO]; +// encoders' ticks per step (calculates @ init) +static int32_t encperstep[MOTORSNO]; -void init_steppers(){ - // init variables - for(int i = 0; i < MOTORSNO; ++i){ - mottimers[i]->CR1 &= ~TIM_CR1_CEN; - stepsleft[i] = 0; - microsteps[i] = 0; - stopflag[i] = 0; - motdir[i] = 1; - stppos[i] = 0; - } - timers_setup(); +// current speed +static uint16_t curspeed[MOTORSNO]; +// delta V according to current acceleration & INTERVAL +static uint16_t dV[MOTORSNO]; +// ==1 to stop @ nearest step +static uint8_t stopflag[MOTORSNO]; +// motor state +static stp_state state[MOTORSNO]; +// move to zero state +static mvto0state mvzerostate[MOTORSNO]; + +// lowest ARR value (highest speed), highest (lowest speed) +//static uint16_t stphighARR[MOTORSNO]; +// microsteps=1<ARR = (MOTORFREQ / curspeed[i]) >> ustepsshift[i]; } -// get position -errcodes getpos(int i, int32_t *position){ +// run this function after each steppers parameters changing +void init_steppers(){ + timers_setup(); // reinit timers & stop them + // init variables + for(int i = 0; i < MOTORSNO; ++i){ + stopflag[i] = 0; + motdir[i] = 1; + curspeed[i] = 0; + dV[i] = the_conf.accel[i] * MOTCHKINTERVAL; + dV[i] /= 1000; // interval in ms, but accel in steps/s^2 + if(dV[i] == 0) dV[i] = 1; + state[i] = STP_RELAX; + ustepsshift[i] = MSB(the_conf.microsteps[i]); + encperstep[i] = the_conf.encrev[i] / 200; + if(!the_conf.motflags[i].donthold) MOTOR_EN(i); + else MOTOR_DIS(i); + } +} + +// get absolute position by encoder +static int32_t encoder_position(uint8_t i){ + int32_t pos = encpos[i]*the_conf.encrev[i]; + if(the_conf.motflags[i].encreverse) pos -= mottimers[i]->CNT; + else pos += mottimers[i]->CNT; + return pos; +} + +// get current position +errcodes getpos(uint8_t i, int32_t *position){ if(the_conf.motflags[i].haveencoder){ - *position = encpos[i] + enctimers[i]->CNT; + *position = encoder_position(i) / encperstep[i]; }else *position = stppos[i]; return ERR_OK; } -// set position -errcodes setpos(int i, int32_t newpos){ - if(newpos < -(int32_t)the_conf.maxsteps[i] || newpos > (int32_t)the_conf.maxsteps[i]) - return ERR_BADPAR; - int32_t diff = newpos - stppos[i]; - if(diff == 0) return ERR_OK; - if(diff > 0) motdir[i] = 1; - else{ - diff = -diff; - motdir[i] = -1; - } - // TODO: set direction pin, run timer, set target position - // depends on the_conf.motflags[i].reverse + +errcodes getremainsteps(uint8_t i, int32_t *position){ + *position = targstppos[i] - stppos[i]; return ERR_OK; } -void stopmotor(int i){ +// move to absolute position +errcodes motor_absmove(uint8_t i, int32_t newpos){ + //if(i >= MOTORSNO) return ERR_BADPAR; // bad motor number + if(state[i] != STP_RELAX) return ERR_CANTRUN; // can't move: motor isn't stopping + if(newpos > (int32_t)the_conf.maxsteps[i] || newpos < -(int32_t)the_conf.maxsteps[i] || newpos == stppos[i]) + return ERR_BADVAL; // too big position or zero + targstppos[i] = newpos; + prevencpos[i] = encpos[i]; + prevstppos[i] = stppos[i]; + uint8_t inv = the_conf.motflags[i].reverse; + int32_t delta = newpos - stppos[i]; + if(delta > 0){ // positive direction + if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid + decelstartpos[i] = targstppos[i] - accdecsteps[i]; + }else{ // triangle speed profile + decelstartpos[i] = stppos[i] + delta/2; + } + motdir[i] = 1; + if(inv) MOTOR_CCW(i); + else MOTOR_CW(i); + }else{ // negative direction + delta = -delta; + if(delta > 2*(int32_t)accdecsteps[i]){ // can move by trapezoid + decelstartpos[i] = targstppos[i] + accdecsteps[i]; + }else{ // triangle speed profile + decelstartpos[i] = stppos[i] - delta/2; + } + motdir[i] = -1; + if(inv) MOTOR_CW(i); + else MOTOR_CCW(i); + } + curspeed[i] = MOTORMINSPEED; + recalcARR(i); + MOTOR_EN(i); + mottimers[i]->CR1 |= TIM_CR1_CEN; // start timer + state[i] = STP_ACCEL; + return ERR_OK; +} + +// move i'th motor for relsteps +errcodes motor_relmove(uint8_t i, int32_t relsteps){ + return motor_absmove(i, stppos[i] + relsteps); +} + +void stopmotor(uint8_t i){ stopflag[i] = 1; - stepsleft[i] = 0; - microsteps[i] = 0; +} + +stp_state getmotstate(uint8_t i){ + return state[i]; } // count steps @tim 14/15/16 -void addmicrostep(int i){ +void addmicrostep(uint8_t i){ + static volatile uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position if(mottimers[i]->SR & TIM_SR_UIF){ + if(ESW_state(i)){ // ESW active + switch(the_conf.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(++microsteps[i] == the_conf.microsteps[i]){ microsteps[i] = 0; - if(stopflag[i]){ // stop NOW + stppos[i] += motdir[i]; + if(stopflag[i] || stppos[i] == targstppos[i]){ // stop NOW stopflag[i] = 0; mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer + if(the_conf.motflags[i].donthold) + MOTOR_DIS(i); // turn off power + state[i] = STP_RELAX; } - stppos[i] += motdir[i]; } } mottimers[i]->SR = 0; } -void encoders_UPD(int i){ +void encoders_UPD(uint8_t i){ if(enctimers[i]->SR & TIM_SR_UIF){ int8_t d = 1; // positive (-1 - negative) if(enctimers[i]->CR1 & TIM_CR1_DIR) d = -d; // negative @@ -96,3 +207,138 @@ void encoders_UPD(int i){ } enctimers[i]->SR = 0; } + +// check if motor is stalled +// @return 0 if moving OK, +static t_stalled chkSTALL(uint8_t i){ + if(!the_conf.motflags[i].haveencoder) return STALL_NO; + static uint8_t Nstalled = 0; // counter of STALL + int32_t curencpos = encoder_position(i), Denc = curencpos - prevencpos[i]; + int32_t curstppos = stppos[i], Dstp = curstppos - prevstppos[i]; + prevencpos[i] = curencpos; + if(Dstp == 0){ // veird things + stopmotor(i); + return STALL_STOP; + } + if(Denc < the_conf.encperstepmin[i]*Dstp || the_conf.encperstepmax[i]*Dstp < Denc){ // stall? + if(++Nstalled >= NSTALLEDMAX){ + stopflag[i] = 1; + Nstalled = 0; + return STALL_STOP; + }else{ + uint16_t spd = curspeed[i] >> 1; // speed / 2 + curspeed[i] = (spd > MOTORMINSPEED) ? spd : MOTORMINSPEED; + recalcARR(i); + if(state[i] == STP_MOVE) + state[i] = STP_ACCEL; + return STALL_ONCE; + } + }else Nstalled = 0; + curstppos = curencpos / encperstep[i]; // recalculate current position + stppos[i] = curstppos; + prevstppos[i] = curstppos; + return STALL_NO; +} + +// check state of i`th stepper +static void chkstepper(int i){ + int32_t newspeed; + switch(state[i]){ + case STP_ACCEL: // acceleration to max speed + if(STALL_NO == chkSTALL(i)){ + newspeed = curspeed[i] + dV[i]; + if(newspeed >= the_conf.maxspd[i]){ // max speed reached -> move with it + curspeed[i] = the_conf.maxspd[i]; + state[i] = STP_MOVE; + }else{ // increase speed + curspeed[i] = newspeed; + } + recalcARR(i); + } + // check position for triangle profile + if(motdir[i] > 0){ + if(stppos[i] >= decelstartpos[i]) // reached end of acceleration + state[i] = STP_DECEL; + }else{ + if(stppos[i] <= decelstartpos[i]) + state[i] = STP_DECEL; + } + break; + case STP_MOVE: // move @ constant speed until need to decelerate + if(STALL_NO == chkSTALL(i)){ + // check position + if(motdir[i] > 0){ + if(stppos[i] >= decelstartpos[i]) // reached start of deceleration + state[i] = STP_DECEL; + }else{ + if(stppos[i] <= decelstartpos[i]) + state[i] = STP_DECEL; + } + } + break; + case STP_DECEL: + if(STALL_NO == chkSTALL(i)){ + newspeed = curspeed[i] - dV[i]; + if(newspeed > MOTORMINSPEED){ + curspeed[i] = newspeed; + }else{ + curspeed[i] = MOTORMINSPEED; + state[i] = STP_MVSLOW; + } + recalcARR(i); + } + // fallthrough + case STP_MVSLOW: + if(motdir[i] > 0){ + if(stppos[i] >= targstppos[i]) // reached start of deceleration + stopflag[i] = 0; + }else{ + if(stppos[i] <= targstppos[i]) + stopflag[i] = 0; + } + break; + default: // RELAX, STALL, ERR -> do nothing + return; + } + switch(mvzerostate[i]){ + case M0FAST: + if(state[i] == STP_RELAX){ // stopped -> move to + + if(ERR_OK != motor_relmove(i, 50)){ + state[i] = STP_ERR; + mvzerostate[i] = M0RELAX; + }else + mvzerostate[i] = M0PLUS; + } + break; + case M0PLUS: + if(state[i] == STP_RELAX){ // stopped -> move + if(ERR_OK != motor_relmove(i, -100)){ + state[i] = STP_ERR; + mvzerostate[i] = M0RELAX; + }else{ + state[i] = STP_MVSLOW; + mvzerostate[i] = M0SLOW; + } + } + break; + case M0SLOW: + if(state[i] == STP_RELAX){ + encpos[i] = 0; + stppos[i] = 0; + mottimers[i]->CNT = 0; // set encoder counter to zero + mvzerostate[i] = M0RELAX; + } + break; + default: // RELAX: do nothing + break; + } +} + +void process_steppers(){ + static uint32_t Tlast = 0; + if(Tms - Tlast < MOTCHKINTERVAL) return; // hit every 10ms + Tlast = Tms; + for(int i = 0; i < MOTORSNO; ++i){ + chkstepper(i); + } +} diff --git a/F0-nolib/3steppersLB/steppers.h b/F0-nolib/3steppersLB/steppers.h index 3612709..a155d89 100644 --- a/F0-nolib/3steppersLB/steppers.h +++ b/F0-nolib/3steppersLB/steppers.h @@ -23,15 +23,40 @@ #include #include "commonproto.h" -// direction -extern int8_t motdir[]; +// amount of tries to detect motor stall +#define NSTALLEDMAX (5) -void addmicrostep(int i); -void encoders_UPD(int i); +// 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_state; + +// end-switches reaction +typedef enum{ + ESW_IGNORE, // don't stop @ end-switch + ESW_ANYSTOP, // stop @ esw in any moving direction + ESW_STOPMINUS // stop only in negative moving +} esw_react; + +// find zero stages: fast -> 0, slow -> +, slow -> 0 + +void addmicrostep(uint8_t i); +void encoders_UPD(uint8_t i); void init_steppers(); -errcodes getpos(int i, int32_t *position); -errcodes setpos(int i, int32_t newpos); -void stopmotor(int i); +errcodes getpos(uint8_t i, int32_t *position); +errcodes getremainsteps(uint8_t i, int32_t *position); +errcodes motor_absmove(uint8_t i, int32_t abssteps); +errcodes motor_relmove(uint8_t i, int32_t relsteps); + +void stopmotor(uint8_t i); +stp_state getmotstate(uint8_t i); +void process_steppers(); #endif // STEPPERS_H__