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 e1b8217..0f6a3d3 100755 Binary files a/F0-nolib/3steppersLB/steppers.bin and b/F0-nolib/3steppersLB/steppers.bin differ diff --git a/F0-nolib/3steppersLB/steppers.c b/F0-nolib/3steppersLB/steppers.c index 86a2adb..42f070c 100644 --- a/F0-nolib/3steppersLB/steppers.c +++ b/F0-nolib/3steppersLB/steppers.c @@ -20,73 +20,184 @@ #include "hardware.h" #include "steppers.h" -int8_t motdir[MOTORSNO]; // motors' direction: 1 for positive, -1 for negative (we need it as could be reverse) +// goto zero stages +typedef enum{ + M0RELAX, // normal moving + M0FAST, // fast move to zero + M0PLUS, // move 200 steps + + M0SLOW // slowest move to zero +} mvto0state; -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) +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__