Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
/* 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]);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment