1

I’m working on a 4 wheeled robot driven with stepper motors. I have a Raspberry Pi that sends a random (based on input image) pulse count to the Arduino board serially and then steps that amount. I’m using an Arduino CNC board to drive all 4 stepper motors, just FYI. My problem is that currently my steppers will not simultaneously move. I’ve read over the documentation for the AccelStepper library pretty well and tried some of the example code with no luck. I have a lot of function/ parsing code prior to the actual stepper code. It’s almost at the end, the function called ‘yMovement’. Currently, the int value is received then yStepper moves, then yStepper1 moves after.

My updated code now makes them move simultaneously but the done command does not behave as before.

#include <AccelStepper.h>

const int stepperCount = 4;
AccelStepper FLStepper(AccelStepper::FULL2WIRE, 2, 5);
AccelStepper FRStepper(AccelStepper::FULL2WIRE, 3, 6);
AccelStepper BRStepper(AccelStepper::FULL2WIRE, 4, 7);
AccelStepper BLStepper(AccelStepper::FULL2WIRE, 12, 13);

bool movementComplete = false;

// defines pins numbers

//Front left wheel
const int stepX = 2;
const int dirX  = 5;

//Front right wheel
const int stepY = 3;
const int dirY  = 6;

//Back left wheel
const int stepZ = 4;
const int dirZ  = 7;

//Back right wheel
const int stepA = 12;
const int dirA  = 13;
const int enPin = 8;


char split = ':';         //this is the character that would be used for seperating the different parts of your commands
                          //the syntax for commands would be:   command:value1:value2

int listSize = 5;                                     //the amount of commands in the list
String commands[] = {"hello", "add", "sub", "YMOV", "XMOV"};     //the list of every command name


void setup()
{
  Serial.begin(115200);     //sets the data transfer rate for the serial interface
                          //9600 is good for basic testing, but should be as high
                          //as possible for both devices.
  FRStepper.setMaxSpeed(500);
  FRStepper.setAcceleration(400);
  BRStepper.setMaxSpeed(500);
  BRStepper.setAcceleration(400);
 
  FLStepper.setMaxSpeed(500);
  FLStepper.setAcceleration(400);
  BLStepper.setMaxSpeed(500);
  BLStepper.setAcceleration(400);

  pinMode(stepX, OUTPUT);
  pinMode(dirX, OUTPUT);
 
  pinMode(stepY, OUTPUT);
  pinMode(dirY, OUTPUT);
 
  pinMode(stepZ, OUTPUT);
  pinMode(dirZ, OUTPUT);

  pinMode(enPin, OUTPUT);

  digitalWrite(enPin, LOW);
  digitalWrite(dirX, HIGH);
  digitalWrite(dirY, HIGH);
  digitalWrite(dirZ, HIGH);
  digitalWrite(dirA, HIGH);


}

void loop()
{
  CommCheck(); //checks serial buffer for data commands
  runMotors();
}

void runMotors()
{

  if ((FLStepper.distanceToGo() != 0) || (FRStepper.distanceToGo() != 0) || (BLStepper.distanceToGo() != 0) || (BRStepper.distanceToGo() != 0))
  {
   
    FLStepper.run();
    //BLStepper.run();
    FRStepper.run();
    //BRStepper.run();
    movementComplete = true;
  }
  if (movementComplete == true)
  {
    CommConfirm();
    movementComplete = false;
  }
  //if ((FLStepper.distanceToGo() == 0) && (FRStepper.distanceToGo() == 0) && (BLStepper.distanceToGo() == 0) && (BRStepper.distanceToGo() == 0))
  //if ((FLStepper.distanceToGo() == 0) || (FRStepper.distanceToGo() == 0) || (BLStepper.distanceToGo() == 0) || (BRStepper.distanceToGo() == 0))
  //{
    //movementComplete = true;
  //}
}
void CommCheck()
{
  if(Serial.available())                    //checks to see if there is serial data has been received
  {
    //int len = Serial.available();           //stores the character lengh of the command that was sent
                                             //this is used for command parsing later on
                                           
    String command = Serial.readString();   //stores the command as a text string
    int len = command.length();
    //Serial.println(command);
    Serial.flush();
    //command.remove(len-2,1);                //removes characters added by the pi's serial data protocol
    //command.remove(0,2);
    //len -= 3;                               //updates the string length value for parsing routine

    int points[2] = {0, 0};                 //offset points for where we need to split the command into its individual parts
   
    for(int x = 0; x < len; x++)            //this loop will go through the entire command to find the split points based on
    {                                       //what the split variable declared at the top of the script is set to.
      //Serial.print("Char ");
      //Serial.print(x);
      //Serial.print("- ");
      //Serial.println(command[x]);
      if(command[x] == split)               //this goes through every character in the string and compares it to the split character
      {
        if(points[0] == 0)                  //if the first split point hasn't been found, set it to the current spot
        {
          points[0] = x;
        }
        else                                //if the first spot was already found, then set the second split point
        {                                   //this routine is currently only set up for a command syntax that is as follows
          points[1] = x;                    //command:datavalue1:datavalue2
        }
      }
    }
    CommParse(command, len, points[0], points[1]);      //now that we know the command, command length, and split points,
  }                                                     //we can then send that information out to a routine to split the data
}                                                       //into individual values.

void CommParse(String command, int len, int point1, int point2)
{
  //Serial.print("Command Length: ");
  //Serial.println(len);
  //Serial.print("Split 1: ");
  //Serial.println(point1);
  //Serial.print("Split 2: ");
  //Serial.println(point2);

 
  String com = command;                 //copy the full command into all 3 parts
  String val1 = command;                //this is needed for the string manipulation
  String val2 = command;                //that follow

  com.remove(point1, len - point1);     //each of these use the string remove to delete
  val1.remove(point2, len - point2);    //the parts of the command that aren't needed
  val1.remove(0, point1 + 1);           //basically splitting the command up into its
  val2.remove(0, point2 + 1);           //individual pieces
  val2.remove(val2.length()-1,1);

  CommLookup(com, val1, val2);    //these pieces are then sent to a lookup routine for processing
}


void CommLookup(String com, String val1, String val2)
{
 
  int offset = 255;                   //create a variable for our lookup table's offest value
                                      //we set this to 255 because there won't be 255 total commands
                                      //and a valid command can be offset 0, so it's just to avoid
                                      //any possible coding conflicts if the command sent doesn't
                                      //match anything.
                                     
  for(int x = 0; x < listSize; x++)   //this goes through the list of commands and compares
  {                                   //them against the command received
    if(commands[x] == com)
    {
      offset = x;                     //if the command matches one in the table, store that command's offset
    }
  }
 
  switch(offset)                //this code compares the offset value and triggers the appropriate command
  {
    case 0:                                 //essentially a hello world.                       |  Syntax: hello:null:null
      CommHello();                          //this activates the hello world subroutine        |  returns Hello!
      break;
    case 1:                                 //adds both values together and return the sum.    |  Syntax: add:value1:value2
      CommAdd(val1.toInt(), val2.toInt());  //this activates the addition subroutine           |  returns value1 + value2
      break;
    case 2:                                 //subtracts both values and return the difference  |  Syntax: subtract:value1:value2
      CommSub(val1.toInt(), val2.toInt());  //this activates the subtraction subroutine        |  returns value1 - value2
      break;
    case 3:
      yMovement(val1.toInt(), val2.toInt());
      break;
    case 4:
      xMovement(val1.toInt(), val2.toInt());
    default:                                        //this is the default case for the command lookup and will only
      Serial.println("Command not recognized");     //trigger if the command sent was not known by the arduino
      break;
  }
}


void CommHello()                               //each of these routines are what will be triggered when they are successfully processed
{
  Serial.println("Hello!");
  CommConfirm();
}

void CommAdd(int val1, int val2)
{
  Serial.println(val1 + val2);
  CommConfirm();
}

void CommSub(int val1, int val2)
{
  Serial.println(val1 - val2);
  CommConfirm();
}

void yMovement(int val1, int val2)
{
  if (val1 < 0) {
    int yMoveNew = (val1 * (-4));
    //Serial.println(val1 * (-1));
    //delay(500);
   
    FRStepper.moveTo(-yMoveNew);
   
    BRStepper.moveTo(-yMoveNew);
   
    FLStepper.moveTo(-yMoveNew);
   
    BLStepper.moveTo(-yMoveNew);

    //delayMicroseconds(500);
  }

  else {

    int yMoveNew = (val1 * 4);
    //Serial.println(val1);
    //delay(500);
    FRStepper.moveTo(yMoveNew);
   
    BRStepper.moveTo(yMoveNew);
   
    FLStepper.moveTo(yMoveNew);
   
    BLStepper.moveTo(yMoveNew);

    //delayMicroseconds(500);
   
  }
}

void xMovement(int val1, int val2)
{
  if (val1 < 0) {
    int xMoveNew = (val1 * (-4));
    //Serial.println(val1 * (-1));
    //delay(1000);
    FLStepper.moveTo(-xMoveNew);
   
    BLStepper.moveTo(-xMoveNew);

    FRStepper.moveTo(-xMoveNew);

    BRStepper.moveTo(-xMoveNew);

    //delayMicroseconds(500);
  }

  else {

    int xMoveNew = (val1 * 4);
    //Serial.println(val1);
    //delay(1000);
    FLStepper.moveTo(xMoveNew);

    BLStepper.moveTo(xMoveNew);

    FRStepper.moveTo(xMoveNew);

    BRStepper.moveTo(xMoveNew);

    //delayMicroseconds(500);
   
  }
}
void CommConfirm()                                  
{                                                    
  Serial.println("Done");
 
}


1 Answer 1

1

With the AccelStepper.move() function, you are setting the target position of the motor. Then you run the motor with AccelStepper.runToPosition(). But that is a blocking function. It will not exit, until the motor reached it's target position. So you just picked the wrong function from the AccelStepper library.


Small adjacent about simultaneous action on microcontrollers: Since most microcontrollers, like in the Arduino, only have one core, they can only do exactly one thing at a time. But you can still execute things one after another, but so fast, that a human would say, they where happening at the same time. For this we need to divide down the actions into small chunks, that are executed very fast. In your case we have the big actions of running multiple motors to their target positions. Each of these big actions take long in total, but we can divide them down. And with stepper motors that is really easy, because one step would be one small chunk of that action.

So we want to go through our code very fast, successively checking for each motor, if it is time to do a step with it. If yes, we only do that one step. Then we proceed further to the next motor.


The AccelStepper library helps you in doing this checking if it is time to do a step. This is what happens in the AccelStepper.run() function. You just need to call it regularily, fast enough to not miss the time for a step.

So the solution is to remove all the calls to AccelStepper.runToPosition() and instead call the AccelStepper.run() function for all your stepper motors directly in the loop() function.

3
  • Thanks for the help! The problem now is that my commConfirm() function does not send at the right time. I tried adding it in the loop with run() but now realize why it doesn't work like that because technically run() is being ran every step. So the done command is send as soon as the motors start. May 19, 2021 at 14:05
  • You have if (movementComplete == true) for executing commConfirm() with the variable set in the if statement above. Make it an else if clause of the if statement above (but checking for false instead of true) and set movementComplete = true; in that if else clause. The else will cause that part to only be executed, when the distances to go are zero, and only executes once. So something like else if(movementComplete == false){ commConfirm(); movementComplete = true;}. Also make sure to reset movementComplete to false, when you start another movement via Serial.
    – chrisl
    May 19, 2021 at 16:57
  • Thanks for the help! Shortly after I figured it out! May 19, 2021 at 17:11

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.