add some more commands

F0-nolib/3steppersLB/steppers.c

@@ -16,8 +16,83 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#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;
+}