two-optical-actuators-synchronous-control.ino

/* Written by Firgelli Automations
 * Limited or no support: we do not have the resources for Arduino code support
 * This code exists in the public domain
 * 
 */

#include <elapsedMillis.h>
elapsedMillis timeElapsed;

#define numberOfActuators 2       
int downPin = 7;
int stopPin = 8;
int upPin = 9;        
int RPWM[numberOfActuators]={6, 11};                                  //PWM signal right side
int LPWM[numberOfActuators]={5,10};        
int opticalPins[numberOfActuators]={2,3};                             //connect optical pins to interrupt pins on Arduino. More information: https://www.arduino.cc/reference/en/language/functions/external-interrupts/attachinterrupt/
volatile unsigned long lastDebounceTime[numberOfActuators]={0,0};     //timer for when interrupt is triggered
int pulseTotal[numberOfActuators]={908, 906};                         //values found experimentally by first running two-optical-actuators-sync-calibration.ino

int desiredSpeed=255;                            
int adjustedSpeed;
int Speed[numberOfActuators]={};    

#define falsepulseDelay 20                                            //noise pulse time, if too high, ISR will miss pulses. If using 35lb actuator, set to 8ms                          
volatile int counter[numberOfActuators]={};   
volatile int prevCounter[numberOfActuators]={};     
volatile float normalizedPulseCount[numberOfActuators]={};
int Direction;                                                        //-1 = retracting
                                                                      // 0 = stopped
                                                                      // 1 = extending
float error;
int K_p=12000;                                                        //optimized experimentally. adjust this to fine tune your system
int laggingIndex, leadingIndex;                                       //index of the slowest/fastest actuator

void setup(){ 
  pinMode(stopPin, INPUT_PULLUP);
  pinMode(downPin, INPUT_PULLUP);
  pinMode(upPin, INPUT_PULLUP);
  for(int i=0; i<numberOfActuators; i++){
    pinMode(RPWM[i],OUTPUT); 
    pinMode(LPWM[i], OUTPUT);
    pinMode(opticalPins[i], INPUT_PULLUP);
    Speed[i]=desiredSpeed;
  }
  attachInterrupt(digitalPinToInterrupt(opticalPins[0]), count_0, RISING);
  attachInterrupt(digitalPinToInterrupt(opticalPins[1]), count_1, RISING);  
  Serial.begin(9600);
  Serial.println("Calibrating the origin");
  Serial.println("Actuator retracting...");
  Direction = -1;
  moveTillLimit(Direction, 255);  
  for(int i=0; i<numberOfActuators; i++){
    counter[i]=0;                                                     //reset variables 
    prevCounter[i]=0;
    normalizedPulseCount[i] = 0;
  }
  delay(1000);
  Serial.println("Actuator fully retracted");
} 

void loop() {  
  checkButtons();

  if(Direction==1){                                                   //based on direction of motion identify the leading and lagging actuator by comparing pulse counts
    if(normalizedPulseCount[0] < normalizedPulseCount[1]){
      laggingIndex = 0; 
      leadingIndex = 1;
    }
    else{
      laggingIndex = 1;
      leadingIndex = 0;
    }
  }
  else if(Direction==-1){
    if(normalizedPulseCount[0] > normalizedPulseCount[1]){
      laggingIndex = 0;
      leadingIndex = 1;
    }
    else{
      laggingIndex = 1;
      leadingIndex = 0;
    }
  }
  
  error=abs(normalizedPulseCount[laggingIndex]-normalizedPulseCount[leadingIndex]);
  if(Direction!=0){       
    adjustedSpeed=desiredSpeed-int(error*K_p);                 
    Speed[leadingIndex]=constrain(adjustedSpeed, 0, 255);               //slow down fastest actuator
    Speed[laggingIndex]=desiredSpeed;
  }
  for(int i=0; i<numberOfActuators; i++){
    Serial.print("  ");
    Serial.print(Speed[i]);
    Serial.print("  ");
    Serial.print(normalizedPulseCount[i]*1000);
    driveActuator(i, Direction, Speed[i]);
  }
  Serial.println();
}

void checkButtons(){
  //latching buttons: direction remains the same when let go
  if(digitalRead(upPin)==LOW){ Direction=1; }                           //check if extension button is pressed
  if(digitalRead(downPin)==LOW){ Direction=-1; }  
  if(digitalRead(stopPin)==LOW){ Direction=0; }
}

void moveTillLimit(int Direction, int Speed){
  //function moves the actuator to one of its limits
  for(int i = 0; i < numberOfActuators; i++){
    counter[i] = 0;                                                     //reset counter variables
    prevCounter[i] = 0;
  }  
  do {
    for(int i = 0; i < numberOfActuators; i++) {
      prevCounter[i] = counter[i];
    }
    timeElapsed = 0;
    while(timeElapsed < 200){                                           //keep moving until counter remains the same for a short duration of time
      for(int i = 0; i < numberOfActuators; i++) {
        driveActuator(i, Direction, Speed);
      }
    }
  } while(compareCounter(prevCounter, counter));                        //loop until all counters remain the same
}

bool compareCounter(volatile int prevCounter[], volatile int counter[]){
  //compares two arrays and returns false when every element of one array is the same as its corresponding indexed element in the other array
  bool areUnequal = true;
  for(int i = 0; i < numberOfActuators; i++){
    if(prevCounter[i] == counter[i]){
      areUnequal = false;
    } 
    else{                                                               //if even one pair of elements are unequal the entire function returns true
      areUnequal = true;
      break;
    }
  }
  return areUnequal;
}

void driveActuator(int Actuator, int Direction, int Speed){
  int rightPWM=RPWM[Actuator];
  int leftPWM=LPWM[Actuator];
  switch(Direction){
    case 1:       //extension
      analogWrite(rightPWM, Speed);
      analogWrite(leftPWM, 0);
      break;  
    case 0:       //stopping
      analogWrite(rightPWM, 0);
      analogWrite(leftPWM, 0);
      break;
    case -1:      //retraction
      analogWrite(rightPWM, 0);
      analogWrite(leftPWM, Speed);
      break;
  }
}

void count_0(){
  //This interrupt function increments a counter corresponding to changes in the optical pin status
  if ((millis() - lastDebounceTime[0]) > falsepulseDelay) {             //reduce noise by debouncing IR signal with a delay
    lastDebounceTime[0] = millis();
    if(Direction==1){
      counter[0]++;
    }
    if(Direction==-1){
      counter[0]--;
    }
    normalizedPulseCount[0]=float(counter[0])/float(pulseTotal[0]);
  }
}

void count_1(){
  if ((millis() - lastDebounceTime[1]) > falsepulseDelay) {   
    lastDebounceTime[1] = millis();
    if(Direction==1){
      counter[1]++;
    }
    if(Direction==-1){
      counter[1]--;
    }
    normalizedPulseCount[1]=float(counter[1])/float(pulseTotal[1]);
  } 
}