Class StepGen2 done after Stepgen.odb

Firmware/doc/.~lock.Stepgen.odp#

@@ -1 +0,0 @@
-,HAKAN-PC/Hakan,Hakan-PC,05.02.2024 20:27,file:///C:/Users/Hakan/AppData/Roaming/LibreOffice/4;

Firmware/include/StepGen2.h

@@ -7,49 +7,40 @@ class StepGen2
 {
 private:
     volatile double_t actualPosition;
-    volatile double_t requestedPosition;
-    volatile double_t oldPosition;
-    volatile int32_t oldStepPosition;
-    volatile uint8_t enabled;
     volatile int32_t nSteps;
+    volatile uint32_t timerPulseSteps;
     volatile float Tstart;
     volatile float Tstop;
     volatile float Tstep;
-    HardwareTimer *MyTim;
+    const float maxAllowedFrequency = 100000; // 100 kHz for now
-    HardwareTimer *MyTim2; // 10,11,13,14
+    HardwareTimer *pulseTimer;
-    int16_t stepsPerMM;
+    uint32_t pulseTimerChan;
+    HardwareTimer *startTimer; // 10,11,13,14
     uint8_t dirPin;
     PinName stepPin;
-    uint32_t timerChan;
     const uint32_t maxFreq = 100000;
-    volatile uint32_t prevFreq1 = 0;
-    volatile uint32_t prevFreq2 = 0;
     const float Tjitter = 50.0; // Time unit is microseconds
 
     uint32_t err = 0;
 
 public:
-    static uint32_t sync0CycleTime;
+    volatile double_t commandedPosition;    // End position when this cycle is completed
+    volatile int32_t commandedStepPosition; // End step position when this cycle is completed
+    volatile double_t initialPosition;      // From previous cycle
+    volatile int32_t initialStepPosition;   // From previous cycle
+    int16_t stepsPerMM;                     // This many steps per mm
+    volatile uint8_t enabled;               // Enabled step generator
+    volatile float frequency;
+
+    static uint32_t sync0CycleTime;  // Nominal EtherCAT cycle time
     volatile uint32_t lcncCycleTime; // Linuxcnc nominal cycle time (1 ms often)
 
     StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
 
     uint32_t handleStepper(void);
-    void timerCB();
+    void startTimerCB();
-    void timer2CB();
+    void pulseTimerCB();
-    void enable(uint8_t yes);
+    uint32_t updatePos(uint32_t i);
-
-    void reqPos(double_t pos) { requestedPosition = pos; };
-    double reqPos() { return requestedPosition; };
-    void oldPos(double_t pos) { oldPosition = pos; };
-    double oldPos() { return oldPosition; };
-    void oldStepPos(int32_t pos) { oldStepPosition = pos; }
-    int32_t oldStepPos() { return oldStepPosition; }
-    void actPos(double_t pos) { actualPosition = pos; };
-    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

@@ -5,117 +5,82 @@
 StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
 {
     actualPosition = 0;
-    requestedPosition = 0;
+    commandedPosition = 0;
-    oldPosition = 0;
+    commandedStepPosition = 0;
-    oldStepPosition = 0;
+    initialPosition = 0;
+    initialStepPosition = 0;
     stepsPerMM = 0;
     enabled = 0;
 
     dirPin = _dirPin;
     stepPin = _stepPin;
-    timerChan = _timerChannel;
+    pulseTimerChan = _timerChannel;
-    MyTim = new HardwareTimer(Timer);
+    pulseTimer = new HardwareTimer(Timer);
-    MyTim->attachInterrupt(irq);
+    pulseTimer->attachInterrupt(irq);
     pinMode(dirPin, OUTPUT);
-    MyTim2 = new HardwareTimer(Timer2);
+    startTimer = new HardwareTimer(Timer2);
-    MyTim2->attachInterrupt(irq2);
+    startTimer->attachInterrupt(irq2);
 }
 
 uint32_t StepGen2::handleStepper(void)
 {
     if (!enabled)
-        return 1;
+        return updatePos(0);
     lcncCycleTime = StepGen2::sync0CycleTime;
+    commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
+    if (initialStepPosition == commandedStepPosition)              // No movement
+        return updatePos(1);
 
-    float y0TRAJ = oldPos() * getScale();                   // Straight line equation between old and new point
+    float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step
-    float y1TRAJ = reqPos() * getScale();                   // Time runs between 0 and lcncCycleTime (1 ms)
+                                 / (float)lcncCycleTime;                           // from here on
-    float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime;        // Slope
+    if (approximateFrequency > maxAllowedFrequency)                                // Stay on this position
-    float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime;           // Intercept
+        return 1;
-    int32_t stepPosStart = floor(y0TRAJ);                   // First step position, integer value of first point position
+
-    int32_t stepPosStop = floor(y1TRAJ);                    // End step position
+    float kTRAJ = (commandedPosition - initialPosition) / float(lcncCycleTime); // Straight line equation
-                                                            //
+    float mTRAJ = initialPosition;                                              // position = kTRAJ x time + mTRAJ
-    float Tstart = (stepPosStart - mTRAJ) / kTRAJ;          // First step at this time
+                                                                                // Operating on incoming positions (not steps)
-    float Tstop = (stepPosStop - mTRAJ) / kTRAJ;            // And the last step
+    if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01)                        // Very flat slope
-    float Tstep = fabs(1.0 / kTRAJ);                        // Time between steps
+    {                                                                           //
-    float stepFrequency = fabs(kTRAJ);                      // 1/Tstep - which is kTRAJ
+        Tstart = 0.5 * lcncCycleTime;                                           // Just take a step in the middle of the cycle
-                                                            //
+        frequency = 10000;                                                      // At some suitable frequency
-    if (Tstart > lcncCycleTime)                             // Not enough movement to make a step
+        nSteps = kTRAJ > 0 ? 1 : -1;                                            // Take only one step
-        return updatePosAndReturn(stepPosStop, 2);          //
-    if (/* 1.0 / Tstep */ kTRAJ > 200000)                   //
-    {                                                       // Too high frequency, deal with this later.
-        return updatePosAndReturn(stepPosStop, 3);          //
-    }                                                       //
-    int8_t dir = stepPosStart > stepPosStop ? -1 : 1;       // Which direction to step in
-                                                            //
-    if (abs(stepPosStart - oldStepPos()) == 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 updatePosAndReturn(stepPosStop, 4);      //
-    }                                                       //
-    if (abs(stepPosStart - oldStepPos()) > 1)               // Shouldn't happen
-    {                                                       //
-        return updatePosAndReturn(stepPosStop, 5);          //
-    }                                                       //
-                                                            // Now the old point and the start point should be separate.
-    if (Tstart > lcncCycleTime)                             // Not enough movement to make a step
-        return updatePosAndReturn(stepPosStop, 6);          // Check this again
-                                                            // Tstart, Tstep and Tstop defines the coming pwm-sequence.
-                                                            //
-    MyTim2->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs
-    MyTim2->resume();
-    return updatePosAndReturn(stepPosStop, 0);
-}
-void StepGen2::timer2CB()
-{
-    MyTim2->pause(); // Once is enough.
-    MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
-    MyTim->setOverflow(floor(1e6 / Tstep), HERTZ_FORMAT);            // 100000 microseconds = 100 milliseconds
-    MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50%
-    nSteps = round((Tstop - Tstart) / Tstep + 1);
-    if (nSteps > 0)
-        MyTim->resume();
-}
-void StepGen2::timerCB()
-{
-#if 0
-    timerStepPosition += timerStepDirection; // The step that was just completed
-    if (timerNewEndStepPosition != 0)        // Are we going to reload?
-    {
-        // Input for reload is timerNewEndStepPosition
-        // The timer has current position and from this
-        // can set new frequency and new endtarget for steps
-        MyTim->pause(); // We are not at stop, let's stop it. Note stepPin is floating
-        int32_t steps = timerNewEndStepPosition - timerStepPosition;
-        if (steps != 0)
-        {
-            uint8_t sgn = steps > 0 ? HIGH : LOW;
-            digitalWrite(dirPin, sgn);
-            float_t freqf = abs(steps) / float(pwmCycleTime * 1.0e-6);
-            uint32_t freq = uint32_t(freqf);
-            timerStepDirection = steps > 0 ? 1 : -1;
-            timerStepPositionAtEnd = timerNewEndStepPosition;
-            timerNewEndStepPosition = 0; // Set to zero to not reload next time
-            MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
-            MyTim->setOverflow(freq, HERTZ_FORMAT);
-            MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
-            MyTim->resume();
-            timerIsRunning = 1;
-        }
     }
-    if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
+    else // Regular step train, up or down
     {
-        timerIsRunning = 0;
+        if (kTRAJ > 0)
-        MyTim->pause();
+            Tstart = (ceil(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
+        else
+            Tstart = (floor(initialPosition * stepsPerMM) - mTRAJ) / kTRAJ;
+        frequency = kTRAJ * stepsPerMM;
+        nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction.
     }
-#endif
+    updatePos(5);
+
+    startTimer->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs
+    startTimer->resume();
+    return 1;
+}
+void StepGen2::startTimerCB()
+{
+    startTimer->pause(); // Once is enough.
+    digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
+    timerPulseSteps = abs(nSteps);
+    pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
+    pulseTimer->setOverflow(uint32_t(frequency), HERTZ_FORMAT);
+    pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT); // 50%
+    pulseTimer->resume();
+}
+void StepGen2::pulseTimerCB()
+{
+    --timerPulseSteps;
+    if (timerPulseSteps == 0)
+        pulseTimer->pause();
 }
 
-uint32_t StepGen2::updatePosAndReturn(int32_t stepPosStop, uint32_t i)
+uint32_t StepGen2::updatePos(uint32_t i)
-{                            //
+{                                                //
-    oldPos(reqPos());        // Save the numeric position for next step
+    initialPosition = commandedPosition;         // Save the numeric position for next step
-    oldStepPos(stepPosStop); // also the step we are at}
+    initialStepPosition = commandedStepPosition; // also the step we are at}
     return i;
 }
 

Firmware/src/main.cpp

@@ -33,11 +33,11 @@ void timerCallbackStep2(void)
 }
 #endif
 #include "StepGen2.h"
-void timerCallbackStep(void);
+void pulseTimerCallback(void);
-void timerCallbackStepStart(void);
+void startTimerCallback(void);
-StepGen2 Step(TIM1, 4, PA_11, PA12, timerCallbackStep, TIM10, timerCallbackStepStart);
+StepGen2 Step(TIM1, 4, PA_11, PA12, pulseTimerCallback, TIM10, startTimerCallback);
-void timerCallbackStep(void) { Step.timerCB(); }
+void pulseTimerCallback(void) { Step.pulseTimerCB(); }
-void timerCallbackStepStart(void) { Step.timer2CB(); }
+void startTimerCallback(void) { Step.startTimerCB(); }
 CircularBuffer<uint32_t, 200> Tim;
 volatile uint64_t nowTime = 0, thenTime = 0;
 
@@ -57,6 +57,9 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
 
 void handleStepper(void)
 {
+   Step.enabled = true;
+   Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
+
 #if 0
    Step1.handleStepper();
    Step2.handleStepper();