#include "Arduino.h"
#include "TeensyStep.h"
constexpr unsigned PID_Interval = 10; // ms
constexpr float P = 0.01; // (P)roportional constant of the regulator needs to be adjusted (depends on speed and acceleration setting)
Stepper stepper(0, 1);
RotateControl controller;
int32_t target = 0;
void pid()
{
static unsigned lastTick = 0;
if (millis() - lastTick > PID_Interval)
{
float delta = (target - stepper.getPosition()) * (P / PID_Interval); // This implements a simple P regulator (can be extended to a PID if necessary)
float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0
controller.overrideSpeed(factor); // set new speed
lastTick = 0;
}
}
void setup()
{
pinMode(LED_BUILTIN, OUTPUT);
while (!Serial && millis() < 1000);
stepper
.setMaxSpeed(10000)
.setAcceleration(75000);
controller.rotateAsync(stepper);
controller.overrideSpeed(0); // start with stopped slide
}
elapsedMillis retargetTimer = 0;
elapsedMillis outputTimer = 0;
void loop()
{
pid();
if(retargetTimer > 1000) // new random target every second
{
retargetTimer = 0;
target = random(-10000, 10000);
}
if (outputTimer > 20) // print position for display
{
outputTimer = 0;
Serial.printf("%d\t%d\t%d\n", millis(), stepper.getPosition(), target);
}
}