Jims blog A list of stuff others might find useful


Arduino code for the robot arm

Filed under: arduino,iot,robot — Jim @ 01:32 pm

Following on from the previous post, I thought I should explain the Arduino code used to control the robot arm

You can download the code from here

In summary the program sits in a loop waiting for commands from the serial port, it responds to them by moving the arm in the requested direction and then reports the current positional state of each of the joints. Asynchronously it will adjust the wrist joint to maintain the ‘hand’ as horizontal as it can within a few degrees.

Here then is my explanation for the parts of the code. I’ve left out the comments that are in the code where they would just be duplicated.

First we include the headers needed for the motor shield and to control the servo.

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <Servo.h>

Then we create the motor shield, motor and servo objects

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myShoulder = AFMS.getMotor(1);
Adafruit_DCMotor *myElbow    = AFMS.getMotor(2);
Adafruit_DCMotor *myWrist    = AFMS.getMotor(3);
Adafruit_DCMotor *myBase     = AFMS.getMotor(4);
Servo servo;

These servo position values of 100 and 135 are the absolute positions of the servo arm needed to open and close the clamp

#define OPEN 100
#define CLOSE 135

On the Arduino UNO you can only attach interrupts to pins 2 and 3

#define FRONTPIN 2
#define BACKPIN 3

Although not exposed in the scratch or web applications, the motor run time can be changed to one of 6 values. (array position 0 is not used only 1 to 6) The default duration used is 100ms in position 4

int motor[] = { 0, 15, 30, 60, 100, 250, 500 };
int duration = 4;
int motorTime = motor[duration];

The default clamp state is open at startup corresponding to servo value 100

int clampState = OPEN;

Here we set some variables that are needed in the interrupt routines so they are declared volitile, the de-bounce time means we don’t get too many false triggers of the code.

unsigned long debounce = 50;
volatile int frontPinState = 0;
volatile int backPinState = 0;
volatile unsigned long frontLastTime = 0;
volatile unsigned long backLastTime = 0;

All of the initialisation is done in the setup() function.

  • Set the interrupt pins to input and set their pull-up to high
  • Start the serial port at 19,200 baud
  • Initialise the motor shield
  • Set the initial ‘speed’ of the motors, which won’t change. 0 is off, 255 is max speed. These are 3v motors on a 5v supply so not driving at full speed
  • Attach the servo to pin 9, this is exposed on the motor shield as a 3 pin header and makes it simple to plug a servo in. Then ‘write’ the value 100 to open the clamp if not already.
  • Attach the sub-routines ‘front’ and ‘back’ to interrupt 0 and 1 respectively and specify they are to trigger on a pin CHANGE
  • delay for 1/4 second and report the first set of position values.
void setup() {
   pinMode(FRONTPIN, INPUT);
   pinMode(BACKPIN, INPUT);
   digitalWrite(FRONTPIN, HIGH);
   digitalWrite(BACKPIN, HIGH)
   attachInterrupt(0, front, CHANGE);
   attachInterrupt(1, back, CHANGE);

The main loop, first set some variables

void loop() {
   String validCommands = "rsSeEwWbBcd";  // The valid commands we will accept for control
   bool cmdComplete = false;
   static int parm = 0;
   static char command;                   // Command character

Now if a character arrives on the serial input build up a command until a comma terminates it

   while ((Serial.available() > 0) & (!cmdComplete)) {
      char ch = Serial.read();
      if (ch != ',') {                 // Not a comma
         if (ch >= '0' && ch <= '9') {  // Accumulate the decimal parameter into parm if character read is numeric
            parm = parm * 10 + ch - '0';
         } else if (validCommands.indexOf(ch) != -1) {
            command = ch;                // If it's not numeric see if it is one of our valid commands
         } else {
            Serial.println("Invalid command");
      } else {
         cmdComplete = true;            // When we get a comma the command is complete

If cmdComplete is true then we can action that. Use a big switch statement to decide what to do for each command

   if (cmdComplete) {
      cmdComplete = false;
      // Do the action depending on the character received.
      switch (command) {
         case 'r':                   // r just read the sensors and write their values to serial output, don't move
         case 's':                   // s moves the shoulder forwards
         case 'S':                   // S moves the shoulder backwards
         case 'e':                   // e moves the elbow down
         case 'E':                   // E moves the elbow up
         case 'w':                   // w moves the wrist down
         case 'W':                   // W moves the wrist up
         case 'b':                   // b turns the base clockwise (looking down)
         case 'B':                   // B turns the base anti-clockwise (looking down)
         case 'c':                   // c toggles the clamp open and closed
         case 'd':                   // Set the duration of motor run time
            duration = parm;
            motorTime = motor[duration];

After the command has been actioned call readsensors() to report all the joint positions out to the serial port, default the command to an ‘r’ and the parameter to 0

      command = 'r';
      parm = 0;

Before ending the loop and checking for more characters, check to see if either of the interrupt routines have been triggered by the tilt switch. If either has, move the wrist in the opposite direction to keep it level, reset the state and report all the new joint positions.

   if (frontPinState == 1) {
      frontPinState = 0;
   if (backPinState == 1) {
      backPinState = 0;

Now the subroutines. First the readsensors() routine which will read the analog values of all four potentiometers into variables and then concatenate the values together to print to the serial output.

void readsensors() {
   delay (5);                            // Delay between analog reads is meant to allow time for 'settling'
   int sensorValue1 = analogRead(A0);    // Position of shoulder
   delay (5);
   int sensorValue2 = analogRead(A1);    // Position of elbow
   delay (5);
   int sensorValue3 = analogRead(A2);    // Position of wrist
   delay (5);
   int sensorValue4 = analogRead(A3);    // Position of base

   // State of all sensors is reported in one comma separated line
   Serial.println(String(sensorValue4) + ","
                + String(sensorValue1) + ","
                + String(sensorValue2) + ","
                + String(sensorValue3) + ","
                + ((clampState == OPEN) ? "open" : "closed") + ","
                + String(duration));

The next four routines ‘run’ the motors for the corresponding joint in the required direction for the pre-defined run time before stopping the motor again.

void moveShoulder(int dir){

void moveElbow(int dir){

void moveWrist(int dir){

void moveBase(int dir){

The clamp() routine toggles the clamp, if it is open it will close it and vice versa. The servo is moved in increments of 5 between the open (100) and closed (135) values with a small delay to slow the movement down.

void clamp() {
   int pos = 0;
   if (clampState == OPEN) {                     // If the clamp is currently open
      for(pos = OPEN; pos <= CLOSE; pos += 5) {
      clampState = CLOSE;                        // now it's closed
   } else {                                      // else step back the other way
      for(pos = CLOSE; pos>= OPEN; pos -= 5) {
      clampState = OPEN;

The last two routines are the ones attached to the the interrupt pins 2 and 3. The routines are called when the respective pin changes from low to high or high to low. We’ll only trigger a wrist movement if debounce milliseconds have passed between successive calls. (I’m not sure this is really a true ‘debounce’ like you would a switch but it seems to work OK). All the routine does is to set the *PinState flag to 1, it is checked in the main program loop.

void front() {               // The interrupt routine, called when front pin changes
   if((long)(millis() - frontLastTime) >= debounce) {  
      frontLastTime = millis();
      frontPinState = 1;

void back() {               // As above except for the back pin
   if((long)(millis() - backLastTime) >= debounce) {
      backLastTime = millis();
      backPinState = 1;

Powered by WordPress