Each motor can lift more than 50 Lbs

4 Motor Control – Arduino code to move four motors at the same speed

A member at Techshop had four motors and needed to use them to lift a heavy plate. He needed each motor to lift one corner; working in synch or else one corner of the plate would end up way to high.

The guy, a very talented woodworker & machinist, wanted a little help on the code. I wrote a simple Arduino program that implements a closed-loop position control algorithm. Because ultimately the mechanism was to be controlled with an Iphone / Bluetooth interface, I wrote it sort of like a “state machine”; implementing modes like “OFF”, “STOPPED”, “FORWARD”, “REVERSE”.

After a little debugging it actually works really good!

The software uses feedback from the encoders to keep each motor within a calibratable maximum angular displacement
The software uses feedback from the encoders to keep each motor within a calibratable maximum angular displacement

Each motor can lift more than 50 Lbs
Each motor can lift more than 50 Lbs
Parrallax Encoders have a 5 degree resolution
Parrallax Encoders have a 5 degree resolution

Here’s the code:

// FourMotorPositionControl_OnOff
// Shaun Bowman, February 16 2013
// www.technodiy.com || shaun_bowman@hotmail.com
// 
// This code controls 4 motors's such that they tend to be at the same rotational angle.
// The method of control is "On/Off"; if a motor gets too far ahead, its speed is set commanded to 0
// When the other motors are far enough past a stopped motor, the stopped motor is restarted.
// Each motor has an encoder. When an encorder state changes, an interrupt is triggered. The interrupt
// records that the particular motor has changed angle by 1 encoder position. The main loop grabs a copy
// of the encoder change log & then resets the log. It uses the copy to increment an array which tracks 
// the relative position of each motor. Then the loop checks to see if any motor should be started or stopped
// based on its position relative to the others. The next part of the main loop responds to the overall state
// of the program. The program can be set to STOP, FWD, REV, or OFF. Stop means the motors are commanded to 0 
// speed, but the servo control of the motor is active. FWD causes the motors to move in the forward direction.
// REV, moves the motors in reverse. OFF commands the motors to 0 speed and then deactivates the servo control.
// The program depends on the main loop running faster than the interrupts. It cannot handle 2 or more 
// encoder changes of a particular motor between loops. This means that adding delays & serial communication
// inside the main loop may cause the controller to fail. Another limitation is handling transisions in
// direction. If moving forward motorX is 3 encoder changes ahead; upon reverse the controller will
// act as though it is still ahead 3 changes, when infact it is behind. All of these limitations can
// be removed with a little be of extra code.

#include  

// CALIBRATIONS:
#define PWM_STOP 1500  // Stop Command
#define PWM_REV  1400  // Reverse Speed Command
#define PWM_FWD  1600  // Forward Speed Command
#define PWM_SLOWOFST  25  // Ofset added or subtracted from PWM_STOP which is used to slow down a servo that has gotten too far ahead
#define GAIN_PWM0   0.990  // Gain for Servo1 motor commands; floating point X.XXX
#define GAIN_PWM1   1.005  // Gain for Servo1 motor commands; floating point X.XXX
#define GAIN_PWM2   1.000  // Gain for Servo1 motor commands; floating point X.XXX
#define GAIN_PWM3   1.000  // Gain for Servo1 motor commands; floating point X.XXX
#define DELTA_STOP  4  // When a motor is more than X encoder changes ahead, stop
#define DELTA_START 0  // When a stopped motor encoder is <= X changes from furthest motor, restart motor
const unsigned long START_DELAY = 350;  // Number of milliseconds to delay engaging the closed loop position control. Gives the motors a chance to energize & begin movement. Note: datatype unsigned long, for use with millis(), will cause errors if used in math with int
const unsigned long STOP_TIMEOUT = 1000; // number of milisecons to wait for a motor to begin moving once it has stopped, before stopping all motors. Safety feature. (not implemented yet)

// Constants used in the program
#define MODE_STOP 0    // Enumeration representing state of overall controller (stop, fwd, rev)
#define MODE_FWD  1    // Enumeration representing state of overall controller (stop, fwd, rev)
#define MODE_REV  2    // Enumeration representing state of overall controller (stop, fwd, rev)
#define MODE_RUN  3    // Enumeration representing state of overall controller (stop, fwd, rev)
#define MODE_OFF  4    // Enumeration representing state of overall controller (stop, fwd, rev)
#define MODE_START 5   // Enumeration representing state of overall controller (stop, fwd, rev) (not used at this time)
#define SERVO0_MASK B00000001 // Bit position of encoderChangeTF used to track the change of a given servo
#define SERVO1_MASK B00000010 // Bit position of encoderChangeTF used to track the change of a given servo
#define SERVO2_MASK B00000100 // Bit position of encoderChangeTF used to track the change of a given servo
#define SERVO3_MASK B00001000 // Bit position of encoderChangeTF used to track the change of a given servo
#define N_SERVOS 4    // Number of Servo's
#define SERVO0   0    // Array index of EncoderDeltaArray representing a particular servo
#define SERVO1   1    // Array index of EncoderDeltaArray representing a particular servo
#define SERVO2   2    // Array index of EncoderDeltaArray representing a particular servo
#define SERVO3   3    // Array index of EncoderDeltaArray representing a particular servo
#define PINSERVO0 13    // Pin number of servo
#define PINSERVO1 12    // Pin number of servo
#define PINSERVO2 11    // Pin number of servo
#define PINSERVO3 10    // Pin number of servo

// Variables used for the program
volatile int encoderChangeTF = 0; // changed by the interrupts. Binary; ex: 0000 0001 = "Servo0 changed position", ex2: 0000 1010 = "Servo3 & Servo1 changed position"
int encoderChangeSnapshotTF = 0;  // a copy of encoderChangeTF made in the main loop, making a copy prevents the interrupt's from changing the variable during the main loop
int EncoderDeltaArray[] = {0, 0, 0, 0};  // an array of the relative positions of each servo ex: {0, 2, 4, 0} = "Servo2 is 4 encoder changes ahead of Servo's 0&3; Servo1 is 2 behind Servo2 & 2 ahead Servo's 0&3"
int i_Servo = 0; // used in loops
int minDelta = 0; // smallest number in EncoderDeltaArray
int Servo0SpeedCommand = PWM_STOP; // Speed to be commanded by the servo, in units of PWM (ie: PWM_STOP = 1500, or PWM_FWD = 2000)
int Servo1SpeedCommand = PWM_STOP; // Speed to be commanded by the servo, in units of PWM (ie: PWM_STOP = 1500, or PWM_FWD = 2000)
int Servo2SpeedCommand = PWM_STOP; // Speed to be commanded by the servo, in units of PWM (ie: PWM_STOP = 1500, or PWM_FWD = 2000)
int Servo3SpeedCommand = PWM_STOP; // Speed to be commanded by the servo, in units of PWM (ie: PWM_STOP = 1500, or PWM_FWD = 2000)
int Servo0Mode = MODE_STOP;  // Mode of a given servo, this is manipulated by the closed-loop position controller, units: Run or Stop
int Servo1Mode = MODE_STOP;  // Mode of a given servo, this is manipulated by the closed-loop position controller, units: Run or Stop
int Servo2Mode = MODE_STOP;  // Mode of a given servo, this is manipulated by the closed-loop position controller, units: Run or Stop
int Servo3Mode = MODE_STOP;  // Mode of a given servo, this is manipulated by the closed-loop position controller, units: Run or Stop
int runMode = MODE_STOP;    // Overall mode of the program, ie: Stop, Off, Forward, or Reverse
boolean b_EngageCL = false;  // Boolean controlling whether or not to engage Closed Loop position control.
boolean b_Starting = false;  // Boolean intidating whether or not we are in the process of starting
unsigned long TimeStartBegan = 0;  // variable used to track when function corresponding to runMode = MODE_START began. when millis() - TimeStartBegan > START_DELAY; transision from MODE_START to MODE_RUN

Servo myservo0;   // object representing a particular servo; even though the DC motor's are not really servo's, they are controlled by PWM so a servo object can be used to control it. However unlike a servo, the PWM command represents angular velocity, not position
Servo myservo1;   // object representing a particular servo; even though the DC motor's are not really servo's, they are controlled by PWM so a servo object can be used to control it. However unlike a servo, the PWM command represents angular velocity, not position
Servo myservo2;   // object representing a particular servo; even though the DC motor's are not really servo's, they are controlled by PWM so a servo object can be used to control it. However unlike a servo, the PWM command represents angular velocity, not position
Servo myservo3;   // object representing a particular servo; even though the DC motor's are not really servo's, they are controlled by PWM so a servo object can be used to control it. However unlike a servo, the PWM command represents angular velocity, not position

//encoder 0
int encoder0PinA = 20;
int encoder0PinB = 4;
int encoder0Pos = 0;
int revolution0 = 0;

//encoder 1
int encoder1PinA = 21;
int encoder1PinB = 2;
int encoder1Pos = 0;
int revolution1 = 0;

//encoder 2
int encoder2PinA = 19;
int encoder2PinB = 6;
int encoder2Pos = 0;
int revolution2 = 0;

//encoder 3
int encoder3PinA = 18;
int encoder3PinB = 8;
int encoder3Pos = 0;
int revolution3 = 0;

void setup() { 
  //encoders
  pinMode (encoder0PinA,INPUT);
  digitalWrite(encoder0PinA, HIGH); //I have tried both HIGH and LOW with no change.
  pinMode (encoder0PinB,INPUT);
  digitalWrite(encoder0PinB, HIGH);
  pinMode (encoder1PinA,INPUT);
  digitalWrite(encoder1PinA, HIGH);
  pinMode (encoder1PinB,INPUT);
  digitalWrite(encoder1PinB, HIGH);
  pinMode (encoder2PinA,INPUT);
  digitalWrite(encoder2PinA, HIGH);
  pinMode (encoder2PinB,INPUT);
  digitalWrite(encoder2PinB, HIGH);
  pinMode (encoder3PinA,INPUT);
  digitalWrite(encoder3PinA, HIGH);
  pinMode (encoder3PinB,INPUT);
  digitalWrite(encoder3PinB, HIGH);

  attachInterrupt(3, doEncoder0, CHANGE); 
  attachInterrupt(2, doEncoder1, CHANGE); 
  attachInterrupt(4, doEncoder2, CHANGE); 
  attachInterrupt(5, doEncoder3, CHANGE); 

  Serial.begin (9600);
  Serial.println ("SERVOS OFF");
  delay(1000);

  Serial.println ("Starting");
  runMode = MODE_FWD;  // Set the program to RUN mode. This variable could be manipulated by a button event, or wireless remote control. Note: Initializing position & changing from REV to FWD is not handled smoothly by the position control code below.
} 

void loop() { 
  if (((runMode == MODE_FWD) || (runMode == MODE_REV)) && b_EngageCL){
    // Relative Encoder Position Tracking Code - Interrupts change encoderChangeTF, grab it & reset it, then add any changes in encoderChangeTF to EncoderDeltaArray
    encoderChangeSnapshotTF = encoderChangeTF; // grab snapshot of change tracker
    encoderChangeTF = 0; // reset change tracker
    EncoderDeltaArray[SERVO0] += ((encoderChangeSnapshotTF & SERVO0_MASK) == SERVO0_MASK); // increment relative position of servo
    EncoderDeltaArray[SERVO1] += ((encoderChangeSnapshotTF & SERVO1_MASK) == SERVO1_MASK); // increment relative position of servo
    EncoderDeltaArray[SERVO2] += ((encoderChangeSnapshotTF & SERVO2_MASK) == SERVO2_MASK); // increment relative position of servo
    EncoderDeltaArray[SERVO3] += ((encoderChangeSnapshotTF & SERVO3_MASK) == SERVO3_MASK); // increment relative position of servo
    minDelta = 128;  // reset the minDelta tracking variable
    for (i_Servo = 0; i_Servo < N_SERVOS; i_Servo ++){  // find the smallest number in encoder delta array
      if (EncoderDeltaArray[i_Servo] < minDelta){
        minDelta = EncoderDeltaArray[i_Servo];
      }
    }
    for (i_Servo = 0; i_Servo < N_SERVOS; i_Servo ++){  // subtract smallest number in encoder delta array from each value in the array
      EncoderDeltaArray[i_Servo] -= minDelta;
    }
  } // end Relative Encoder Position Tracking Code

  if (((runMode == MODE_FWD) || (runMode == MODE_REV)) && b_EngageCL){
    // Closed Loop Position Control Mode - Keep all motors withing DELTA_STOP encoder positions of each other by using Start/Stop control
    for (i_Servo = 0; i_Servo < N_SERVOS; i_Servo ++){  // start or stop each motor based on encoder position relative to the other motors
      if( EncoderDeltaArray[i_Servo] >= DELTA_STOP ){ // servo_i is too far ahead of slowesest one, stop it & wait for others to catch up
        if (i_Servo == SERVO0){
          Servo0Mode = MODE_STOP;
        }else if (i_Servo == SERVO1){
          Servo1Mode = MODE_STOP;
        }else if (i_Servo == SERVO2){
          Servo2Mode = MODE_STOP;
        }else if (i_Servo == SERVO3){
          Servo3Mode = MODE_STOP;
        }
      }else if ( EncoderDeltaArray[i_Servo] <= DELTA_START ){ // servo_i is enough behind that it should be made to run if it's stopped
        if (i_Servo == SERVO0){
          Servo0Mode = MODE_RUN;
        }else if (i_Servo == SERVO1){
          Servo1Mode = MODE_RUN;
        }else if (i_Servo == SERVO2){
          Servo2Mode = MODE_RUN;
        }else if (i_Servo == SERVO3){
          Servo3Mode = MODE_RUN;
        }
      }
    }
  }  // END position controller mode code

  if (((runMode == MODE_FWD) || (runMode == MODE_REV)) && !b_EngageCL){ 
  // Open Loop Velocity Control - Used to get motors started 
    Servo0Mode = MODE_RUN;  
    Servo1Mode = MODE_RUN;
    Servo2Mode = MODE_RUN;
    Servo3Mode = MODE_RUN;
    if (!b_Starting){  // Test if this is the first time we've entered the soft-start loop, if so grab the execution time & set the first time flag to false
      Serial.println ("Open Loop starting");
      TimeStartBegan = millis();
      b_Starting = true;
    }
    if (millis() - TimeStartBegan > START_DELAY){ // Finished! we've waited long enough & so let's bring on the closed-loop system
      Serial.println ("Closed Loop starting");
      encoderChangeTF = 0; // reset change tracker, in prep for closed-loop
      EncoderDeltaArray[SERVO0] = 0; // zero out relative position integrator, in prep for closed-loop
      EncoderDeltaArray[SERVO1] = 0; // zero out relative position integrator, in prep for closed-loop
      EncoderDeltaArray[SERVO2] = 0; // zero out relative position integrator, in prep for closed-loop
      EncoderDeltaArray[SERVO3] = 0; // zero out relative position integrator, in prep for closed-loop    
      b_EngageCL = true;   // set to engage closed loop
      b_Starting = false;  // reset b_Starting for naaext time CL is disengaged & command is MODE_FWD or Mode_REV (ie: need a start)
    }
  }  // END open loop position controller

  // Determine Motor Speed Commands Based on Modes:
  if ((runMode == MODE_STOP) || (runMode == MODE_OFF)){ // direction is to stop or shutdown, use stop speed command
    Servo0SpeedCommand = PWM_STOP;
    Servo1SpeedCommand = PWM_STOP;
    Servo2SpeedCommand = PWM_STOP;
    Servo3SpeedCommand = PWM_STOP;
  } else if (runMode == MODE_FWD){  // direction is forward, use forward speed command; if the position control code has set a given servo to RUN
    if (Servo0Mode == MODE_RUN){
      Servo0SpeedCommand = (int)(PWM_FWD*GAIN_PWM0 + 0.5);
    }if (Servo1Mode == MODE_RUN){
      Servo1SpeedCommand = (int)(PWM_FWD*GAIN_PWM1 + 0.5);
    }if (Servo2Mode == MODE_RUN){
      Servo2SpeedCommand = (int)(PWM_FWD*GAIN_PWM2 + 0.5);
    }if (Servo3Mode == MODE_RUN){
      Servo3SpeedCommand = (int)(PWM_FWD*GAIN_PWM3 + 0.5);
    }
    if (Servo0Mode == MODE_STOP){
      Servo0SpeedCommand = PWM_STOP+PWM_SLOWOFST;
    }if (Servo1Mode == MODE_STOP){
      Servo1SpeedCommand = PWM_STOP+PWM_SLOWOFST;
    }if (Servo2Mode == MODE_STOP){
      Servo2SpeedCommand = PWM_STOP+PWM_SLOWOFST;
    }if (Servo3Mode == MODE_STOP){
      Servo3SpeedCommand = PWM_STOP+PWM_SLOWOFST;
    }    
  } else if (runMode == MODE_REV){  // direction is reverse, use reverse speed command; if the position control code has set a given servo to RUN
    if (Servo0Mode == MODE_RUN){
      Servo0SpeedCommand = (int)(PWM_REV*GAIN_PWM0 + 0.5);
    }if (Servo1Mode == MODE_RUN){
      Servo1SpeedCommand = (int)(PWM_REV*GAIN_PWM1 + 0.5);
    }if (Servo2Mode == MODE_RUN){
      Servo2SpeedCommand = (int)(PWM_REV*GAIN_PWM2 + 0.5);
    }if (Servo3Mode == MODE_RUN){
      Servo3SpeedCommand = (int)(PWM_REV*GAIN_PWM3 + 0.5);
    }
    if (Servo0Mode == MODE_STOP){
      Servo0SpeedCommand = PWM_STOP-PWM_SLOWOFST;
    }if (Servo1Mode == MODE_STOP){
      Servo1SpeedCommand = PWM_STOP-PWM_SLOWOFST;
    }if (Servo2Mode == MODE_STOP){
      Servo2SpeedCommand = PWM_STOP-PWM_SLOWOFST;
    }if (Servo3Mode == MODE_STOP){
      Servo3SpeedCommand = PWM_STOP-PWM_SLOWOFST;
    }       
  }

  // Send Out Motor Commands:
  if (runMode != MODE_OFF){  // if run mode isn't OFF, make sure all the servo's are attached & then send out the speed command
    if (!myservo0.attached()){  
        myservo0.attach(PINSERVO0);
    } if (!myservo1.attached()){
        myservo1.attach(PINSERVO1);
    } if (!myservo2.attached()){
        myservo2.attach(PINSERVO2);
    } if (!myservo3.attached()){
        myservo3.attach(PINSERVO3);
    }      
    myservo0.writeMicroseconds (Servo0SpeedCommand);
    myservo1.writeMicroseconds (Servo1SpeedCommand);
    myservo2.writeMicroseconds (Servo2SpeedCommand);
    myservo3.writeMicroseconds (Servo3SpeedCommand);
  } else if (runMode == MODE_OFF) {  // if run mode is OFF, stop all the servo's & detach the PWM signal
    myservo0.writeMicroseconds (PWM_STOP);
    myservo1.writeMicroseconds (PWM_STOP);
    myservo2.writeMicroseconds (PWM_STOP);
    myservo3.writeMicroseconds (PWM_STOP);
    myservo0.detach();
    myservo1.detach();  
    myservo2.detach();
    myservo3.detach();
  }
} 

void doEncoder0(){
  encoder0Pos++;
  if(encoder0Pos == 72){
    revolution0++;
    encoder0Pos = 0;
  }
  encoderChangeTF = (encoderChangeTF | SERVO0_MASK);
}

void doEncoder1(){
  encoder1Pos++;
  if(encoder1Pos == 72){
    revolution1++;
    encoder1Pos = 0;
  }
  encoderChangeTF = (encoderChangeTF | SERVO1_MASK);
}

void doEncoder2(){
  encoder2Pos++;
  if(encoder2Pos == 72){
    revolution2++;
    encoder2Pos = 0;

  }
  encoderChangeTF = (encoderChangeTF | SERVO2_MASK);
}

void doEncoder3(){
  encoder3Pos++;
  if(encoder3Pos == 72){
    revolution3++;
    encoder3Pos = 0;
  }
  encoderChangeTF = (encoderChangeTF | SERVO3_MASK);
}