diff --git a/Firmware/.vscode/settings.json b/Firmware/.vscode/settings.json index 2fe9d6a..912692f 100755 --- a/Firmware/.vscode/settings.json +++ b/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 diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h new file mode 100755 index 0000000..cf44a59 --- /dev/null +++ b/Firmware/include/StepGen2.h @@ -0,0 +1,53 @@ + +#ifndef STEPGEN +#define STEPGEN +#include + +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 \ No newline at end of file diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp new file mode 100755 index 0000000..162eb4b --- /dev/null +++ b/Firmware/src/StepGen2.cpp @@ -0,0 +1,114 @@ +#include +#include +#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; diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 4106e83..2ce8068 100755 --- a/Firmware/src/main.cpp +++ b/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 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; }