Writing motion control software for stepper motors

Hi there.

I'm in the planning stages of my motion control project and I was hoping to get some clarification on where to start.

I have a motion control system with Pan-Tilt head along a linear dolly track. I have the 3D model of the system recreated in Modo with a pre-animated motion path. I'm pulling the keyframes for each axis and outputting them in an axis/position table in a text file.

In a specific example, I have a shot that needs to traverse 3 meters along the track for a 5 second shot. All axes start and end at the same time, but have varying positions at each keyframe. Fortunately, this form of videography is essentially a timelapse, so each frame needs to be exposed for 2 seconds.

As such: 5 second shot * 24 (frames/second) * 2 second exposures = 240seconds = 4 minutes

Furthermore, I have 120 unique positions to traverse per axis.

So in essence, I need to have these stepper motors start and end within a 4 minute period with varying positions between each keyframe. The issue is I can't simply plot linear paths between each frame. Rather, I need to calculate the curves to allow the stepper motors to accelerate and decelerate for smooth motion.

From what I have gathered this is a coordinated stepper motor motion planner. I'm just unsure how to approach programming this. It seems that Accelstepper library wouldn't work since its based on # of steps and acceleration rate, but not time. While I could coordinate the stepper motors with the MultiStepper library, it doesn't handle acceleration.

I guess I am essentially asking how to write a software package that is the equivalent to running a 3D printer/CNC machine, but under the lens of keyframing animation within a specific period.

I recognize the question I posed is large in scope. I was just wondering if anyone knows of small-scale starting points to tackle a program like this?

Are there specific libraries that people are aware of that does this sort of thing? Thank you for your time!

Are there specific libraries

What has your research turned up to date?

The chances that someone has written an open source library that does you want is essentially zero, which means that for anything you do find, you will have to understand almost all of what it does before you can add new functions.

Most likely, it would be easier to start from scratch.

That was the impression I got too. I guess the question about "specific libraries" was less about getting exactly what I want from the get-go, but rather bits or pieces of software that might aid me.

I have some doubts about your need.

  1. Would it be just one motor or are there 3 motors(3D)?
  2. How is the movement time of the motor(s) determined?
    Could you post a "image sketch" of your project.
    Images say "easier".

don't understand why you say you need different accelerations. Acceleration is ft/sec/sec, how quickly speed changes

don't you want the each motor to reach a specified position at a specified time regardless of the # of steps. These intermediate positions define a curved path. isn't this what a 3d printer does?

not clear if you want the (2 sec) exposure to occur as the motors move to the next intermediate point or while at that point? will motor vibration affect the exposure?

  1. There are actually 7 total stepper motors. I just mentioned the three in the OG post to not go too in depth of the setup.

  2. I'd be happy to. Here is a video showing the movement of the system. Again, just to clarify, the real-time movement is in the form of a timelapse, so all the stepper motors should be moving very slowly. This whole animation would happen in the span of 4 minutes.

https://imgur.com/a/ndX1fhR

Yeh you're right. Utter brain fart on my part. So I guess I can just plot straight paths between each point given the distances will be pretty small right?

The two-second exposure per frame can run separately. Motor vibration is not too much of a concern. If there will be any vibration, it will be really subtle and can be stabilized in after effects pretty easily.

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:

  1. 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)

  2. 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.

  3. 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.

  4. 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)

  5. 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.

  6. 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() {

}

I suppose it would also be helpful if I supplied an example text file of the keyframes. I should also mention for claritication that at this point in the programming process, I've stripped the acceleration code and am primarily focusing on feedrates.
kuperSH1.txt (5.9 KB)

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.