logic might be there

Firmware/.vscode/settings.json

@@ -10,7 +10,8 @@
         "vector": "cpp",
         "system_error": "cpp",
         "numeric": "cpp",
-        "ostream": "cpp"
+        "ostream": "cpp",
+        "cmath": "cpp"
     },
     "C_Cpp.errorSquiggles": "disabled",
     "cmake.configureOnOpen": false

Firmware/include/StepGen2.h

@@ -0,0 +1,53 @@
+
+#ifndef STEPGEN
+#define STEPGEN
+#include <HardwareTimer.h>
+
+class StepGen2
+{
+private:
+    volatile uint8_t timerIsRunning;
+    volatile int32_t timerStepPosition;
+    volatile int32_t timerStepDirection;
+    volatile int32_t timerStepPositionAtEnd;
+    volatile int32_t timerNewEndStepPosition;
+    volatile uint32_t timerNewCycleTime;
+    volatile double_t actualPosition;
+    volatile double_t requestedPosition;
+    volatile double_t oldPosition;
+    volatile int32_t oldStepPosition;
+    volatile uint8_t enabled;
+    HardwareTimer *MyTim;
+    int16_t stepsPerMM;
+    uint8_t dirPin;
+    PinName stepPin;
+    uint32_t timerChan;
+    const uint32_t maxFreq = 100000;
+    volatile uint32_t prevFreq1 = 0;
+    volatile uint32_t prevFreq2 = 0;
+
+    uint32_t err = 0;
+
+public:
+    static uint32_t sync0CycleTime;
+    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));
+
+    void handleStepper(void);
+    void timerCB();
+    void enable(uint8_t yes);
+
+    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; }
+};
+
+#endif

Firmware/src/StepGen2.cpp

@@ -0,0 +1,114 @@
+#include <Arduino.h>
+#include <stdio.h>
+#include "StepGen2.h"
+
+StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void))
+{
+    timerIsRunning = 0;
+    timerStepPosition = 0;
+    timerStepDirection = 0;
+    timerStepPositionAtEnd = 0;
+    timerNewEndStepPosition = 0;
+    actualPosition = 0;
+    requestedPosition = 0;
+    oldPosition = 0;
+    oldStepPosition = 0;
+    stepsPerMM = 0;
+    enabled = 0;
+
+    dirPin = _dirPin;
+    stepPin = _stepPin;
+    timerChan = _timerChannel;
+    MyTim = new HardwareTimer(Timer);
+    MyTim->attachInterrupt(irq);
+    pinMode(dirPin, OUTPUT);
+}
+
+void StepGen2::handleStepper(void)
+{
+    if (!enabled)
+        return;
+    lcncCycleTime = StepGen2::sync0CycleTime;
+
+    float y0TRAJ = oldPos() * getScale();            // Straight line equation between old and new point
+    float y1TRAJ = reqPos() * getScale();            // Time runs between 0 and lcncCycleTime (1 ms)
+    float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope
+    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;                                       //
+    if (/* 1.0 / Tstep */ kTRAJ > 200000)             //
+    {                                                 // Too high frequency, deal with this later.
+        err = 1;                                      //
+        return;                                       //
+    }                                                 //
+    int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in
+                                                      //
+    switch (abs(stepPosStart - oldStepPos()))         //
+    {                                                 //
+    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;                                   //
+        break;                                        //
+    case 1:                                           //
+                                                      // Let it slide through and deal with it after the case switch
+        break;                                        //
+    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;                                       //
+    // 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.
+}
+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?
+    {
+        timerIsRunning = 0;
+        MyTim->pause();
+    }
+#endif
+}
+
+uint32_t StepGen2::sync0CycleTime = 0;

Firmware/src/main.cpp

@@ -17,23 +17,22 @@ void indexPulseEncoderCB1(void)
 {
    Encoder1.indexPulse();
 }
-
+#if 0
 #include "StepGen.h"
-
 void timerCallbackStep1(void);
 StepGen Step1(TIM1, 4, PA_11, PA12, timerCallbackStep1);
 void timerCallbackStep1(void)
 {
    Step1.timerCB();
 }
-
 void timerCallbackStep2(void);
 StepGen Step2(TIM3, 4, PC_9, PC10, timerCallbackStep2);
 void timerCallbackStep2(void)
 {
    Step2.timerCB();
 }
-
+#endif
+#include "StepGen2.h"
 CircularBuffer<uint32_t, 200> Tim;
 volatile uint64_t nowTime = 0, thenTime = 0;
 
@@ -41,19 +40,22 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
 {
    Encoder1.setLatch(Obj.IndexLatchEnable);
    Encoder1.setScale(Obj.EncPosScale);
-
+#if 0
    Step1.reqPos(Obj.StepGenIn1.CommandedPosition);
    Step1.setScale(Obj.StepGenIn1.StepsPerMM);
    Step1.enable(Obj.Enable1);
    Step2.reqPos(Obj.StepGenIn2.CommandedPosition);
    Step2.setScale(Obj.StepGenIn2.StepsPerMM);
    Step2.enable(Obj.Enable1);
+#endif
 }
 
 void handleStepper(void)
 {
+#if 0
    Step1.handleStepper();
    Step2.handleStepper();
+#endif
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -62,10 +64,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    Obj.EncPos = Encoder1.currentPos();
    Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
    Obj.IndexByte = Encoder1.getIndexState();
-
+#if 0
    Obj.StepGenOut1.ActualPosition = Step1.actPos();
    Obj.StepGenOut2.ActualPosition = Step2.actPos();
-
+#endif
    uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
    Tim.push(dTim);
    uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
@@ -179,6 +181,9 @@ uint16_t dc_checker(void)
 {
    // Indicate we run DC
    ESCvar.dcsync = 1;
-   StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
+   #if 0
+   StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000;  // usecs
+   #endif
+   StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
    return 0;
 }