#include "Arduino.h"
#include "TeensyStep.h"
constexpr unsigned motorStpPerRev = 200 * 16; // e.g. fullstep/rev * microstepping
constexpr unsigned motorAcceleration = 15000;
float targetMotorSpeed, oldMotorSpeed;
int motorTargetPosition = 0;
int motorTargetDirection = 2; // 0 == up 1 == down 2 == none
int prevMotorPosition = 0;
constexpr unsigned maxSpeed = 1000;
RotateControl controller;
Stepper motor(4, 5);
void setup() {
Serial.begin(9600);
while(!Serial) {
;
}
motor.setMaxSpeed(1); // -> parameter of overrideSpeed equals real speed
motor.setAcceleration(motorAcceleration);
// startup controller
controller.rotateAsync(motor);
controller.overrideSpeed(0.0f);
// setMotorTarget(-4000);
}
void setMotorTarget(int t) {
motorTargetPosition = t;
if(t > motor.getPosition()) {
motorTargetDirection = 0;
} else if(t < motor.getPosition()){
motorTargetDirection = 1;
} else {
motorTargetDirection = 2; //none
}
}
void loop() {
int p = motor.getPosition();
int pp = prevMotorPosition;
int t = motorTargetPosition;
if(motorTargetDirection == 0) {
// we go up
setMotorSpeed(maxSpeed);//up
if(p > t) motorTargetDirection = 2;
} else if(motorTargetDirection == 1){
// we go down
setMotorSpeed(-maxSpeed);//down
if(p < t) motorTargetDirection = 2;
} else {
// nowhere
setMotorSpeed(0);
}
prevMotorPosition = p;
updateSpeeds();
int sinT = sin(millis()*0.0001)*9000; // send it a changing target (sine wave for testing)
setMotorTarget(sinT);
}
void setMotorSpeed(float rpm)
{
targetMotorSpeed = rpm / 60.0f * motorStpPerRev;
}
void updateSpeeds()
{
if(targetMotorSpeed != oldMotorSpeed) // if target speed changed -> update all
{
oldMotorSpeed = targetMotorSpeed;
controller.overrideSpeed(targetMotorSpeed);
}
}
inline int getCurMotorSpeed() { return controller.isRunning() ? controller.getCurrentSpeed() : 0; }