Is there a better way to increment through each step/phase of a program instead of using if statements? (AccelStepper example)

Projects Programming
1

Hi everyone! I've been doing a lot of experimenting with the AccelStepper library, and I'm finally at a point where I can pretty reliably control my motors and tell them where to go.

The problem: If I want to program a sequence of movements, it is very cumbersome to write the code.

For example, if I want motorX to move to a position, when it's done, then motorY move to a position, then when it's done, do something else, etc....

I have to do something like

int phase = 0;

if(phase == 0)
{
//motor X do your thing
//when motorX is done...increment the phase
phase++;
}
else if(phase == 1)
{
//motor Y do your thing
//maybe activate a relay or turn on an LED light
//when motorY is done...increment the phase
 phase++;
}
else if(phase == 2)
{
//do something else
//when you're done, increment the phase
phase++;
}

The above works fine (actual code example below). However,

  • It is annoying to keep writing "else if(phase == 3), else if(phase == 4)....etc.
  • If I later decide I want to edit my program and add a few steps somewhere in between, I have to re-number all the phases because I have changed how many phases there are!

The Question: is there a more efficient way to write this code and increment through phases/steps, so that once a particular phase is done, the code knows not to re-execute the already executed commands?

Code example:

//Read the potentiometer, map it to a program value between 1 and 5, and run that program using the Switch statement in the loop

//------PINOUT CONFIGURATION---------------

#define MOTOR_X_STEP_PIN 2 //2 on CNC shield v3
#define MOTOR_X_DIR_PIN 5  //5 on CNC shield v3
#define MOTOR_Y_STEP_PIN 3 //3 on CNC shield v3
#define MOTOR_Y_DIR_PIN 6 //6 on CNC shield v3
#define MOTOR_Z_STEP_PIN 4 //4 on CNC shield v3
#define MOTOR_Z_DIR_PIN 7 //7 on CNC shield v3

#define ENABLE_PIN 8 //enable pin to enable CNC shield v3 motors

int potentiometerPin = A0;

//-----STEPPER MOTOR SETUP-----

  #include <AccelStepper.h>
    
  AccelStepper motorX(1,MOTOR_X_STEP_PIN,MOTOR_X_DIR_PIN); // first argument "1" sets this as a "DRIVER", second argument is "step pin", third is "direction pin"
  AccelStepper motorY(1,MOTOR_Y_STEP_PIN,MOTOR_Y_DIR_PIN);   
  AccelStepper motorZ(1,MOTOR_Z_STEP_PIN,MOTOR_Z_DIR_PIN); 
  

  int MAX_SPEED_X = 4000; 
  int MAX_SPEED_Y = 4000; 
  int MAX_SPEED_Z = 4000; 

  #define ACCELERATION_X 10000
  #define ACCELERATION_Y 10000
  #define ACCELERATION_Z 10000 

//-----Custom Programs/Functions-----

int phase = 0; //variable to check what phase we are in the overall program, so we can tell the program to move to the next phase as each motor moves into its target position

bool allMovesComplete() //check if all motors have moved to where they should be
  {      
    
    if(motorX.distanceToGo()== 0 && motorY.distanceToGo() == 0 && motorZ.distanceToGo() == 0) 
    {
      return true;      
    }
    else
    {
      return false;
      
    }
  }

int lastProgram = 0;
int program = 0; //
//program 1 = move the X motor to the right, then move X back to its original position
//program 2 = move X to the right, then move Y forward, then move Z down
//I can have as many programs as I want in the switch statement in the loop function


void setup() {
  
//Debugging (optional)
  Serial.begin(115200);

  //Stepper Motors Setup  
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW); // enable "enable" pin

  motorX.setMaxSpeed(MAX_SPEED_X);
  motorX.setAcceleration(ACCELERATION_X);
  
  motorY.setMaxSpeed(MAX_SPEED_Y);
  motorY.setAcceleration(ACCELERATION_Y);
  
  motorZ.setMaxSpeed(MAX_SPEED_Z);
  motorZ.setAcceleration(ACCELERATION_Z);
  

  motorX.setSpeed(MAX_SPEED_X);
  motorY.setSpeed(MAX_SPEED_Y);  
  motorZ.setSpeed(MAX_SPEED_Z);

  motorX.setCurrentPosition(0);
  motorY.setCurrentPosition(0);
  motorZ.setCurrentPosition(0); 

}

void loop() {

    program = map(analogRead(potentiometerPin),0,1023,1,5); //map the potentiometer value between programs 1 and 5 so that I can use my potentiometer to tell the software which program to run in the switch statement below

    if(lastProgram != program) //if you change the potentiometer value and therefore the program changes, reset the phase back to 0 so the selected program starts from the beginning
      {
        lastProgram = program;
        
        //Reset the phase so you can start at the beginning of another program
        phase = 0;
    
        //print the current program number in the Serial monitor for informational purposes
        Serial.print("Program: ");
        Serial.println(program);    
      }
    
    switch(program)
    {
      case 1: //move the X motor to the right, then move X back to its original position

              if(phase == 0)
              {
                motorX.moveTo(1000);
                if(allMovesComplete())
                {
                  phase++;
                }
              }
              else if(phase == 1)
              {
                motorX.moveTo(0);
                if(allMovesComplete())
                {
                  phase++;
                }
              }

              motorX.run();
              
              break;


      case 2: //move X to the right, then move Y forward, then move Z down

              if(phase == 0)
              {
                motorX.moveTo(1000);
                if(allMovesComplete())
                {
                  phase++;
                }
              }
              else if(phase == 1)
              {
                motorY.moveTo(500);
                if(allMovesComplete())
                {
                  phase++;
                } 
              }
              else if(phase == 2)
              {
                motorZ.moveTo(-500);
                if(allMovesComplete())
                {
                  phase++;
                }
              }

              motorX.run();
              motorY.run();
              motorZ.run();
              break;

       case 3: //move all three motors to a location, and once they have all arrived, move them somewhere else
              if(phase == 0)
              {
                motorX.moveTo(1000);
                motorY.moveTo(500);
                motorZ.moveTo(-50);
                if(allMovesComplete())
                {
                  phase++;
                }
              }
              else if(phase == 1)
              {
                delay(1000); // wait 1 second before doing moving to the next phase
              }
              else if(phase == 2)
              {
                motorX.moveTo(0);
                motorY.moveTo(0);
                motorZ.moveTo(0);
                if(allMovesComplete())
                {
                  phase++;
                }
              }

              motorX.run();
              motorY.run();
              motorZ.run();
              break;

              
       case 4:
              //room for another program

              break;

              
       case 5:
              //room for another program

              break;
      
            
    } //end of switch
  

} // end of main loop

Side note 1: moving motors may not be the only thing I'm doing in each "phase", maybe I want to turn on a relay, turn on an LED light, etc...so I need to be able to run several lines of code in each "phase".

Side note 2: I am intentionally NOT using any sort of blocking moves. (e.g. the AccelStepper runToNewPosition() function). For functional and for safety reasons, I should be able to perform other commands and/or change the program value at any time. If that wasn't a constraint, then it would be super simple to write something like:

case 2:
        motorX.runToNewPosition(1000);
        motorY.runToNewPosition(500);
        motorZ.runToNewPosition(-500);
        break;

..but unfortunately the above statements will block all other functions (since they cause each motor to run in a "while" loop), which is not acceptable for my application.

Any advice is appreciated! :slight_smile:

2

Somewhat similar but different:

An enum describing the state and a switch case

3

Perhaps you could encapsulate the axis moves in a subroutine, passing the values.

int moveMotors(int X, int Y, int Z) {
  if(X) {motorX.moveTo(X);}
  if(Y) {motorY.moveTo(Y);}
  if(Z) {motorZ.moveTo(Z);}
return(allMovesComplete());
}

Now you can build an array like
int moves[4][3];
and populate it with values, and walk through it using a for() loop.
Another advantage of such a construction is if you ever decide to add something like a G-code interpreter, you need only replace the table reads with the interpreted values.

  • Wes
1 Like
4

Thank you @weshowe, that's a cool idea!

If I understand correctly, what you're saying is I could do something like:

// 4 phases of moves, each phase will set the coordinates for 3 motors: X, Y, Z
int moves[4][3] = 
{
   {1000, 500, -500}, // in the first phase, go to coordinates X = 1000, Y = 500, Z = -500
   {0, 0, 0}, //in the second phase, go to coordinates X = 0, Y = 0, Z = 0
   {150, 150, -150}, //and so forth
   {0, 0, 0}, //and so forth
};


for(int i =0; i < 4; i++)
{
  while(!moveMotors(moves[i][0],moves[i][1],moves[i][2])) //issue the moveMotors command and stay in this loop until allMovesComplete == true
  {
  motorX.run();
  motorY.run();
  motorZ.run();
  }
}

That does seem to simplify the code for sequences of motor moves, but if I also want to do things in between the motor moves, such as:

  1. move XYZ motor to a given position
  2. once they've reached their target position, turn on an LED
  3. move the motors XYZ to a new position

Then, it seems the for() loop will start to get messy, and the original method I posted will be easier to read chronologically (but be lengthy to code):

if(phase == 0)
{
  //move XYZ to a target position
  if(!moveMotors(1000,500,-500)) //initiate a move command using your simpler moveMotors function
  {
  phase++; //increment to the next phase if the moveMotors command returns true (meaning all motors reached their destination)
  }
}
else if(phase == 1)
{
  digitalWrite(LED_BUILTIN,HIGH); //turn on the LED
  phase++; //increment to the next phase
}
else if(phase == 2)
{
  //move XYZ to a target position
  if(!moveMotors(0,0,0)) 
  {
  phase++; //increment to the next step
  }
}

So in the example above, using a more consolidated function definitely reduces a bunch of coding (thank you for that!), but I feel like I'm still stuck trying to use some sort of if() or for() statement to keep track and increment through each phase/step.

Is that really what I have to do?

5

The 'switch/case' statement is specifically designed to be a shortcut for:

if (intVariable == 1)
{
}
else if (intVariable == 2)
{
}
else if (intVariable == 3)
{
}
else if (intVariable == anyIntegerConstant)
{
}
else
{
// default
}

The short version is:

switch (intVariable)
{
case 1:
  break;

case 2:
  break;

case 3:
  break;

case anyIntegerConstant:
  break

default:
  // default
  break;
}
1 Like
6

Perhaps I originally misinterpreted your needs and suggested 0 value as a "no move" instead of a valid destination. There is no reason the table has to be limited to 3 "columns", you can add things like a flag that would be bitmapped for each axis and phase and an LED change flag.
So it could be

#define XMOVES 0x0001
#define YMOVES 0x0002
#define ZMOVES 0x0004
#define LEDGOESON 0x0008
#define LEDGOESOFF 0x0010

int moves[4][4] = 
{
   {1000, 500, -500, XMOVES|YMOVES|ZMOVES|LEDGOESOFF}, // in the first phase, go to coordinates X = 1000, Y = 500, Z = -500
   {0, 0, 0, XMOVES|YMOVES|ZMOVES|LEDGOESON}, //in the second phase, go to coordinates X = 0, Y = 0, Z = 0
   {150, 150, -150, XMOVES|YMOVES|ZMOVES}, //and so forth
   {0, 0, 0, XMOVES|YMOVES|ZMOVES}, //and so forth
};

Then expand the subroutine using if() statements to follow the table. Ultimately, you are going to need an index that loops through the table, like

for(int i=0; i<4; i++) {moveMotors(moves[i][0], moves[i][1], moves[i][2]);}

Ultimately, it appears you are trying to code what a G-code interpreter does for every input line. I am at this point not clear on what the "phase" part of your code is for, but it is a state variable that could also be included in a table.

  • Wes
7

Thank you both, I think it's starting to make sense now...

It seems it is inevitable for me to either use a "state" variable, or to cycle through an array with pre-determined values. If the latter, then @weshowe has shown some helpful examples.

If the former, then I think @johnwasser has answered that the the way to check for state changes quickly without using the if() statement is to instead use a switch statement, which requires typing fewer amount of characters to achieve the same thing as an if() statement. (which now I realize is what @J-M-L was probably referring to).

I was hoping there would be some one or two line method of telling the code:
"hey don't repeat the above few lines of code ever again, and move to the next set of lines"...but without such a feature, the only way is to really have a state variable (e.g. "phase") and just keep cycling through it with an if() or switch() statement, or use an array with a for() loop.

I haven't really looked into a G code interpreter because I assumed G code will merely control 4-6 motor axes, whereas I will likely want to do something like:

  1. move motors XYZA to a location. (A is the end effector claw)
  2. Once it's reached the target location, use a servo to open a gate/door that is hiding a ball, and turn on an LED to signify the door is open
  3. move motors XYZ down towards the ball
  4. once in position, move motor A such that it causes the end effector to grab the ball
  5. close the servo gate/door thing, turn off the LED
  6. lift the ball and move motors XYZA somewhere else
    etc...

Thank you!

8

I wasn't thinking about robotic parts in my answers, more like milling machines, but those solutions will help.
Ultimately, you want to do the steps once for(;;) or over-and-over while(1) but put all the low-level stuff in routines, and that will make your task ultimately easier to manage.

  • Wes
9

Yes but I was also trying to address your point about renumbering steps if you want to modify the sequence by using a state machine with enum.

The idea was that instead of state++ at the end of a given stage you would do state = XXXX; where XXXX is the next enum entry that is applicable

Then the switch makes testing easy to read and changing the order in the sequence is just modifying a couple XXXX

10

If one only wants to do a step once put the code in setup() it runs once and its done.

11

The Main Loop in Arduino is void loop() and blocking code is bad.

12

Is there a motor library command that tells you the current position?

13

Yes curentPosition()

https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html#a5dce13ab2a1b02b8f443318886bf6fc5

There is also the distanceToGo() that is related

https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html#a748665c3962e66fbc0e9373eb14c69c1

14

distance to go tells me that there is a go-to that doesn't block, right?

15

Correct - you set a target position and you call often run() in the loop which takes a step when needed based on desired acceleration and speed parameters

16

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