Как синхронизировать два линейных привода с помощью Arduino

/* 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
 * 
 * Program requires two (or more) of our supported linear actuators:
 * FA-OS-35-12-XX
 * FA-OS-240-12-XX
 * FA-OS-400-12-XX
 * Products available for purchase at https://www.firgelliauto.com/collections/linear-actuators/products/optical-sensor-actuators
 */

#include <elapsedMillis.h>
elapsedMillis timeElapsed;

#define numberOfActuators 2 
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 long lastDebounceTime_0=0; //timer for when interrupt was triggered
volatile long lastDebounceTime_1=0;

int Speed = 255; //choose any speed in the range [0, 255]

#define falsepulseDelay 20 //noise pulse time, if too high, ISR will miss pulses. 
volatile int counter[numberOfActuators]={}; 
volatile int prevCounter[numberOfActuators]={}; 
int Direction; //-1 = retracting
               // 0 = stopped
               // 1 = extending

int extensionCount[numberOfActuators] = {}; 
int retractionCount[numberOfActuators] = {}; 
int pulseTotal[numberOfActuators]={}; //stores number of pulses in one full extension/actuation

void setup(){
  for(int i=0; i<numberOfActuators; i++){
    pinMode(RPWM[i],OUTPUT);
    pinMode(LPWM[i], OUTPUT);
    pinMode(opticalPins[i], INPUT_PULLUP);
    counter[i]=0; //initialize variables as array of zeros
    prevCounter[i]=0;
    extensionCount[i] = 0;
    retractionCount[i] = 0;
    pulseTotal[i] = 0;
  }
  attachInterrupt(digitalPinToInterrupt(opticalPins[0]), count_0, RISING);
  attachInterrupt(digitalPinToInterrupt(opticalPins[1]), count_1, RISING); 

  Serial.begin(9600);
  Serial.println("Initializing calibration");
  Serial.println("Actuator retracting...");
  Direction = -1;
  moveTillLimit(Direction, 255); 
  Serial.println("Actuator fully retracted");
  delay(1000);

  for(int i=0; i<numberOfActuators; i++){
    Serial.print("\t\t\t\tActuator ");
    Serial.print(i);
  }

  Direction = 1;
  moveTillLimit(Direction, 255); //extend fully and count pulses
  Serial.print("\nExtension Count:");
  for(int i=0; i<numberOfActuators; i++){
  extensionCount[i]=counter[i];
  Serial.print("\t\t"); 
  Serial.print(extensionCount[i]);
  Serial.print("\t\t\t"); 
  }
  delay(1000);

  Direction = -1;
  moveTillLimit(Direction, 255); //retract fully and count pulses
  Serial.print("\nRetraction Count:");
  for(int i=0; i<numberOfActuators; i++){
  retractionCount[i]=counter[i];
  Serial.print("\t\t"); 
  Serial.print(abs(retractionCount[i]));
  Serial.print("\t\t\t"); 
  }
  Serial.print("\n");

  for(int i=0; i<numberOfActuators; i++){
    Serial.print("\nActuator ");
    Serial.print(i);
    Serial.print(" average pulses: ");
    pulseTotal[i]=(extensionCount[i]+abs(retractionCount[i]))/2; //takes the average of measurements
    Serial.print(pulseTotal[i]); 
  } 
  Serial.println("\n\nEnter these values in the synchronous control progam.");
}

void loop() { 
}

void moveTillLimit(int Direction, int Speed){
  //this 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 counts 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 
    lastDebounceTime_0 = millis();
    if(Direction==1){
      counter[0]++;
    }
    if(Direction==-1){
      counter[0]--;
    }
  }
}

void count_1(){
  if ((millis() - lastDebounceTime_1) > falsepulseDelay) { 
    lastDebounceTime_1 = millis();
    if(Direction==1){
      counter[1]++;
    }
    if(Direction==-1){
      counter[1]--;
    }
  }
}

/* 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]);
  } 
}