For my robotics projects, I have had the need to control multiple motors, sensors, servos, etc. so that they perform a variety of moves in sequence. But it's not obvious in Arduino how to program a set of instructions or robotic commands.
I ended up developing a way to do that, so I am posting it here in case it helps anyone along the way. And I do want to thank the Arduino community because learning how to utilize the motors, the resetting of the stagemanager function, and endless tips, advice and help along the way is how I got here in the first place. I'm still a pretty noob programmer, so this method may not be perfect, but it works.
As an aside, I made a separate post about how to move multiple motors in a synchronized/coordinated way, with acceleration, with AccelStepper (e.g. for moving a robot arm). Hope that is helpful as well.
Example
Let's say you want a robot to perform a set of commands, such as:
pickUpObject(whichObject)
{
Step 1: Move the robot to the location of the object
Step 2: Open a gripper
Step 3: Open a servo to release some object so the gripper can grab it
Step 4: Wait a few moments for the object to settle into the gripper
Step 5: Close the gripper
Step 6: Move the robot to a neutral location
}
dropObject(whichLocation)
{
Step 1: Move the robot to whatever target location or container
Step 2: Open the gripper
Step 3: Wait a few moments for the object to fall out
Step 4: Move the robot to a neutral location
}
Now what if you then want a program that does something like...
void loop()
{
Step 1: Wait for a button press to start the automatic program
Step 2: pickUpObject(greenBall)
Step 3: dropObject(greenBucketLocation)
Step 4: Check the weight sensor in greenBucket to make sure the ball was detected
Step 5: Turn on a green LED light
Step 6: Wait for a button press to start again...
}
Clearly, there is a need to be able to program "steps." And sometimes you have steps within a step.
The solution I used is to have a StageManager Class, which tracks which step in the process each command is at.
-
Each robot function will have a static StageManager Class
-
To move onto the next step, issue the next() function, which will wait until all the motor moves are finished, and will then automatically proceed to the next step
-
If you want to add a delay/pause between moves without actually freezing up your program (since you may want still listen for an emergency stop button, or some other sensor), you can use the waitThenNext(milliseconds) command, which will wait for all motor moves to finish, then wait for a period of time, then move to the next step.
Code example: (sorry if I have artifacts or errors, I tried my best to extract it from my own code)
bool pickUpObject(whichObject)
{
bool operationCompleted = false;
static StageManager pickUpObjectStageManager;
if (pickUpObjectStageManager.stage == 1)
{
//move your motors to a set of coordinates using the AccelStepper library
pickUpObjectStageManager.next(); //automatically identify when motors have reached their target positions, and then move to the next step
}
else if (pickUpObjectStageManager.stage == 2)
{
//Open a gripper by controlling a servo, e.g. using a digitalWrite() command
pickUpObjectStageManager.waitThenNext(1000); //this will wait 1 second before going to the next step, in case you want to give time for the servo to open or something to settle down, since we can't track when the servo has actually moved to its new position)
}
else if (pickUpObjectStageManager.stage == 3)
{
//move your motors so the gripper is above the bucket
pickUpObjectStageManager.next();
}
else if (pickUpObjectStageManager.stage == 4)
{
//move the gripper servo so the gripper opens again
pickUpObjectStageManager.waitThenNext(5000); //wait 5 seconds so that the ball also has a chance to fall into the bucket and settle down
}
else if (pickUpObjectStageManager.stage == 5)
{
//close the gripper
pickUpObjectStageManager.next();
}
else if (pickUpObjectStageManager.stage == 6)
{
//move the motors so the robot moves to a neutral position
pickUpObjectStageManager.next();
}
else if (pickUpObjectStageManager.stage == 7)
{
operationCompleted = true;
}
return operationCompleted;
}
Now let's say you wanted the robot to only do anything while you're holding a button down...
void loop()
{
if (buttonIsPressed == true)
{
if (pickUpObject(greenBall)
{
Serial.println("pickUpObject() completed!");
//you can reset the status of your button so that you wait for a new input command instead of automatically repeating this same command again
}
}
}
Note: you will want to call StageManager::resetAll() at some point. For example, if the button is ever let go, or if you switch between various programs, or if you hit an emergency stop button. This way, all of your commands reset to "step 1." You don't want to accidentally call a robotic command and expect it to start at Step 1 but because the last time, you didn't let it finish Step 3, it thinks it's still on Step 3. That's the purpose of the StageManager::resetAll() function.
Conclusion:
So clearly, I have a lot of "if" statements in my code, and I'm sure better programmers out there may be able to explain how that could be inefficient. I have seen some suggest GCODE. However GCODE seems well suited for a CNC machine where you just send a bunch of coordinates, but it's not very human readable and I haven't seen an easy way to use it when you have motors moves AND other commands, sensors, serial input, maybe a joystick or buttons to activate various movements, etc.
So, I hope this helps. Feel free to weigh in. And sorry if I made a mistake, I tried my best to reproduce the code as best as I could.
Since I have been using VS Code, I split my INO file into a header and CPP file, but you can just combine them and remove whatever is redundant:
StageManager.h
//Stage manager class to allow all functions to have their own instance of a stage variable and wait timers to
#ifndef STAGE_MANAGER_H
#define STAGE_MANAGER_H
#include <Arduino.h>
#include <vector>
#include <algorithm>
#include "motor_control.h"
static constexpr size_t maxInstances = 100; //just some large number that is larger than the number of unique StageManager instances you would ever have
class StageManager // a class that can be created as a "static" member in each function, serves as a full stage manager that creates a stage variable, "next" functions, , non blocking delay/wait timer
{
public:
StageManager();
~StageManager();
int stage;
void captureTimeStamp();
void waitThenNext(int milliseconds); //use this to start waiting at the end of a stage - easy to use
void wait(int milliseconds); //this requires captureTimeStamp() to be called at the end of the previous stage - not as easy to use
void next();
static void resetAll(); //resets stage and waitStage variables for each instance
private:
int waitStage;
int capturedTimeStamp;
//static std::vector<StageManager*> stageManagerInstances; //vector of pointers to each instance of StageManager
static StageManager *stageManagerInstances[maxInstances]; //array instead of vector
};
#endif
StageManager.cpp
//Stage manager class to allow all functions to have their own instance of a stage variable and wait timers to
#include "StageManager.h"
//std::vector<StageManager*> StageManager::stageManagerInstances = {nullptr}; //the vector way
StageManager * StageManager::stageManagerInstances[maxInstances] = {nullptr}; //the pointer array way
StageManager::StageManager()
{
stage = 0;
waitStage = 0;
//stageManagerInstances.push_back(this);
//using array:
for (auto &element : stageManagerInstances)
{
if (element == nullptr)
{
element = this;
break;
}
}
}
StageManager::~StageManager()
{
//stageManagerInstances.erase(std::remove(stageManagerInstances.begin(), stageManagerInstances.end(), this), stageManagerInstances.end()); //find and delete it from the list
for (auto &element : stageManagerInstances)
{
if (element == this)
{
element = nullptr;
break;
}
}
}
void StageManager::captureTimeStamp()
{
capturedTimeStamp = millis();
}
void StageManager::waitThenNext(int milliseconds)
{
if(waitStage == 0 && allMovesComplete())
{
captureTimeStamp();
waitStage++;
}
else if(waitStage == 1)
{
if(millis() >= (capturedTimeStamp + milliseconds))
{
stage++;
waitStage = 0;
//consider resetting all the motors' speed and acceleration here if you change them during move commands
}
}
}
void StageManager::wait(int milliseconds)
{
if(millis() >= (capturedTimeStamp + milliseconds))
{
stage++;
}
}
void StageManager::next()
{
{
if(allMovesComplete())
{
stage++; //move to the next stage
//consider resetting all the motors' speed and acceleration here if you change them during move commands
}
}
}
void StageManager::resetAll()
{
/*
for (auto &element : stageManagerInstances)
{
element->stage = 0;
element->waitStage = 0;
}
*/
for (auto &element : stageManagerInstances)
{
if (element != nullptr)
{
element->stage = 0;
element->waitStage = 0;
}
}
}
All Moves Complete function
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;
}
}
