add some code to 3steppersLB

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
+#if MICROSTEPSMAX > 512
-#error "Change the code here!"
+#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;
+        if(m != 1<<MSB(m)) return ERR_BADVAL;
-        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<<mostbit) return ERR_BADVAL;
         the_conf.microsteps[n] = m;
     }
     *val = the_conf.microsteps[n];
     return ERR_OK;
 }
 
+static errcodes encstepsminparser(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 - 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 (steps/s^2)"},
-    [CMD_ACCEL] = {"accel", accparser, "set/get accel/decel (usteps/s per 10ms)"},
+    [CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (steps per sec)"},
-    [CMD_MAXSPEED] = {"maxspeed", maxspdparser, "set/get max speed (usteps 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"},
 };
 

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
 };

F0-nolib/3steppersLB/flash.c

@@ -24,6 +24,7 @@
 #include <stm32f0.h>
 #include <string.h> // 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);

F0-nolib/3steppersLB/flash.h

@@ -32,8 +32,8 @@
 
 // limiting values
 #define MICROSTEPSMAX       (512)
-// (STEPS per second per 10ms)
+// (STEPS per second^2)
-#define ACCELMAXSTEPS       (100)
+#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 accel[MOTORSNO];       // acceleration/deceleration (steps/s^2)
-    uint16_t maxspd[MOTORSNO];      // max motor speed (microsteps per second)
+    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)

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->CR1 = 0;
-    TIM->ARR = 1000; // starting ARR value
+    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);

F0-nolib/3steppersLB/hardware.h

@@ -22,6 +22,17 @@
 
 #include <stm32f0.h>
 
+// 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__

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);

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
+typedef enum{
-static int32_t encpos[MOTORSNO]; // current encoder position (4 per ticks) minus TIM->CNT
+    STALL_NO,       // moving OK
-static int32_t stepsleft[MOTORSNO]; // steps left to stop (full steps or encoder's counters)
+    STALL_ONCE,     // Nstalled < limit
+    STALL_STOP      // Nstalled >= limit
+} t_stalled;
 
-static uint16_t microsteps[MOTORSNO] = {0}; // current microsteps position
+// motors' direction: 1 for positive, -1 for negative (we need it as could be reverse)
-static uint8_t stopflag[MOTORSNO]; // ==1 to stop @ nearest step
+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(){
+// current speed
-    // init variables
+static uint16_t curspeed[MOTORSNO];
-    for(int i = 0; i < MOTORSNO; ++i){
+// delta V according to current acceleration & INTERVAL
-        mottimers[i]->CR1 &= ~TIM_CR1_CEN;
+static uint16_t dV[MOTORSNO];
-        stepsleft[i] = 0;
+// ==1 to stop @ nearest step
-        microsteps[i] = 0;
+static uint8_t stopflag[MOTORSNO];
-        stopflag[i] = 0;
+// motor state
-        motdir[i] = 1;
+static stp_state state[MOTORSNO];
-        stppos[i] = 0;
+// move to zero state
-    }
+static mvto0state mvzerostate[MOTORSNO];
-    timers_setup();
+
+// lowest ARR value (highest speed), highest (lowest speed)
+//static uint16_t stphighARR[MOTORSNO];
+// microsteps=1<<ustepsshift
+static uint16_t ustepsshift[MOTORSNO];
+// amount of steps for full eceleration/deceleration
+static uint32_t accdecsteps[MOTORSNO];
+
+// recalculate ARR according to new speed
+TRUE_INLINE void recalcARR(int i){
+    mottimers[i]->ARR = (MOTORFREQ / curspeed[i]) >> ustepsshift[i];
 }
 
-// get position
+// run this function after each steppers parameters changing
-errcodes getpos(int i, int32_t *position){
+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){
+errcodes getremainsteps(uint8_t i, int32_t *position){
-    if(newpos < -(int32_t)the_conf.maxsteps[i] || newpos > (int32_t)the_conf.maxsteps[i])
+    *position = targstppos[i] - stppos[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){
+// 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);
+    }
+}

F0-nolib/3steppersLB/steppers.h

@@ -23,15 +23,40 @@
 #include <stm32f0.h>
 #include "commonproto.h"
 
-// direction
+// amount of tries to detect motor stall
-extern int8_t motdir[];
+#define NSTALLEDMAX     (5)
 
-void addmicrostep(int i);
+// stepper states
-void encoders_UPD(int i);
+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 getpos(uint8_t i, int32_t *position);
-errcodes setpos(int i, int32_t newpos);
+errcodes getremainsteps(uint8_t i, int32_t *position);
-void stopmotor(int i);
+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__