Last active
September 24, 2020 23:08
-
-
Save devyte/2b5d797663726095fbe616646ce3877d to your computer and use it in GitHub Desktop.
Stepper accel limiter
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
class AccelLimiter | |
{ | |
public: | |
int accel; // unit is Hz/s, i.e.: pulses/s2, always positive | |
int maxAccel; //this is the physical limitation. A frequency change above this value between consecutive updates is not allowed. Always positive. | |
AccelLimiter(int accel, int maxAccel) | |
: accel(accel), maxAccel(maxAccel) | |
{ | |
} | |
private: | |
unsigned long prevFreq = 0; | |
unsigned long prevTime = 0; | |
bool firstTime = false; | |
unsigned long targetFreq = 0; | |
public: | |
unsigned long calculateNewFreq() | |
{ | |
if(firstTime) | |
{ | |
prevTime = millis(); | |
firstTime = false; | |
return 0; | |
} | |
unsigned long now = millis(); | |
unsigned long deltaTime = now - prevTime; //should be about the same as the periodic update time | |
long deltaFreq = accel * deltaTime / 1000; //accel is Hz/s, deltaTime is in ms | |
deltaFreq = std::min(deltaFreq, maxAccel); //cap rate at maxAccel between updates, this ensures sticking to the limit if there is stretching of time between accel updates | |
long errorFreq = targetFreq - prevFreq; //This says how far away we are from the target speed (may be negative) | |
//cap the freq error: if target is closer than deltaFreq, then just change by the missing amount, because the entire delta would overshoot | |
if(errorFreq >= 0) | |
deltaFreq = std::min(errorFreq, deltaFreq); | |
else | |
deltaFreq = std::max(errorFreq, -deltaFreq); | |
long newFreq = prevFreq + deltaFreq; | |
newFreq = std::max(newFreq, 0L); //because paranoid | |
prevFreq = newFreq; | |
prevTime = now; | |
return newFreq; | |
} | |
void setTargetFreq(int newTarget) {targetFreq = newTarget;} | |
void reset() {prevFreq = 0; firstTime = true;} | |
bool isStopped() {return prevFreq == 0;} | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "AccelLimiter.h" | |
... | |
void setup() | |
{ | |
... | |
} | |
/* | |
Use should be along the lines of the below. | |
Need to check that setting the same speed again in the waveform gen has no negative impact. If it does, then StepperManager::setSpeed() should only set a new speed if different than current speed. | |
*/ | |
periodic timeToUpdate(100); | |
StepperManager s1(...); | |
StepperManager s2(...); | |
AccelLimiter a1(...); //args are: desired accel, maxAccel | |
AccelLimiter a2(...); | |
void stepperOverseer(StepperManager s, AccelLimiter a) | |
{ | |
//Beware: there is a min on the settable frequency for a stepper. The AccelLimiter is not aware of that. Going to freq == 0 means trying to set a speed below that min, which the waveform gen should ignore. | |
//To be explicit: AccelLimiter::isStopped() can be true, returned freq will be 0, but the steppers will continue to move when setting speed below the waveform gen min. | |
//The following check is meant to address that: if the accel shows freq dropped to 0, just stop the stepper. In between the min waveform gen freq and 0, the stepper should continue to move at min freq. | |
if(a.isStopped()) | |
{ | |
s.stop(); | |
} | |
else | |
{ | |
auto newFreq1 = a1.calculateNewFreq(); | |
s1.updateSpeed(newFreq1); | |
} | |
} | |
void loop() | |
{ | |
... | |
auto newTargetFreq1 = GetNewTargetFreq1FromSomewhere(); | |
auto newTargetFreq2 = GetNewTargetFreq2FromSomewhere(); | |
a1.setTargetFreq(newTargetFreq1); | |
a2.setTargetFreq(newTargetFreq2); | |
if(timeToUpdate) | |
{ | |
stepperOverseer(s1, a1); | |
stepperOverseer(s2, a2); | |
} | |
... | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
`#include "Arduino.h"
#include "steppermanager.h"
#include "AccelLimiter.h"
#include <PolledTimeout.h>
namespace
{
constexpr int pinEnS1 = 5;
constexpr int pinPulseS1 = 4;
constexpr int linkagePin = 14;
StepperManager stepper1(pinEnS1, pinPulseS1);
AccelLimiter a1(30, 300); //args are: desired accel, maxAccel
}
bool accelSwitch = true;
using periodic = esp8266::polledTimeout::periodicMs;
//periodic accelerator(1500);
periodic timeToUpdate(10);
void stepperOverseer(StepperManager s, AccelLimiter a)
{
if(a.isStopped())
{
s.stop();
}
else
{
auto newFreq1 = a1.calculateNewFreq();
stepper1.updateSpeed(newFreq1);
Serial.println(newFreq1);
}
}
void setup(){
Serial.begin(115200);
Serial.print("starting");
}
void loop(){
stepper1.handle();
auto newTargetFreq1 = (2000);
// auto newTargetFreq2 = GetNewTargetFreq2FromSomewhere();
a1.setTargetFreq(newTargetFreq1);
// a2.setTargetFreq(newTargetFreq2);
if(timeToUpdate)
{
stepperOverseer(stepper1, a1);
Serial.print("stepper speed ");
Serial.println(stepper1.getSpeed());
// stepperOverseer(s2, a2);
}
}`