wip, before timer stuff

Firmware/include/StepGen2.h

@@ -34,7 +34,7 @@ public:
 
     StepGen2(TIM_TypeDef *Timer, uint32_t timerChannel, PinName stepPin, uint8_t dirPin, void irq(void));
 
-    void handleStepper(void);
+    uint32_t handleStepper(void);
     void timerCB();
     void enable(uint8_t yes);
 
@@ -48,6 +48,8 @@ public:
     double actPos() { return actualPosition; };
     void setScale(int16_t spm) { stepsPerMM = spm; }
     int16_t getScale() { return stepsPerMM; }
+    uint32_t updatePosAndReturn(int32_t stepPosStop, uint32_t i);
+
 };
 
 #endif

Firmware/src/StepGen2.cpp

@@ -24,10 +24,10 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
     pinMode(dirPin, OUTPUT);
 }
 
-void StepGen2::handleStepper(void)
+uint32_t StepGen2::handleStepper(void)
 {
     if (!enabled)
-        return;
+        return 1;
     lcncCycleTime = StepGen2::sync0CycleTime;
 
     float y0TRAJ = oldPos() * getScale();              // Straight line equation between old and new point
@@ -36,45 +36,36 @@ void StepGen2::handleStepper(void)
     float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime;      // Intercept
     int32_t stepPosStart = floor(y0TRAJ);              // First step position, integer value of first point position
     int32_t stepPosStop = floor(y1TRAJ);               // End step position
-
+                                                       //
     float Tstart = (stepPosStart - mTRAJ) / kTRAJ;     // First step at this time
     float Tstop = (stepPosStop - mTRAJ) / kTRAJ;       // And the last step
     float Tstep = fabs(1.0 / kTRAJ);                   // Time between steps
     float stepFrequency = fabs(kTRAJ);                 // 1/Tstep - which is kTRAJ
                                                        //
-    oldPos(reqPos());                                 // Save the numeric position for next step
-    oldStepPos(stepPosStop);                          // also the step we are at
-                                                      //
     if (Tstart > lcncCycleTime)                        // Not enough movement to make a step
-        return;                                       //
+        return updatePosAndReturn(stepPosStop, 2);     //
     if (/* 1.0 / Tstep */ kTRAJ > 200000)              //
     {                                                  // Too high frequency, deal with this later.
-        err = 1;                                      //
+        return updatePosAndReturn(stepPosStop, 3);     //
-        return;                                       //
     }                                                  //
     int8_t dir = stepPosStart > stepPosStop ? -1 : 1;  // Which direction to step in
                                                        //
-    switch (abs(stepPosStart - oldStepPos()))         //
+    if (abs(stepPosStart - oldStepPos()) == 0)         // StepPosStart and oldStepPos() are often the same, but don't redo the step
     {                                                  //
-    case 0:                                           // StepPosStart and oldStepPos() are often the same, but don't redo the step
         stepPosStart += dir;                           // New first step
         Tstart += Tstep;                               //
         if (Tstart > lcncCycleTime)                    // Not enough movement to make a step
-            return;                                   //
+            return updatePosAndReturn(stepPosStop, 4); //
-        break;                                        //
+    }                                                  //
-    case 1:                                           //
+    if (abs(stepPosStart - oldStepPos()) > 1)          // Shouldn't happen
-                                                      // Let it slide through and deal with it after the case switch
+    {                                                  //
-        break;                                        //
+        return updatePosAndReturn(stepPosStop, 5);     //
-    default:                                          //
-        err = 2;                                      //
-        return;                                       //
-        break;                                        //
     }                                                  //
                                                        // Now the old point and the start point should be separate.
     if (Tstart > lcncCycleTime)                        // Not enough movement to make a step
-        return;                                       //
+        return updatePosAndReturn(stepPosStop, 6);     // Check this again
                                                        // Tstart, Tstep and Tstop defines the coming pwm-sequence.
-    // Always do one pulse at Tstart when we come here. Next Tstart+Tstep and so on until Tstop.
+    return 0;                                          // Always do one pulse at Tstart when we come here. Next Tstart+Tstep and so on until Tstop.
 }
 void StepGen2::timerCB()
 {
@@ -110,5 +101,11 @@ void StepGen2::timerCB()
     }
 #endif
 }
+uint32_t StepGen2::updatePosAndReturn(int32_t stepPosStop, uint32_t i)
+{                            //
+    oldPos(reqPos());        // Save the numeric position for next step
+    oldStepPos(stepPosStop); // also the step we are at}
+    return i;
+}
 
 uint32_t StepGen2::sync0CycleTime = 0;