So I did a bit of digging and discovered this video: https://www.youtube.com/watch?v=fHAO7SW-SZI
I went ahead and pulled the code that I thought I would need. Thankfully for this stage of the project I don't need to actually run a stepper motor in realtime. Rather, I can just run the program and print out what should be the equivalent delayInMicroseconds() but in the form of an ISR so I'm not blocking.
Here are general steps I'm taking in this program:
-
Read an SD card, pull the data and sort it into a 2D array, where the columns are each AXIS and the rows are each KEYFRAME position (either mm or deg)
-
I have a method to go through the 2d array and multiply each value by a specified coefficient to convert from mm or deg to steps. Each coefficient per axis relates to the mechanical pulley staging and such. The track is the only axis that is linear so it needed slightly different calcs. How I calculated each axes coefficient is commented on below.
-
I'm also setting the camera's exposure time. If I wanted a 1-second exposure per frame, an additional second is added to the variable to account for the image save time. So in actuality, the exposure time would technically be 2 in this case.
-
I then have a method that iterates through each element of the 2D array, where a specific element is a step #. I'm taking the delta between the next step and the current. I feed this delta into the prepareMovement() function that also takes in the specific stepper motor axis. The feedrate is simply calculated through: delta / EXPOSURE_TIME. (adjustSpeed() isn't quite working yet, but all values result in 1 so it won't affect calculations)
-
Here is where I think I am getting confused. I have a function called setNextInterruptInterval() that iterates through each stepper axis and setting a local variable called delay to the ith stepper's personal delay value. I recognize that myTimer.begin(ISR, delay); would only run the last steppers delay instead of each individual one, but I am unsure how to order the function so each stepper has their appropriate delay set on a timer.
-
ISR() basically iterates the stepCount and checks if it has reached the totalStep count. If it has, it'll update the stepper motor's internal bool and the byte stepper flag. Regardless, the method checks if the feedrate isn't 0 (so I don't divide by 0) and sets a specific stepper motor's stepperDelay to the calculation you see in the code. Otherwise, it'll be set to 0.
In my runMoco() method I'm briefly noInterrupting() and Interupting() to grab a specific stepper motor's stepper delay value within the ISR and printing it for debug purposes.
At the moment, stepperDelay for all axes is always printing 0. I did check feedrate, totalSteps, and speedScale. All of those are thankfully real values so I'm not dividing or multiplying by 0 anywhere.
The issue is probably in setNextInterruptInterval(), but I'm a little confused about how to resolve the issue. It seems I'm only beginning the timer with the last stepper motor's delay value at the end of the method, but I know I shouldn't put the myTimer.begin within the for loop. So I'm unsure how to approach fixing this problem.
I'll do a bit more digging into ISRs to get a better understanding, but I thought i'd pose the question provided I have code to show now.
#include <SPI.h>
#include <SD.h>
#include "IntervalTimer.h"
File myFile;
const int chipSelect = BUILTIN_SDCARD;
IntervalTimer myTimer;
//TRACK
#define X_DIR_PIN 55
#define X_STEP_PIN 54
#define X_ENABLE_PIN 38
#define X_STEP_COEF 64.35
// 1 divided by 0.01554 = 64.35 STEPS PER MM.
// 46.62mm (circ of pulley from teeth) divided by 3000 steps == 0.01554 mm per step
// (default for Nema23 15:1 is 360deg divided by 0.12deg/step == 3000 steps)
//PAN
#define Y_DIR_PIN 61
#define Y_STEP_PIN 60
#define Y_ENABLE_PIN 56
#define Y_STEP_COEF 8.33
// 1 divided by 0.12 = 8.33 step per deg
// 360deg divided by 0.12deg/step == 3000 steps)
// 1.8deg / 15:1 reduction = 0.12deg/step
// The reduction of the mech adds another 3:1. 3:1 x 5:1 = 15:1 reduction
// (default for Nema17 5:1. 1.8deg divided by 5 = 0.36 deg/step)
//TILT
#define Z_DIR_PIN 48
#define Z_STEP_PIN 46
#define Z_ENABLE_PIN 62
#define Z_STEP_COEF 8.33
// 1 divided by 0.12 = 8.33 step per deg
// 360deg divided by 0.12deg/step == 3000 steps
// 1.8deg / 15:1 reduction = 0.12deg/step
// The reduction of the mech adds another 3:1. 3:1 x 5:1 = 15:1 reduction
// (default for Nema17 5:1. 1.8deg divided by 5 = 0.36 deg/step)
//YAW
#define A_DIR_PIN 28
#define A_STEP_PIN 26
#define A_ENABLE_PIN 24
#define A_STEP_COEF 35.7
// 1 divided by 0.02799377916 = 35.7 step per deg
// 360deg divided by 0.02799377916 = 12860 steps
// 1.8deg / 64.3:1 reduction = 0.02799377916 deg/step
// The reduction of the mech adds another 4.3:1. 4.3:1 x 15:1 = 64.3:1 reduction
// (default for Nema23 51:1. 1.8deg divided by 15 = 0.12 deg/step)
//ROLL (CARRIAGE)
#define B_DIR_PIN 34
#define B_STEP_PIN 36
#define B_ENABLE_PIN 30
#define B_STEP_COEF 6.83
// theta = (56.65*180) / (475*pi) = 6.83 step per deg
// R = 475mm
// theta = (L*180) / (pi*R) (this converts from radians to degrees as well)
// Assume length is an arclength, figure out theta
// 1 divided by 0.01765333333 = 56.65 STEPS PER MM.
// 53mm (circ of pulley from teeth) divided by 3000 steps == 0.01765333333 mm per step
// (default for Nema23 15:1 is 360deg divided by 0.12deg/step == 3000 steps)
//PITCH
#define C_DIR_PIN 32
#define C_STEP_PIN 47
#define C_ENABLE_PIN 45
#define C_STEP_COEF 75
// 1 divided by 0.01333333333 = 75 step per deg
// 360deg / 0.01333333333 = 27000 steps
// 1.8deg / 135:1 reduction = 0.01333333333 deg/step
// The reduction of the mech adds another 27:1. 27:1 x 5:1 = 135:1 reduction
// (default for Nema17 5:1. 1.8deg divided by 5 = 0.36 deg/step)
#define X_STEP_HIGH digitalWrite(X_STEP_PIN, HIGH);
#define X_STEP_LOW digitalWrite(X_STEP_PIN, LOW);
#define Y_STEP_HIGH digitalWrite(Y_STEP_PIN, HIGH);
#define Y_STEP_LOW digitalWrite(Y_STEP_PIN, LOW);
#define Z_STEP_HIGH digitalWrite(Z_STEP_PIN, HIGH);
#define Z_STEP_LOW digitalWrite(Z_STEP_PIN, LOW);
#define A_STEP_HIGH digitalWrite(A_STEP_PIN, HIGH);
#define A_STEP_LOW digitalWrite(A_STEP_PIN, LOW);
#define B_STEP_HIGH digitalWrite(B_STEP_PIN, HIGH);
#define B_STEP_LOW digitalWrite(B_STEP_PIN, LOW);
#define C_STEP_HIGH digitalWrite(C_STEP_PIN, HIGH);
#define C_STEP_LOW digitalWrite(C_STEP_PIN, LOW);
struct stepperInfo {
// externally defined parameters
void (*dirFunc)(int);
void (*stepFunc)(unsigned int);
// derived parameters
long stepPosition; // current position of stepper (total of all movements taken so far)
// per movement variables (only changed once per movement)
volatile int dir; // current direction of movement, used to keep track of position
volatile float totalSteps; // number of steps requested for current movement
volatile bool movementDone = false; // true if the current movement has been completed (used by main program to wait for completion)
volatile float speedScale; // used to slow down this motor to make coordinated movement with other motors
volatile float feedrate = 0; // feedrate of stepper (mm/deg divided by time to get to position)
volatile float stepsPerMM = 1; //fix value unique coefficient per stepper motor since each reduction will be different
volatile float stepsPerDEG = 1; //fix value unique coefficient per stepper motor since each reduction will be different
// per iteration variables (potentially changed every interrupt)
volatile float stepCount; // number of steps completed in current movement
volatile unsigned int stepperDelay;
};
void xStep(unsigned int microSecondDelay) {
X_STEP_HIGH
delayMicroseconds(microSecondDelay);
X_STEP_LOW
}
void xDir(int dir) {
digitalWrite(X_DIR_PIN, dir);
}
void yStep(unsigned int microSecondDelay) {
Y_STEP_HIGH
delayMicroseconds(microSecondDelay);
Y_STEP_LOW
}
void yDir(int dir) {
digitalWrite(Y_DIR_PIN, dir);
}
void zStep(unsigned int microSecondDelay) {
Z_STEP_HIGH
delayMicroseconds(microSecondDelay);
Z_STEP_LOW
}
void zDir(int dir) {
digitalWrite(Z_DIR_PIN, dir);
}
void aStep(unsigned int microSecondDelay) {
A_STEP_HIGH
delayMicroseconds(microSecondDelay);
A_STEP_LOW
}
void aDir(int dir) {
digitalWrite(A_DIR_PIN, dir);
}
void bStep(unsigned int microSecondDelay){
B_STEP_HIGH
delayMicroseconds(microSecondDelay);
B_STEP_LOW
}
void bDir(int dir) {
digitalWrite(B_DIR_PIN, dir);
}
void cStep(unsigned int microSecondDelay) {
C_STEP_HIGH
delayMicroseconds(microSecondDelay);
C_STEP_LOW
}
void cDir(int dir) {
digitalWrite(C_DIR_PIN, dir);
}
#define NUM_STEPPERS 6
#define NUM_KEYFRAMES 165 //change per file. Find some way to calculate text file row # from import
float kuper[NUM_KEYFRAMES][NUM_STEPPERS];
volatile stepperInfo steppers[NUM_STEPPERS];
int EXPOSURE_TIME = 1; //default to 1 second exposure but query to exposure time + 1 second for saving image
void setup() {
pinMode(X_STEP_PIN, OUTPUT);
pinMode(X_DIR_PIN, OUTPUT);
pinMode(X_ENABLE_PIN, OUTPUT);
pinMode(Y_STEP_PIN, OUTPUT);
pinMode(Y_DIR_PIN, OUTPUT);
pinMode(Y_ENABLE_PIN, OUTPUT);
pinMode(Z_STEP_PIN, OUTPUT);
pinMode(Z_DIR_PIN, OUTPUT);
pinMode(Z_ENABLE_PIN, OUTPUT);
pinMode(A_STEP_PIN, OUTPUT);
pinMode(A_DIR_PIN, OUTPUT);
pinMode(A_ENABLE_PIN, OUTPUT);
pinMode(B_STEP_PIN, OUTPUT);
pinMode(B_DIR_PIN, OUTPUT);
pinMode(B_ENABLE_PIN, OUTPUT);
pinMode(C_STEP_PIN, OUTPUT);
pinMode(C_DIR_PIN, OUTPUT);
pinMode(C_ENABLE_PIN, OUTPUT);
//PROGRAM CURRENTLY HAS IT SET TO ALWAYS HAVE THE STEPPER MOTORS ON
//EVENTUALLY, AN EMERGENCY STOP WILL BE ADDED TO ENABLE THEM HIGH
digitalWrite(X_ENABLE_PIN, LOW);
digitalWrite(Y_ENABLE_PIN, LOW);
digitalWrite(Z_ENABLE_PIN, LOW);
digitalWrite(A_ENABLE_PIN, LOW);
digitalWrite(B_ENABLE_PIN, LOW);
digitalWrite(C_ENABLE_PIN, LOW);
steppers[0].dirFunc = yDir;
steppers[0].stepFunc = yStep;
steppers[0].stepsPerDEG = Y_STEP_COEF;
steppers[1].dirFunc = zDir;
steppers[1].stepFunc = zStep;
steppers[1].stepsPerDEG = Z_STEP_COEF;
steppers[2].dirFunc = xDir;
steppers[2].stepFunc = xStep;
steppers[2].stepsPerMM = X_STEP_COEF;
steppers[3].dirFunc = aDir;
steppers[3].stepFunc = aStep;
steppers[3].stepsPerDEG = A_STEP_COEF; //UPDATE THIS
steppers[4].dirFunc = bDir;
steppers[4].stepFunc = bStep;
steppers[4].stepsPerDEG = B_STEP_COEF; //UPDATE THIS
steppers[5].dirFunc = cDir;
steppers[5].stepFunc = cStep;
steppers[5].stepsPerDEG = C_STEP_COEF; //UPDATE THIS
Serial.begin(9600);
while (!Serial) {
; // wait for serial port to connect.
}
Serial.print("Initializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println("initialization failed!");
return;
}
Serial.println("initialization done.");
// open the file.
setExposureTime(1.0); //1 second exposure + 1 second save
queryKuper();
convertKuperToSteps();
runMoco();
}
//Will be implemented at some point
void setExposureTime(unsigned int exposureTime){
EXPOSURE_TIME = EXPOSURE_TIME + exposureTime; //1 second extra for saving time
}
void queryKuper(){
myFile = SD.open("kuperSH1.txt", FILE_READ);
// re-open the file for reading:
myFile = SD.open("kuperSH1.txt");
if (myFile) {
//This culls the text file and propagates the data into a 2D array
String header = "";
header = myFile.readStringUntil('\n'); //clear header first
Serial.println(header);
String curNum = "";
curNum = myFile.readStringUntil(' '); //clear out "ghost" 0 at beginning
for(int i = 0; i < NUM_KEYFRAMES; i++){
for(int j = 0; j < NUM_STEPPERS; j++){
curNum = myFile.readStringUntil(' ');
kuper[i][j] = curNum.toFloat();
}
}
//print all kuper data
// for(int i = 0; i < NUM_KEYFRAMES; i++){
// for(int j = 0; j < NUM_STEPPERS; j++){
// Serial.println(kuper[i][j], 4);
// }
// }
myFile.close();
}
else {
// if the file didn't open, print an error:
Serial.println("error opening test.txt");
}
}
//Convert kuper files mm and deg values in terms of steps
void convertKuperToSteps(){
int metersToMM = 1000; //mm
for(int i = 0; i < NUM_KEYFRAMES; i++){
for(int j = 0; j < NUM_STEPPERS; j++) {
volatile stepperInfo& s = steppers[j];
if(j == 2) // 2 is Track
kuper[i][j] *= (s.stepsPerMM*metersToMM); // stepsPerMM is a bit misleading since Modo exports the track units as Meters. Conversion is needed to MMs
else
kuper[i][j] *= s.stepsPerDEG;//*(360/3.14);
}
}
//DEBUG print altered kuper array
// for(int i = 0; i < NUM_KEYFRAMES; i++){
// for(int j = 0; j < NUM_STEPPERS; j++) {
// Serial.println(kuper[i][j], 4);
// }
// }
}
void resetStepper(volatile stepperInfo& si) {
si.stepCount = 0;
si.movementDone = false;
si.speedScale = 1;
//si.stepperDelay = 0;
//maybe set si.stepperDelay to previous so it doesn't start at 0 every time
}
volatile byte remainingSteppersFlag = 0;
// steps will be the difference between the next - current
// could be pos or neg direction
void prepareMovement(int whichMotor, float steps) {
volatile stepperInfo& si = steppers[whichMotor];
si.dirFunc( steps < 0 ? HIGH : LOW );
si.dir = steps > 0 ? 1 : -1;
si.totalSteps = abs(steps); //reset total # of steps for movement for each keyframe interval
resetStepper(si); //reset so you recalculate speedScale and set stepCount to zero
si.feedrate = steps / EXPOSURE_TIME; //feedrate of each stepper motor for keyframe(i) in steps/sec
remainingSteppersFlag |= (1 << whichMotor);
Serial.println("Prepared Movement");
//DEBUG print feedrate
//String feedrate = String(si.feedrate, 4);
//Serial.println("Stepper " + String(whichMotor) + " feedrate: " + feedrate);
}
void adjustSpeedScales() {
float maxSpeed = 0;
//SOLUTION: NEVER ACTUALLY DIVIDE BY 0. Check if value is 0 beforehand. If not, continue, otherwise, just set result to 0
//iterate through all steppers and grab the max speed from a specific stepper
for (int i = 0; i < NUM_STEPPERS; i++) {
if (steppers[i].feedrate > maxSpeed)
maxSpeed = steppers[i].feedrate;
}
//update each stepper's speed scale so all other steppers are slaved to "master" stepper
//ensures all steppers finish at same time
for (int i = 0; i < NUM_STEPPERS; i++) {
if(maxSpeed != 0 && steppers[i].feedrate != 0) steppers[i].speedScale = maxSpeed / steppers[i].feedrate;
else steppers[i].speedScale = 0;
//DEBUG print Speedscale
//Serial.println("Speedscale for Axis " + String(i) + ": " + String(steppers[i].speedScale, 4));
}
Serial.println("Adjusted Speedscales");
}
volatile byte nextStepperFlag = 0;
void setNextInterruptInterval() {
bool movementComplete = true;
unsigned int delay = 0;
for (int i = 0; i < NUM_STEPPERS; i++) {
if ( ((1 << i) & remainingSteppersFlag)) {
delay = steppers[i].stepperDelay;
}
}
nextStepperFlag = 0;
for (int i = 0; i < NUM_STEPPERS; i++) {
if(!steppers[i].movementDone)
movementComplete = false;
if (((1 << i) & remainingSteppersFlag))
nextStepperFlag |= (1 << i);
}
if(remainingSteppersFlag == 0) {
myTimer.end();
}
myTimer.begin(ISR, delay);
//starts up ISR for specific motor axis at calculated feedrate
}
void ISR(){
for (int i = 0; i < NUM_STEPPERS; i++) {
volatile stepperInfo& s = steppers[i];
unsigned int mind = 999999; //value for # of microseconds in a second
if(s.stepCount < s.totalSteps) {
//disabling temporarily to do simulation
//s.stepFunc(stepperDelay); //triggers step pulse
s.stepCount++; //updates step count
s.stepPosition += s.dir; //updates step position
if (s.stepCount >= s.totalSteps ) { //if movement is done
s.movementDone = true;
remainingSteppersFlag &= ~(1 << i);
}
}
if(s.feedrate != 0) s.stepperDelay = (s.totalSteps / (s.feedrate*s.speedScale))*mind;
else s.stepperDelay = 0; //In microseconds. THIS IS ALSO DI IN THE OTHER CODE
}
setNextInterruptInterval();
}
void runAndWait() {
adjustSpeedScales();
setNextInterruptInterval();
//while(remainingSteppersFlag);
remainingSteppersFlag = 0;
nextStepperFlag = 0;
}
void runMoco(){
for(int i = 0; i < NUM_KEYFRAMES; i++){
for(int j = 0; j < NUM_STEPPERS; j++) {
float delta = kuper[i+1][j] - kuper[i][j]; //takes difference between current and next in STEPS
//DEBUG printing delta
// String stringDelta = String(delta, 4);
// Serial.println("Delta: " + stringDelta);
prepareMovement(j, delta); //input: currStepper, delta distance
//DEBUG. print stepper delay value
noInterrupts();
String mystring = String(steppers[j].stepperDelay);
interrupts();
Serial.println("Stepper delay: " + mystring);
runAndWait(); //wait for all axes to finish keyframe(n) to keyframe(n+1) movement
}
}
}
void loop() {
}