Hello,
I am using a stepper motor with encoder to move a linear rail carriage back and forth 5mm. I am doing all control using a Teensy4.1 microcontroller. I orignianlly had this control working just fine when using the AccelSteper.h library and telling it to move(steps) and shift directions when the distanceToGo=0.
However, I have more recently been told I need to have a sinewave input to control the motion. In the code below, the function "LinearMotion()" attempts to do this. It works decently when I use an amplitude of 2.5; however, when I bump this up to the desired 5.0 mm amplitude it does not work (see attached figures). I am using moveTo() since it is giving me the appropriate +/-amplitude about zero.
If anyone has any suggestions about how to address this issue or improve the coding, I would greatly appreciate it!
Thanks,
Kate
And to note...I am switching up the way I previously coded the stepper motor because I am also trying to program a servo motor to go +/-10 deg at the same time as the linear motion. I was having phase shift problems between the two motor outputs with this which is why I am going the sine wave input route. The thought is if I use sinewave inputs for both stepper and servo motor, then I should be able to get rid of the phase shift during syncing.
#include <AccelStepper.h>
#include <math.h>
#include <ezButton.h>
#include <Encoder.h>
#include <Servo.h>
// Define stepper motor connections and steps per revolution
#define STEP_PIN 3
#define DIR_PIN 4
#define STEPS_PER_REVOLUTION 200
#define STEPS_PER_REV 200
#define MICROSTEPS_PER_STEP 16
#define pitch 5
#define Encoder_CPR 4000.00
int cycles = 10;
// Define the parameters for the sinusoidal input
float AMPLITUDE_MM = 5.0; // Amplitude of the sinusoidal input in millimeters
float FREQUENCY_HZ = 2.0; // Frequency of the sinusoidal input in Hz
// Create a new instance of the AccelStepper class
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
int A_PLUS = 41;
int B_PLUS = 40;
Encoder myEnc(A_PLUS,B_PLUS); // encoder setup
ezButton limitSwitch_Red(21); // limit switch WHITE
ezButton limitSwitch_Blue(22); // limit switch BLUE
// CALCULATED VARIABLES USED FOR LINEAR MOTION
float linear_hertz = 2.0; // Change to 2.0 if UNI, Change to 4.0 if MULTI
float speed_desired = 21.0; // mm per s --> Change to 21.0 if UNI, Change to 25.0 if MULTI
int stroke_mm = 5; // stroke in mm
float t0 = 0.015;
float rev_per_mm = 1.0 / pitch;
float steps_per_mm = rev_per_mm * (MICROSTEPS_PER_STEP)*STEPS_PER_REV;
int steps_per_mm_int = static_cast<int>(steps_per_mm); //640
float speed_steps = speed_desired * steps_per_mm; //steps per second
float milliseconds_per_cycle = ((stroke_mm * 2) / speed_desired) * 1000;
int stroke_steps = stroke_mm * steps_per_mm_int; // Stroke given in steps 3200
// CALCULATIONS USED FOR ACCELERATION
float period = 1.0/(2.0*linear_hertz);
float t1 = period - t0;
int accel_steps = (stroke_steps) / ((t0/(2*linear_hertz))-(t0*t0)); // adjusted on 9-11-23
// CALCULATIONS USED FOR STEPPER ENCODER
float encoder_offset = (STEPS_PER_REV * MICROSTEPS_PER_STEP) / Encoder_CPR;
// DEFINED VARIABLES
unsigned long currentTime = 0;
static unsigned long lastTime;
float movement_distance_mm;
float x = 0.0;
unsigned long startTime;
float newPosition_mm;
int newPosition_steps;
int movement_steps;
int y =0;
int freq_ms = 1000/(FREQUENCY_HZ*2);
// FUNCTIONS USED
void Limits();
void Troubleshoot_Data_Output();
void LinearMotion();
void setup() {
// Set up the stepper motor properties
stepper.setMaxSpeed(speed_steps); // steps/s
stepper.setAcceleration(accel_steps); // steps/s/s
// Limit switches
limitSwitch_Red.setDebounceTime(2);
limitSwitch_Blue.setDebounceTime(2);
stepper.setMinPulseWidth(25);
myEnc.write(0); // Sets encoder possition to 0
stepper.setCurrentPosition(0);
}
void loop() {
LinearMotion();
}
void LinearMotion(){
startTime = millis();
while (x <= cycles){
currentTime = millis();
stepper.run();
// Calculate the movement distance based on a sinusoidal inputs
movement_distance_mm = 5.0 * sin(2.0 * PI * (currentTime-startTime) * FREQUENCY_HZ / 1000.0); // Sine wave for stepper
// Convert the movement distance to steps
movement_steps = (movement_distance_mm) * steps_per_mm_int;
// Move the stepper motor by the calculated distance
stepper.moveTo(movement_steps);
// Run the stepper motor
stepper.run();
newPosition_steps = (myEnc.read()*encoder_offset)*-1.0;
newPosition_mm = (myEnc.read()*encoder_offset) / steps_per_mm_int *-1.0;
Troubleshoot_Data_Output();
Limits();
}
}
void Troubleshoot_Data_Output(){
unsigned long time_period = 25; // sampling rate of 25ms resulting in anticipated 20 position readings per cycle
if (millis() - lastTime >= time_period){
if (y % freq_ms == 0){ // Records the cycle time based on cycles!
x = x + 0.5;
}
Serial.print(millis()-startTime);
Serial.print("\t");
Serial.print(x);
Serial.print("\t");
Serial.print(movement_steps);
Serial.print("\t");
Serial.print(newPosition_steps);
Serial.print("\t");
Serial.print(movement_distance_mm);
Serial.print("\t");
Serial.println(newPosition_mm);
Limits();
lastTime = millis();
y = y + time_period;
}
}
