Last active
January 26, 2025 21:35
-
-
Save Will-Firgelli/44a14a4f3cac3209164efe8abe3285b6 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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