add servo-emulation @ stepper motors

F0-nolib/3steppersLB/commonproto.c

@@ -53,7 +53,7 @@ static errcodes buzzerparser(uint8_t par, int32_t *val){
 
 static errcodes adcparser(uint8_t par, int32_t *val){
     uint8_t n = PARBASE(par);
-    if(n > NUMBER_OF_ADC_CHANNELS) return ERR_BADPAR;
+    if(n > NUMBER_OF_ADC_CHANNELS-1) return ERR_BADPAR;
     *val = (int32_t) getADCval(n);
     return ERR_OK;
 }

F0-nolib/3steppersLB/flash.c

@@ -34,7 +34,7 @@ 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, .donthold = 1, .eswinv = 1}
+#define DEFMF   {.haveencoder = 1, .donthold = 1, .eswinv = 1, .keeppos = 1}
 
 #define USERCONF_INITIALIZER  {             \
      .userconf_sz = sizeof(user_conf)       \
@@ -42,12 +42,12 @@ static uint32_t maxCnum = 1024 / sizeof(user_conf); // can't use blocksize here
     ,.CANID = 0xaa                          \
     ,.microsteps = {32, 32, 32}             \
     ,.accel = {500, 500, 500}               \
-    ,.maxspd = {3000, 3000, 3000}           \
+    ,.maxspd = {2000, 2000, 2000}           \
     ,.minspd = {20, 20, 20}                 \
     ,.maxsteps = {500000, 500000, 500000}   \
     ,.encrev = {4000,4000,4000}             \
-    ,.encperstepmin = {5,5,5}               \
+    ,.encperstepmin = {17,17,17}            \
-    ,.encperstepmax = {50,50,50}            \
+    ,.encperstepmax = {23,23,23}            \
     ,.motflags = {DEFMF,DEFMF,DEFMF}        \
     ,.ESW_reaction = {ESW_ANYSTOP, ESW_ANYSTOP, ESW_ANYSTOP} \
     }

F0-nolib/3steppersLB/flash.h

@@ -51,6 +51,7 @@ typedef struct{
     uint8_t haveencoder : 1;    // bit2 - have encoder
     uint8_t donthold : 1;       // bit3 - clear power @ stop (don't hold motor when stopped)
     uint8_t eswinv : 1;         // bit4 - invers end-switches
+    uint8_t keeppos : 1;        // bit5 - keep current position (as servo motor)
 } motflags_t;
 
 /*

F0-nolib/3steppersLB/hardware.c

@@ -94,13 +94,11 @@ static void setup_mpwm(int i){
 #error "change the code!"
 #endif
     TIM->CCR1 = MOTORTIM_ARRMIN - 3; // ~10us for pulse duration
-    TIM->CNT = 0;
     TIM->ARR = 0xffff;
 //    TIM->EGR = TIM_EGR_UG; // generate update to refresh ARR
     TIM->BDTR |= TIM_BDTR_MOE; // enable main output
     TIM->CCER = TIM_CCER_CC1E; // turn it on, active high
     TIM->DIER = TIM_DIER_CC1IE; // allow CC interrupt (we should count steps)
-      //TIM->CR1 = TIM_CR1_CEN;
     NVIC_EnableIRQ(motirqs[i]);
 }
 
@@ -124,7 +122,6 @@ static void setup_enc(int i){
     TIM->ARR = the_conf.encrev[i];
     // enable timer
     TIM->CR1 = TIM_CR1_CKD_1 | TIM_CR1_CEN; /* (4) */
-    TIM->CNT = 0;
     NVIC_EnableIRQ(encirqs[i]);
 }
 
@@ -144,9 +141,9 @@ void timers_setup(){
     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
+        if(the_conf.motflags[i].haveencoder){ // motor have the encoder
             setup_enc(i);
-        //}
+        }
     }
 }
 

F0-nolib/3steppersLB/steppers.c

@@ -42,19 +42,19 @@ 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];
+static int32_t targstppos[MOTORSNO] = {0};
 // 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];
+static int32_t prevencpos[MOTORSNO] = {0};
 // encoders' ticks per step (calculates @ init)
 static int32_t encperstep[MOTORSNO];
 
 // current speed
 static uint16_t curspeed[MOTORSNO];
-static uint16_t decstartspeed[MOTORSNO]; // speed when deceleration starts
+static uint16_t startspeed[MOTORSNO]; // speed when deceleration starts
 // ==1 to stop @ nearest step
 static uint8_t stopflag[MOTORSNO];
 // motor state
@@ -81,7 +81,6 @@ TRUE_INLINE void recalcARR(int i){
     else if(ARR > 0xffff) ARR = 0xffff;
     mottimers[i]->ARR = ARR;
     curspeed[i] = (((PCLK/(MOTORTIM_PSC+1)) / (ARR+1)) >> ustepsshift[i]); // recalculate speed due to new val
-    //SEND("New ARR["); bufputchar('0'+i); SEND("]="); printu(ARR); NL();
 }
 
 // run this function after each steppers parameters changing
@@ -115,16 +114,21 @@ int setencpos(uint8_t i, int32_t position){
     int32_t remain = position % the_conf.encrev[i];
     if(remain < 0) remain += the_conf.encrev[i];
     enctimers[i]->CNT = remain;
-    encpos[i] = position - remain;
+    prevencpos[i] = encpos[i] = position - remain;
-    SEND("position=");printi(position);SEND(", remain=");printi(remain);SEND(", CNT=");printu(enctimers[i]->CNT);SEND(", pos=");printi(encpos[i]);NL();
+    SEND("MOTOR"); bufputchar('0'+i); SEND(", position="); printi(position); SEND(", remain=");
+    printi(remain); SEND(", CNT="); printu(enctimers[i]->CNT); SEND(", pos="); printi(encpos[i]); NL();
     return TRUE;
 }
 
 // get current position
 errcodes getpos(uint8_t i, int32_t *position){
-    /*if(the_conf.motflags[i].haveencoder){
+    if(the_conf.motflags[i].haveencoder){
-        *position = encoder_position(i) / encperstep[i];
+        int32_t p = encoder_position(i);
-    }else */
+        if(p < 0) p -= encperstep[i]>>1;
+        else p += encperstep[i]>>1;
+        p /= encperstep[i];
+        *position = p;
+    }else
         *position = stppos[i];
     return ERR_OK;
 }
@@ -134,6 +138,35 @@ errcodes getremainsteps(uint8_t i, int32_t *position){
     return ERR_OK;
 }
 
+// calculate acceleration/deceleration parameters for motor i
+static void calcacceleration(uint8_t i){
+    int32_t delta = targstppos[i] - 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(the_conf.motflags[i].reverse) 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(the_conf.motflags[i].reverse) MOTOR_CW(i);
+        else MOTOR_CCW(i);
+    }
+    state[i] = STP_ACCEL;
+    startspeed[i] = curspeed[i];
+    Taccel[i] = Tms;
+    recalcARR(i);
+}
+
 // move to absolute position
 errcodes motor_absmove(uint8_t i, int32_t newpos){
     //if(i >= MOTORSNO) return ERR_BADPAR; // bad motor number
@@ -144,38 +177,14 @@ errcodes motor_absmove(uint8_t i, int32_t newpos){
     targstppos[i] = newpos;
     prevencpos[i] = encoder_position(i);
     prevstppos[i] = stppos[i];
-    uint8_t inv = the_conf.motflags[i].reverse;
-    int32_t delta = newpos - stppos[i];
-   SEND("delta="); printi(delta);
-    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);
-    }
-   SEND("\ntargstppos="); printi(targstppos[i]);
-   SEND("\ndecelstart="); printi(decelstartpos[i]);
-   SEND("\naccdecsteps="); printu(accdecsteps[i]); NL();
     curspeed[i] = the_conf.minspd[i];
-    recalcARR(i);
+    calcacceleration(i);
+    SEND("MOTOR"); bufputchar('0'+i);
+    SEND(" targstppos="); printi(targstppos[i]);
+    SEND(", decelstart="); printi(decelstartpos[i]);
+    SEND(", accdecsteps="); printu(accdecsteps[i]); NL();
     MOTOR_EN(i);
     mottimers[i]->CR1 |= TIM_CR1_CEN; // start timer
-    state[i] = STP_ACCEL;
-    Taccel[i] = Tms;
     return ERR_OK;
 }
 
@@ -210,12 +219,24 @@ void addmicrostep(uint8_t i){
     if(++microsteps[i] == the_conf.microsteps[i]){
         microsteps[i] = 0;
         stppos[i] += motdir[i];
-        if(stopflag[i] || stppos[i] == targstppos[i]){ // stop NOW
+        uint8_t stop_at_pos = 0;
+        if(motdir[i] > 0){
+            if(stppos[i] >= targstppos[i]){ // reached start of deceleration
+                stop_at_pos = 1;
+            }
+        }else{
+            if(stppos[i] <= targstppos[i]){
+                stop_at_pos = 1;
+            }
+        }
+        if(stopflag[i] || stop_at_pos){ // stop NOW
+            if(stopflag[i]) targstppos[i] = stppos[i]; // keep position (for keep flag)
             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;
+            SEND("MOTOR"); bufputchar('0'+i); SEND(" stop @"); printi(stppos[i]); newline();
         }
     }
 }
@@ -237,45 +258,76 @@ static t_stalled chkSTALL(uint8_t i){
     if(!the_conf.motflags[i].haveencoder) return STALL_NO;
     int32_t curencpos = encoder_position(i), Denc = curencpos - prevencpos[i];
     int32_t curstppos = stppos[i], Dstp = curstppos - prevstppos[i];
-    if(Denc < 0) Denc = -Denc;
+    int difsign = 1;
-    if(Dstp < 0) Dstp = -Dstp;
+    if(Dstp < 0){
+        Dstp = -Dstp;
+        difsign = -difsign;
+    }
     if(Dstp < 10){ // didn't move even @ 10 steps
         return STALL_NO;
     }
+    if(Denc < 0){
+        Denc = -Denc;
+        difsign = -difsign;
+    }
+    if(difsign == -1){ // motor and encoder moves to different sides!!!
+        Denc = -Denc; // init STALL state
+    }
     prevencpos[i] = curencpos;
-    curstppos = curencpos / encperstep[i]; // recalculate current position
+    getpos(i, &curstppos); // recalculate current position
     stppos[i] = curstppos;
     prevstppos[i] = curstppos;
     if(Denc < the_conf.encperstepmin[i]*Dstp || the_conf.encperstepmax[i]*Dstp < Denc){ // stall?
-        SEND("Denc="); printu(Denc); SEND(", Dstp="); printu(Dstp); NL();
+        SEND("MOTOR"); bufputchar('0'+i); SEND(" Denc="); printi(Denc); SEND(", Dstp="); printu(Dstp);
-        if(++Nstalled >= NSTALLEDMAX){
+        SEND(", speed="); printu(curspeed[i]);
+        if(++Nstalled > NSTALLEDMAX){
             stopflag[i] = 1;
             Nstalled = 0;
-            DBG("STALL!");
+            SEND("  ---  STALL!"); NL();
             return STALL_STOP;
         }else{
             uint16_t spd = curspeed[i] >> 1; // speed / 2
             curspeed[i] = (spd > the_conf.minspd[i]) ? spd : the_conf.minspd[i];
-            recalcARR(i);
+            // now recalculate acc/dec parameters
-            if(state[i] == STP_MOVE)
+            calcacceleration(i);
-                state[i] = STP_ACCEL;
+            SEND("  ---  pre-stall, newspeed="); printu(curspeed[i]); NL();
-            SEND("pre-stall, speed="); printu(curspeed[i]); NL();
             return STALL_ONCE;
         }
     }
+    prevstppos[i] = curstppos;
     Nstalled = 0;
     return STALL_NO;
 }
 
 #define TODECEL() do{state[i] = STP_DECEL;  \
-        decstartspeed[i] = curspeed[i];     \
+        startspeed[i] = curspeed[i];        \
         Taccel[i] = Tms;                    \
-        SEND("  -> DECEL@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();}while(0)
+        SEND("MOTOR"); bufputchar('0'+i);   \
+        SEND("  -> DECEL@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL(); \
+        }while(0)
 
 // check state of i`th stepper
 static void chkstepper(int i){
     int32_t newspeed;
     switch(state[i]){
+        case STP_RELAX: // check if need to keep current position
+            if(the_conf.motflags[i].haveencoder && the_conf.motflags[i].keeppos){
+                getpos(i, &newspeed);
+                int32_t diff = stppos[i] - newspeed; // correct `curpos` counter by encoder
+                if(diff){
+                    SEND("MOTOR"); bufputchar('0'+i);
+                    SEND(" diff="); printi(diff);
+                    SEND(", change stppos from "); printi(stppos[i]); SEND(" to "); printi(newspeed); NL();
+                    stppos[i] = newspeed;
+                }
+                diff = targstppos[i] - newspeed; // check whether we need to change position
+                if(diff){ // try to correct position
+                    SEND("MOTOR"); bufputchar('0'+i);
+                    SEND(" curpos="); printi(newspeed); SEND(", need="); printi(targstppos[i]); NL();
+                    motor_absmove(i, targstppos[i]);
+                }
+            }
+        break;
         case STP_ACCEL: // acceleration to max speed
             if(STALL_NO == chkSTALL(i)){
                 //newspeed = curspeed[i] + dV[i];
@@ -283,6 +335,7 @@ static void chkstepper(int i){
                 if(newspeed >= the_conf.maxspd[i]){ // max speed reached -> move with it
                     curspeed[i] = the_conf.maxspd[i];
                     state[i] = STP_MOVE;
+                    SEND("MOTOR"); bufputchar('0'+i);
                     SEND("  -> MOVE@"); printi(stppos[i]); SEND(", V="); printu(curspeed[i]); NL();
                 }else{ // increase speed
                     curspeed[i] = newspeed;
@@ -317,33 +370,20 @@ static void chkstepper(int i){
         case STP_DECEL:
             if(STALL_NO == chkSTALL(i)){
                 //newspeed = curspeed[i] - dV[i];
-                newspeed = decstartspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
+                newspeed = startspeed[i] - (the_conf.accel[i] * (Tms - Taccel[i])) / 1000;
                 if(newspeed > the_conf.minspd[i]){
                     curspeed[i] = newspeed;
                 }else{
                     curspeed[i] = the_conf.minspd[i];
                     state[i] = STP_MVSLOW;
+                    SEND("MOTOR"); bufputchar('0'+i);
                     SEND("  -> MVSLOW@"); printi(stppos[i]); NL();
                 }
                 recalcARR(i);
                 //SEND("spd="); printu(curspeed[i]); SEND(", pos="); printi(stppos[i]); newline();
             }
-            // fallthrough
-        case STP_MVSLOW: // position check is in add_microstep
-            /*
-            if(motdir[i] > 0){
-                if(stppos[i] >= targstppos[i]){ // reached start of deceleration
-                    stopflag[i] = 0;
-                    SEND("  -> STOP@"); printi(stppos[i]); NL();
-                }
-            }else{
-                if(stppos[i] <= targstppos[i]){
-                    stopflag[i] = 0;
-                    SEND("  -> STOP@"); printi(stppos[i]); NL();
-                }
-            }*/
         break;
-        default: // RELAX, STALL, ERR -> do nothing
+        default: // STP_MVSLOW, STALL, ERR -> do nothing
             return;
     }
     switch(mvzerostate[i]){

F0-nolib/3steppersLB/steppers.h

@@ -24,7 +24,7 @@
 #include "commonproto.h"
 
 // amount of tries to detect motor stall
-#define NSTALLEDMAX     (55)
+#define NSTALLEDMAX     (5)
 
 // stepper states
 typedef enum{

F0-nolib/3steppersLB/version.inc

@@ -1,2 +1,2 @@
-#define BUILD_NUMBER "97"
+#define BUILD_NUMBER "114"
-#define BUILD_DATE "2021-11-16"
+#define BUILD_DATE "2021-11-17"