From 263d805bf69be196d0e1c2a0eefdc1d03ccb9a4d Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Mon, 25 Oct 2021 23:52:47 +0300 Subject: [PATCH] add some more commands --- F0-nolib/3steppersLB/commonproto.c | 109 +++++++++++++++++++++++++---- F0-nolib/3steppersLB/commonproto.h | 7 ++ F0-nolib/3steppersLB/flash.c | 9 ++- F0-nolib/3steppersLB/flash.h | 19 +++-- F0-nolib/3steppersLB/hardware.c | 87 +++++++++++++++-------- F0-nolib/3steppersLB/hardware.h | 5 ++ F0-nolib/3steppersLB/main.c | 5 +- F0-nolib/3steppersLB/steppers.bin | Bin 17380 -> 18800 bytes F0-nolib/3steppersLB/steppers.c | 77 +++++++++++++++++++- F0-nolib/3steppersLB/steppers.h | 12 +++- 10 files changed, 276 insertions(+), 54 deletions(-) diff --git a/F0-nolib/3steppersLB/commonproto.c b/F0-nolib/3steppersLB/commonproto.c index 88ada4f..950d87b 100644 --- a/F0-nolib/3steppersLB/commonproto.c +++ b/F0-nolib/3steppersLB/commonproto.c @@ -22,6 +22,7 @@ #include "commonproto.h" #include "flash.h" #include "hardware.h" +#include "steppers.h" #ifdef EBUG #include "strfunct.h" #endif @@ -158,26 +159,103 @@ static errcodes extparser(uint8_t par, int32_t *val){ return ERR_OK; } +/******************* 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!" +#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/the_conf.microsteps[n] > ACCELMAXSTEPS || *val < 1) return ERR_BADVAL; + the_conf.accel[n] = *val; + } + *val = the_conf.accel[n]; + return ERR_OK; +} + +static errcodes maxspdparser(uint8_t par, int32_t *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + if(ISSETTER(par)){ + if(*val/the_conf.microsteps[n] > MAXMAXSPD || *val < 1) return ERR_BADVAL; + the_conf.maxspd[n] = *val; + } + *val = the_conf.maxspd[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; + if(ISSETTER(par)){ + if(*val < 1) return ERR_BADVAL; + the_conf.maxsteps[n] = *val; + } + *val = the_conf.maxsteps[n]; + return ERR_OK; +} + +static errcodes encrevparser(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 > MAXENCREV) return ERR_BADVAL; + the_conf.encrev[n] = *val; + enctimers[n]->ARR = *val; + } + *val = the_conf.encrev[n]; + return ERR_OK; +} + static errcodes saveconfparser(uint8_t _U_ par, int32_t _U_ *val){ if(store_userconf()) return ERR_CANTRUN; return ERR_OK; } +/******************* END of config parsers *******************/ -#if 0 -typedef struct __attribute__((packed, aligned(4))){ - uint16_t microsteps; // microsteps amount per step - uint16_t accdecsteps; // amount of steps need for full acceleration/deceleration cycle - uint16_t motspd; // max motor speed (steps per second) - uint32_t maxsteps; // maximal amount of steps from ESW0 to EWS3 - defflags_t defflags; // default flags -} user_conf; -#endif +/******************* START of motors' parsers *******************/ +static errcodes mstopparser(uint8_t par, int32_t _U_ *val){ + uint8_t n = PARBASE(par); + if(n > MOTORSNO-1) return ERR_BADPAR; + 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; + if(ISSETTER(par)) return setpos(n, *val); + return getpos(n, val); +} +/******************* END of motors' parsers *******************/ /* -static CAN_errcodes parser(const uint8_t _U_ par, const int32_t _U_ *val){ - return CANERR_OK; -}*/ +static errcodes parser(uint8_t _U_ par, int32_t _U_ *val){ + return ERR_OK; +} +*/ // the main commands list, index is CAN command code const commands cmdlist[CMD_AMOUNT] = { @@ -194,5 +272,12 @@ const commands cmdlist[CMD_AMOUNT] = { [CMD_PWM] = {"pwm", pwmparser, "pwm value"}, [CMD_EXT] = {"ext", extparser, "external outputs"}, [CMD_SAVECONF] = {"saveconf", saveconfparser, "save current configuration"}, + [CMD_CURPOS] = {"position", curposparser, "set/get position (in steps)"}, + [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_MAXSTEPS] = {"maxsteps", maxstepsparser, "set/get max steps (from zero)"}, + [CMD_ENCREV] = {"encrev", encrevparser, "set/get max encoder's pulses per revolution"}, }; diff --git a/F0-nolib/3steppersLB/commonproto.h b/F0-nolib/3steppersLB/commonproto.h index 385ae5b..67562b1 100644 --- a/F0-nolib/3steppersLB/commonproto.h +++ b/F0-nolib/3steppersLB/commonproto.h @@ -71,6 +71,13 @@ enum{ ,CMD_PWM // PWM value ,CMD_EXT // value on EXTx outputs ,CMD_SAVECONF // save configuration + ,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_MAXSTEPS // max steps (-max..+max) + ,CMD_ENCREV // encoder's pulses per revolution // 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 b7a091c..9636339 100644 --- a/F0-nolib/3steppersLB/flash.c +++ b/F0-nolib/3steppersLB/flash.c @@ -33,15 +33,18 @@ static const uint32_t blocksize = (uint32_t)&_BLOCKSIZE; // max amount of Config records stored (will be recalculate in flashstorage_init() static uint32_t maxCnum = 1024 / sizeof(user_conf); // can't use blocksize here +#define DEFMF {.haveencoder = 1} + #define USERCONF_INITIALIZER { \ .userconf_sz = sizeof(user_conf) \ ,.CANspeed = 100 \ ,.CANID = 0xaa \ ,.microsteps = {32, 32, 32} \ - ,.accdecsteps = {25, 25, 25} \ + ,.accel = {160, 160, 160} \ ,.maxspd = {1000, 1000, 1000} \ ,.maxsteps = {50000, 50000, 50000} \ - ,.motflags = {{0},{0},{0}} \ + ,.encrev = {800,800,800} \ + ,.motflags = {DEFMF,DEFMF,DEFMF} \ } static int erase_flash(const void*, const void*); static int write2flash(const void*, const void*, uint32_t); @@ -214,7 +217,7 @@ void dump_userconf(_U_ char *txt){ PROPNAME("microsteps"); printu(the_conf.microsteps[i]); PROPNAME("accdecsteps"); - printu(the_conf.accdecsteps[i]); + printu(the_conf.accel[i]); PROPNAME("maxspeed"); printu(the_conf.maxspd[i]); PROPNAME("maxsteps"); diff --git a/F0-nolib/3steppersLB/flash.h b/F0-nolib/3steppersLB/flash.h index bdc613d..b840c06 100644 --- a/F0-nolib/3steppersLB/flash.h +++ b/F0-nolib/3steppersLB/flash.h @@ -30,6 +30,14 @@ #define _U_ __attribute__((unused)) #endif +// limiting values +#define MICROSTEPSMAX (512) +// (STEPS per second per 10ms) +#define ACCELMAXSTEPS (100) +// max speed IN STEPS! +#define MAXMAXSPD (10000) +// max encoder steps per rev +#define MAXENCREV (100000) // register with flash size (in blocks) #ifndef FLASH_SIZE_REG @@ -40,7 +48,9 @@ blocksizeASH_SIZE_REG ((uint32_t)0x1FFFF7CC) // motor flags typedef struct{ - uint8_t reverse : 1; + uint8_t reverse : 1; // reversing motor rotation + uint8_t encreverse : 1; // reversing encoder rotation + uint8_t haveencoder : 1; // have encoder } motflags_t; /* @@ -51,9 +61,10 @@ 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 accdecsteps[MOTORSNO]; // amount of steps need for full acceleration/deceleration cycle - uint16_t maxspd[MOTORSNO]; // max motor speed (steps per second) - uint32_t maxsteps[MOTORSNO]; // maximal amount of steps from ESW0 to EWS3 + uint16_t accel[MOTORSNO]; // acceleration/deceleration (dv microsteps/s per 10ms) + uint16_t maxspd[MOTORSNO]; // max motor speed (microsteps per second) + uint32_t maxsteps[MOTORSNO]; // maximal amount of steps + uint16_t encrev[MOTORSNO]; // encoders' counts per revolution motflags_t motflags[MOTORSNO]; // motor's flags } user_conf; diff --git a/F0-nolib/3steppersLB/hardware.c b/F0-nolib/3steppersLB/hardware.c index 031e51e..e842095 100644 --- a/F0-nolib/3steppersLB/hardware.c +++ b/F0-nolib/3steppersLB/hardware.c @@ -35,6 +35,11 @@ const uint32_t ENpins[MOTORSNO] = {1<<0, 1<<2, 1<<11}; volatile GPIO_TypeDef *DIRports[MOTORSNO] = {GPIOB, GPIOB, GPIOB}; const uint32_t DIRpins[MOTORSNO] = {1<<1, 1<<10, 1<<12}; +// timers for motors +TIM_TypeDef *mottimers[MOTORSNO] = {TIM15, TIM14, TIM16}; +// timers for encoders +TIM_TypeDef *enctimers[MOTORSNO] = {TIM1, TIM2, TIM3}; + void gpio_setup(void){ RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIOFEN; GPIOA->MODER = GPIO_MODER_MODER0_AF | GPIO_MODER_MODER1_AF | GPIO_MODER_MODER2_AF | GPIO_MODER_MODER3_AI | @@ -76,8 +81,10 @@ void iwdg_setup(){ IWDG->KR = IWDG_REFRESH; /* (6) */ } +static IRQn_Type motirqs[MOTORSNO] = {TIM15_IRQn, TIM14_IRQn, TIM16_IRQn}; // motor's PWM -static void setup_mpwm(TIM_TypeDef *TIM){ +static void setup_mpwm(int i){ + TIM_TypeDef *TIM = mottimers[i]; TIM->PSC = 999; // 48kHz TIM->ARR = 1000; // starting ARR value // PWM mode 1 (OCxM = 110), preload enable @@ -85,41 +92,53 @@ static void setup_mpwm(TIM_TypeDef *TIM){ TIM->CCER = TIM_CCER_CC1E; // turn it on, active high TIM->CCR1 = 1; // 20.8us for pulse duration, according to datasheet 1.9us is enough TIM->BDTR |= TIM_BDTR_MOE; // enable main output - TIM->CR1 |= TIM_CR1_CEN; // enable timer +// TIM->CR1 |= TIM_CR1_CEN; // enable timer TIM->EGR |= TIM_EGR_UG; // force update generation TIM->DIER = TIM_DIER_CC1IE; // allow CC interrupt (we should count steps) + NVIC_EnableIRQ(motirqs[i]); +} + +static IRQn_Type encirqs[MOTORSNO] = {TIM1_BRK_UP_TRG_COM_IRQn, TIM2_IRQn, TIM3_IRQn}; +static void setup_enc(int i){ + TIM_TypeDef *TIM = enctimers[i]; + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + /* (1) Configure TI1FP1 on TI1 (CC1S = 01) + configure TI1FP2 on TI2 (CC2S = 01) + filters sampling = fDTS/8, N=6 */ + /* (2) Configure TI1FP1 and TI1FP2 non inverted (CC1P = CC2P = 0, reset value) */ + /* (3) Configure both inputs are active on both rising and falling edges + (SMS = 011), set external trigger filter to f_DTS/8, N=6 (ETF=1000) */ + /* (4) Enable the counter by writing CEN=1 in the TIMx_CR1 register, + set tDTS=4*tCK_INT. */ + TIM->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0 | TIM_CCMR1_IC1F_3 | TIM_CCMR1_IC2F_3; /* (1)*/ + //TIMx->CCER &= (uint16_t)(~(TIM_CCER_CC21 | TIM_CCER_CC2P); /* (2) */ + TIM->SMCR = TIM_SMCR_ETF_3 | TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; /* (3) */ + // enable update interrupt + TIM->DIER = TIM_DIER_UIE; + // generate interrupt each revolution + TIM->ARR = the_conf.encrev[i]; + // enable timer + TIM->CR1 = TIM_CR1_CKD_1 | TIM_CR1_CEN; /* (4) */ + NVIC_EnableIRQ(encirqs[i]); } -/* -static void setup_enc(TIM_TypeDef *TIM, uint16_t arrval){ - TIM->PSC = 9; // F=48/10 = 4.8MHz - TIM->ARR = arrval; // PWM frequency = 4800/256 = 18.75kHz - // PWM mode 1 (OCxM = 110), preload enable - TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE | - TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE; - TIM->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE; - TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E; // active high (CC1P=0), enable outputs - TIM->BDTR |= TIM_BDTR_MOE; // enable main output - TIM->CR1 |= TIM_CR1_CEN; // enable timer - TIM->EGR |= TIM_EGR_UG; // force update generation -}*/ // timers 14,15,16,17 - PWM@ch1, 1,2,3 - encoders @ ch1/2 void timers_setup(){ RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM14EN; RCC->APB2ENR |= RCC_APB2ENR_TIM1EN | RCC_APB2ENR_TIM15EN | RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN; // enable clocking - setup_mpwm(TIM14); - setup_mpwm(TIM15); - setup_mpwm(TIM16); - // setup PWM @ TIM17 + // setup PWM @ TIM17 - single PWM channel TIM17->PSC = 5; // 8MHz for 31kHz PWM TIM17->ARR = 254; // ARR for 8-bit PWM TIM17->BDTR |= TIM_BDTR_MOE; // enable main output TIM17->CCER = TIM_CCER_CC1E; // enable PWM output TIM17->CR1 |= TIM_CR1_CEN; // enable timer - ; - NVIC_EnableIRQ(TIM14_IRQn); - NVIC_EnableIRQ(TIM15_IRQn); - NVIC_EnableIRQ(TIM16_IRQn); + for(int i = 0; i < MOTORSNO; ++i) + setup_mpwm(i); + for(int i = 0; i < MOTORSNO; ++i){ + if(the_conf.motflags[i].haveencoder){ // motor have the encoder + setup_enc(i); + } + } } // pause in milliseconds for some purposes @@ -160,17 +179,23 @@ void Jump2Boot(){ // for STM32F072 } -// count steps @tim 14/15/16 -static void stp_isr(TIM_TypeDef *TIM, int i){ - ++steps[i]; // count steps - TIM->SR = 0; -} void tim14_isr(){ - stp_isr(TIM14, 0); + addmicrostep(0); } void tim15_isr(){ - stp_isr(TIM15, 1); + addmicrostep(1); } void tim16_isr(){ - stp_isr(TIM16, 2); + addmicrostep(2); +} + + +void tim1_isr(){ + encoders_UPD(0); +} +void tim2_isr(){ + encoders_UPD(1); +} +void tim3_isr(){ + encoders_UPD(2); } diff --git a/F0-nolib/3steppersLB/hardware.h b/F0-nolib/3steppersLB/hardware.h index 7898f34..17aa16d 100644 --- a/F0-nolib/3steppersLB/hardware.h +++ b/F0-nolib/3steppersLB/hardware.h @@ -173,6 +173,11 @@ extern const uint32_t DIRpins[MOTORSNO]; extern volatile uint32_t Tms; +// timers of motors +extern TIM_TypeDef *mottimers[]; +// timers for encoders +extern TIM_TypeDef *enctimers[]; + void gpio_setup(); void iwdg_setup(); void timers_setup(); diff --git a/F0-nolib/3steppersLB/main.c b/F0-nolib/3steppersLB/main.c index 194300a..293cfd2 100644 --- a/F0-nolib/3steppersLB/main.c +++ b/F0-nolib/3steppersLB/main.c @@ -22,6 +22,7 @@ #include "custom_buttons.h" #include "flash.h" #include "hardware.h" +#include "steppers.h" #include "strfunct.h" #include "usb.h" #include "usb_lib.h" @@ -72,12 +73,12 @@ int main(void){ char *txt; sysreset(); SysTick_Config(6000, 1); - flashstorage_init(); + flashstorage_init(); // should be called before any other functions gpio_setup(); USB_setup(); CAN_setup(DEFAULT_CAN_SPEED); adc_setup(); - timers_setup(); + init_steppers(); RCC->CSR |= RCC_CSR_RMVF; // remove reset flags iwdg_setup(); diff --git a/F0-nolib/3steppersLB/steppers.bin b/F0-nolib/3steppersLB/steppers.bin index 79b45e5c8dbed7cd628ca2c18a86a26786a1c4a9..e1b82178e0ea8106351d9d6ad3cfedfd4878dd5e 100755 GIT binary patch delta 5861 zcmbVQdvsG}w%^}L+NS9%(1K}b>1oqK`Xo?_C?KIJrzNLA70^O)Olj++3N(x27#Wr+ zir(TdSMcDqwjBq(GMDEpq}HV~gI1As(Z$uoQKx8eMPvvn>hVP$4axnT&?4M5f8AvL zvLE06_P4)%_V+kB|8Gowis{Hg{DGTEvk!P4*gav2n-qb@J_>(w^3_Jg;Qxao=>OVv zK6ZeAT9L2Q{o*(4#zD4eN%M2nORsZ`^$)zsR3Oqx&d9ZmN?&m$+{v&sS#@smRz|!U z&;aL&_!_{nNJ>{%Oj1OZNj1!gLo~G3SxbDxe_py>oyny#>0b41iCO5A#-xMlO`I5( zrY3x>G+a*TSP(W5lRpWU)iL=*xK3cQZnOma@i3KZJyd8~J{Rq;gw1!e*-Yj}4_9CQ zRLe8@{xFm83QI2}ZkaqZpEN$;63_~CwUK4#JZV;vZxNHf4;i;m0hQ`J2ZW=-B$`HM z7nA3NdH8r9ZLEsT`9t+jp9;cKLGk-ch{=zJ^0!bi?2$I|!{n1eCchMvzD=I>D<*#x z{LG())(gRp{h6TWg75i<1f3FPD{9S5mV*b0enPZ5L^&I%2OPkM&}{|n>gpoWah6yt zc14D6FLXPB?enFFwT~pG!j+Cmi75}Wt_G9a&K6UJ1bRat0#Gik}u6oTlT=+mTXoubnG$9 z)78n|q`D?Lp&#iw>u3B~f00(hf8*!)VSf>_dl7R{vK(VXmIb5<4!Yfi>7=%BWm7Qx z1W)?%G)D`K>d$nWh?9>-TTK&2s66AWdUh)vIW|9gUzgfv%uT3oqVxKF2(ro^JkC&$ z93S>8j^S|3UoQMj^km%NuREG5_Eb-VN8MS?m`8V8SAx$-3H6#v#cA8WaH^Y5Seb0=qu$1Nj%u|IDoH$>SgEe-BSXVanDtKx=ZCSGx{c;|b`9v6gc_&y{L1n98fPXrj8~$qt3fi-zDU;MWb3d>~}4Zd{}` zTB>`j(y5fmIg?{1m`snE>qyqccru$wemAP+N&fGs6i%t(E<`0uYHO+GHqz7qb_B6@ z5mpI!-C!!k0@K30{b~?bjnZFcHXD9(G&`(uH+ zu=IACiK`4t-=^hog<&Z*y^Ko>OSh&Qk~Jvz&45d~JAILEHNB%xX6^daZgbfWUhTh2 ze@-tjnDgy5oc!%*qNulj;Mdr+b(wZ^0Vhk?Ak*_$gQU%vrddNN_P^_=FllZ^0W(WY z8NZKjpc((=ZopoT-TrhSmKPlSOt?!}CM*_wgDaP=LNfmxF?EytMx+F(yf<=tJ&!cM z8d(789;KzoZYB9>L`n2r5fS$P5wVGS2g&CmOCUWNSrV7tkJLfxj?{`hg1(#NXCeaR zu837=>~@p9D`JKC5X2rd?uaZB1))!{h_ym5exDV}NXwgNpFc)&QKV5IxizAMkrxjm z2fdoIO_e<^RyC9~l%YKNiSd9kA`P%z7Pn1~+a^Y2_+K4wrAQrG7e;tDT!lxK83jg< z?jYF~Z~lJNh>*ALHRBgSHB^R04<;3*q(Lbun}8m)5GVs)UWi>1 zv>rBFEz+$MJ#KmM>NEOGSRlq`ooV+CYOS9%e_rrx`v;uX+S{B^&{f3pZx+kB-gTi_ zpR8N6K8r80u4&C)XK>_A*IEtsnIh%)3as*3!D8$YZkyLHREVsgPbgouQqa6N*V4bP zUKm+dBlI^L)~#t-yPnM~v#xDj+n!}z+uZ6FM(mmNs6|gk6r8G<^E3&k?mXw`ZTU-f z3tT}u?Go~Ag>%TbM_~MbkX4_%{9V0i)>E!KlCnQqDhNU^-Ld+g?N!!)Hn$1C5}t0p zn;vL-#_q9yv@Ub$Bzvaqiv_)FV+%?w3G}o@Ynx>t6n8mLm zT95R&kT&|9e9G>kq;T6dA{DBdaYPQZOCy=~CD=OfSUwIZYg#wRz4Vql ze~9y(Ez`j&GwsgWAz`buJL^PIOC%PkjWYT7qhUT8*dC1rc1B1(9qcl$yRS)|X>C~- z3*0X~G--?pb?B4oPku?kaMRQdz* zxx{9$FA=A0QI=9-xE96sT}ZK;AEpZ2Jc{NL$rnQQyIB>Q ziUxG>{DD^#^bRIB>LUqLU-KwJ%_>&Tc~vfVX_be`7p2n4cbS-69^B?v>iKDOUKo19 zug4if!w>pJfz8DO&Yz59ymM4KIJv@&y8*9UW40s3-Ac;G&U5AVwF_zP0(FA!0fK|I z!h6C5Ti4)jL1!c5F8s2hJpvWd^z>yJOW1pyS-aoT4{X_PM&KDM|BDl9*&W(fCMX(|vt< z;#2UT)Z&T8(AY7i)Ws0`DE-rIkLLDKU*nqUC#us%epj9HX^QPp$nO|ZJ;@>!C!A#k zhLxC{kMYjhSey_0u}uB$tEa!>1kv!UKqbPkfDKM-aDpFuVPoq^yr7*5^9%^`@CSTsxB9$@nHQ z{Ua()_b^IZs++mLaS3e~n)N%nn`;g1)xi=~3>V+LiEahsgQ zr1={v@Dg zsc*87v^~#WQipJ2NkrpAoW>Bb7*UO&goD12HQ-Hxl+LeaLCL|dpLi4%u52`?K^3E( zMFSW3kGT?8HEE6l%5C>F=#%s$t;MKLzyVnUIs#f|9k^iBGPe?(;ztQUnbUX>b+B0p z48wmJs2b1#@^weF%JsXWYNFBv@25UC+lak`ydV4c(%=~bHQ$n@)-}?O7q043z26yS7!6 z=S?3Kt2}mrOyoJ@dtH3pCg6_KZaluUN8o(-yDUPF5DSb1tpYZr(#zgK@fF+Mg2CP- z=v`z=@UW^_;D3T9@FA}frp|LjJYp+@4>xZ-a)XnF0{Br z{VCWl{sFt>AI0AIu1TJwJ~A>Xo(0b_Unf16{R%Cs>2$CIZoa2>x47HZH5QV)kW49_ zdjxiK%pG_nI5!@byNvVjxY~Jw3*i32AnS<*!of$y?Kaai1i?RE&wOUO*TrVz1{b7? zpjWZ@Iamy9JAzhbNY)AS2%d;sa1xV99$``!Z3G<%BgZ1=Z!!1j=xc0KC1ZI!z!o3^D(!O=(=<9Wl!p? zOb6jq(7XQOrvj~WjWE%*N0>l-DV+&kP>PGq%PS3^`Qbt|@OJE4qP_Kgc0kG6sc^zn z^8V=Cjw1Wf$NC>Gqv0XnY_ZDLfqYo`dDrq04ZIsuvbV)wE|%NeE6}`(pM72PU*pZw z#A#z)E3Ruh5^s}@{X&VDyS!J(6+f@(6OwWM?u|7GWsZ5{)iOI8cqXP)PRQFNluv`6 zUx}GteSp!xj<}yD;jsILE)S1&(ekNP(ZGYTrHj2@wWzkOC{ltx67(j}J-~k8FkqrKR8KS@#;82#og*&Bs%frTu0&rQv;Lpx zZy5{4`Y)m{jYn@44&M+we?#!uu~Ixxk9SmBT5Md-4w&$eE{68w^`wjEt=F%Pp4v1q z<=Wa%mU!ER`!Dcx32(gaLa*Ol_`aVz)F+T}jX?RC&VJ#X^hWV&woA$`$z!XeTS_`6 z?7|K|ss@?HTn!_R!sx`pJ{JOrTuLMyNXyaRMI@LC!I z0Nn}f1K$IxyqLTOn<3Bo!oLDhIsX$PMQngdj&BCDXv3(RBS8$q`MXTf)a?gu8p z{-|Z(vs-?{+#A|kHg$AvZgX}Lb+&Du@$0tD{4Y8ls?F^if89yWj?VVY?HwDD=WBN3 zI^NX2k&pW*rq0bB&bSRIzoBDu$0mMb$Ad)8EiG-h>s;#_muIxP+o0x655#-V=;WPk zoA{aL4V}fbq1n~xY-?-1en9B>xN^Kl+`hA|rI_OOSEupIAaQ#|$(!!o)Ukoz(zdAs zrqs5vWmDU46uaWF8N)G@9j$GfrXz?4)_1mDjRj^M>mN`iF0GiiIt5X;wzb5Al|G$U zoRpTKqpZwH1FHFNs@%#(^yPG<9G91^K<*Tz9JC*J3H)Brmw_(}ai;>+)RJZb_$<&u zAin^$2|6G6J{L6tY6ptIJ3zMr3GlNWbT2Rg{C?0=z{vv2(0IT^fE^Rzyl&tQOP*>j z{%*jMicP|-&A22T#7a@Vd*k1WD&^a7f@mDPO8H_zNY7hK%~ZxL=x8A|U;)pA)-X#K SBNR4K{H_TVy~QAD=zjr;!h`Ps delta 4406 zcmb_feRNdinSb6plQ@|KG9e*MCLy``WXKnRkPi(XLnhpWxqwkZgKM-C5_BSQnTp4z z)Me6bb!oR8S+DIzMA`OW6^v^%#6^@k7On6O?ra4<|!ibgw-ub2CHc?(ZFl z;GX?+lk=P3^L3y5yzhP9_q|tpnqB-mW+NAguhx_0Fz_Pq4_WyE;tJjPL%I89*@LOHUHGP)b!$`bH zBwhke0V{y>Ey`ST?R>79oTf=Z4pOkk-%jGS$SLJ6bGC4qln2bKQm-K33sR1l9}za_ zN@2=b?zoz=W2NpSrhTkecQEaw-XSr~HeCt%gibY9mCBrVHpBm>UUwf`&NN|qwDsyI zR*`Ajb*6Re$}dxQ<(_IL%SAu~e8BKFa_wtUmfaR!L)uqH=`JdvDw}#(8k6Qz1=S5P ztx*?6(tZISYhaDfw_f=~k~T~cI-D8M8(jGE+f-EGMNhse+Th>NeqfysETD|e|0L>-MZPY@m=9<9u z7NZJ!neoa%ep-Qxtw^n|om?azqVN{d@{9`jat-AVX$}i%rxT>To>0C_TXsKbpC>+z z2!DSf6%oW%U z!L|?B)2!^a{%z`Iq|!)9P2bIWlqb^bl|yqX3NjK-B5h%Uk0>=EQe7Qs!5Kc}6En)x zoHpV7j8Z+fzbbq?SuO!vkVJ36V}Wd&$RHzH5Es29+D-(e>-sh2?Ya4mr{mlbD78(P zTp7efbCr40j%aU8(dKrfu1A{7Ey}9Q+=A_K!Rb_~H$76^{GqGwi@o8rv*`;gU>Jv3AG{v@l<_MeOxrP=-< zW{>xD7y5gPvd*@~_AH&TpP>PJ#vW(&UqaTs%6?mk!&&TY6SS#Vs%-au7_oS)9ob%I ziJ<*`OgU>SVjap=TZLsmrF%cJe?m%GRta+|?OA(;`{2)d3I5&l3Id1X&OM|}#49mx zJ@LENqp`Gqj<1BanS08;Zqj}g=d-#YE@P&D8u!R{A8C{EbSWpV{x~%1=^EwH?$qlMxX`aYh+1!PIAfZ(taf`J2Q|U-y)IrS8;yt`N@0F zL+rhIV!i!)tRZ+?Fq1!9OR|cTcqd}plCegN*Lf|bA?uw<@6}ibybVdOmh^rR8~ZV7 zuB0~-D@7d@N$-WE_tTjFClt_@Cf$><0=O3>-5)01AHd)~N zxO2Q+n+6KpT?nvwOm+LVPS>ehI|rHjuKw(;S-y04ws*KBU7lxhfbUNJK8;@uBj<}2 z%iI+)5s{0zs14`Ku;=6W}qJlQ^UwwFo-+0pB7|0nb`zrf_#1pB6M+L6HIQD9fT zX-87wMjn5AQ0h@axhKo_UNfTKNiglTSWHYr4;hK*=!_BlQ)0Mu>qGaNv)z4Ljp)ym zw-)3FP9}`#zsHT}+tW-r>0JO?_&+5-97!EZ@kd@-;=yKj_X)0dLmkf z%pZni$?jv?E_*zs@GX@qZJSw*5Hbw~sv181eK+1NZnZFtD^!x(|#B;%eCGDzaNiw5cz+HnA3o5B1KN|IF#=EJmEA` z9?7pw;pRc*#r%~y*JgP5Y$>)?WI@Q){D)Yv(p&JZ5L~U4EPTVkZ#Nq=wwf$K3}Xzo zk=5g0E)-0TF4%g2eqaz727U_cTdi1&m!?d~JHcJg9ob_*B_h zp)zA74pV?1UtaK2^b~1eGwys1u?Hagf#AyVsYO;6;KlJgyZ}C?*&@y(_7X4%1c4FY z5TH$bDs@X6rL|Ic!ZfsZ%uH035}FLN<)vsq3Vjq7tMqdY>b)bclFlvptenh8*wcj# zmhfmRg=`n>)Z1*OJSUyAUSP|!1OC#Xy9%gE3#I$nbM_&D-j}K3S!p5VQM#{a=y)rs zC&N@JsN^JdG#r$J9!Yd~2TI@FFd_-z?L#hUL^7hE>TU@gU3E3|FY+-Dd*0!74v{lO zWerAD)ww(!;sx5&*W@uzH8O~$W5^J=nc~FPDPEFa@{|)ozE1JljTBZKkcahy@aihIi5nd2iCjww*rH;ea^OuE@`Lh9!rWJr^jmBV)SC4zeaA z`h?y*L;=(ls1{Tar}RhqN~GVVn?{W2jQ*tjq{mr-BE&uGS-6fK7-Gwf=ysiI^$?HH z^~H$pL(zOxyX5bC)*{*}?!G0uD;e#={Jg01(%0*L`8z0mQ?l@e(44Wr&C@_T!4AG7 znZoT7EuoMcVT+DQtiEXIRNHasL232SF{yj4rn1J2rE_XYT^iXY_cT5yjkNQd!KM{4-jA>sgybDQS9xw0Xgn@8B~xEfGnS_N zsA{=dICM5bC00M%Z$m$bRdnv^6mKuqQdAW?hgY@eN#pAY4|POLhxzRNHkMLIp)27Z zq0O#@O$T;Y)8u>M2D!nr1M}h*TfUx`MD&coXYsK}jU1@;Y+B^-ZWbHhZl(t@$%*K3 zgO=!_3VD%d2YhaE$t}Lor0+oK0jW|h+_+yVlrOZ6NNL!{4;pT%+E+i@HOTXGgLlw$ z$Su_@f?e#AZr)>0GLJhTjeg^jzqxUVRm^BeL?1IYtPO?C*uOTF^CC0Xi{YTuhPS}k zW!i&f{b+LABJmLTW#AOHo}=Jl;2ppTeFfkGoU|=D#B~PM=%Ge=+x=KR4R`YTU8sM# zujQ8dn{L)$4tuix*#wJ|3Ea}rZ(LD$v;4+Hbb(Qi2dajqm31|xcbX16@tCHjUv8bJ z4vV7u)^#${K1WW!u}b(FA=Nwez?4XrBjpq*3q=BDA4JTrIEX1!N@BL3O8!H6yXGF| zSLW9iu{@=rc1M;UyV&fstZL1q_2Uz@nJjI+i9|cvs6hGSlD=EC+v{Fg*aQ2S#bhb3 zBufe~8_QD_C~Rp<3b*8quUUEv+5o#h1o^UC+x4nOK6uhv!t%n!e5Ym4T*u{662bdSN2EMNi;;oW7v&>$?)LF7Tkd879dcSaV(uV|9hd`I1g`)ZAh&_N zz*iY482l)34)WvReZZVKWO)WW3Y>xcC^!hb1^IX2e+7Dx=REil@IA;H*o?Q!AE#5k z#R4G*_#+&4a5eB2. */ +#include "flash.h" #include "hardware.h" #include "steppers.h" -int32_t steps[MOTORSNO] = {0,0,0}; +int8_t motdir[MOTORSNO]; // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) +static volatile int32_t stppos[MOTORSNO]; // current position (in steps) by STP counter +static int32_t encpos[MOTORSNO]; // current encoder position (4 per ticks) minus TIM->CNT +static int32_t stepsleft[MOTORSNO]; // steps left to stop (full steps or encoder's counters) + +static uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position +static uint8_t stopflag[MOTORSNO]; // ==1 to stop @ nearest step + +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(); +} + +// get position +errcodes getpos(int i, int32_t *position){ + if(the_conf.motflags[i].haveencoder){ + *position = encpos[i] + enctimers[i]->CNT; + }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 + return ERR_OK; +} + +void stopmotor(int i){ + stopflag[i] = 1; + stepsleft[i] = 0; + microsteps[i] = 0; +} + +// count steps @tim 14/15/16 +void addmicrostep(int i){ + if(mottimers[i]->SR & TIM_SR_UIF){ + if(++microsteps[i] == the_conf.microsteps[i]){ + microsteps[i] = 0; + if(stopflag[i]){ // stop NOW + stopflag[i] = 0; + mottimers[i]->CR1 &= ~TIM_CR1_CEN; // stop timer + } + stppos[i] += motdir[i]; + } + } + mottimers[i]->SR = 0; +} + +void encoders_UPD(int 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 + if(the_conf.motflags[i].encreverse) d = -d; + if(d == 1) encpos[i] += the_conf.encrev[i]; + else encpos[i] -= the_conf.encrev[i]; + } + enctimers[i]->SR = 0; +} diff --git a/F0-nolib/3steppersLB/steppers.h b/F0-nolib/3steppersLB/steppers.h index 895e84a..3612709 100644 --- a/F0-nolib/3steppersLB/steppers.h +++ b/F0-nolib/3steppersLB/steppers.h @@ -21,7 +21,17 @@ #define STEPPERS_H__ #include +#include "commonproto.h" -extern int32_t steps[]; +// direction +extern int8_t motdir[]; + +void addmicrostep(int i); +void encoders_UPD(int i); + +void init_steppers(); +errcodes getpos(int i, int32_t *position); +errcodes setpos(int i, int32_t newpos); +void stopmotor(int i); #endif // STEPPERS_H__