Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#include <Servo.h>
//Declare servos
Servo servo1;
Servo servo2;\
//Declare pin references & other constants
int servo1Pin = 9;
int servo2Pin = 10;
int ledPin = 8;
int IRemitter1Pin = 11;
int IRemitter2Pin = 12;
int IRdetector1Pin = 7;
int IRdetector2Pin = 6;
int PULSE = 38000;
int SERVO_SPEED = 5;
int IR_CHECK_FREQ = 40;
void setup() {
Serial.begin(9600);
pinMode(servo1Pin, OUTPUT);
pinMode(servo2Pin, OUTPUT);
pinMode(IRdetector1Pin, INPUT);
pinMode(IRdetector2Pin, INPUT);
pinMode(ledPin, OUTPUT);
servo1.attach(servo1Pin);
servo2.attach(servo2Pin);
randomSeed(analogRead(0));
servo1.write(90 - SERVO_SPEED);
servo2.write(90 + SERVO_SPEED);
}
void loop(){
for(int i = 0; i < IR_CHECK_FREQ; i++){ //Using a for loop instead of delay() so that I can run other code at the same time.
if(i == IR_CHECK_FREQ - 1){ // Run for every IR_CHECK_FREQ because the sensor cannot handle long duration of IR light.
IRcheck(IRemitter1Pin, IRdetector1Pin, servo1, servo2); //Check 1st pair of IR and go backward a bit if the sensor does not detect the IR reflected from table.
IRcheck(IRemitter2Pin, IRdetector2Pin, servo2, servo1); //Same thing but for the second pair.
}
delay(1); //Wait one mS to record time for the loop.
}
}
void IRcheck(int emitter, int detector, Servo servo, Servo otherServo){
tone(emitter, PULSE); //Tone the emitter to the frequency the detector can detect.
delay(1); //Give time for detector to react;
if(digitalRead(detector)) reactToEdge(servo, otherServo); //If not detect IR reflected from the table, Go back for a random amount of time.
noTone(emitter); //Stop tonning to give detector a rest and let the other pair check IR.
}
void reactToEdge(Servo backServo, Servo otherServo){
int ogBackServoSpeed = (int)backServo.read(); //Save the original speed of servos.
int ogOtherServoSpeed = (int)otherServo.read();
otherServo.write(90); //Stop the not related servo from moving.
backServo.write(180 - ogBackServoSpeed); //Turn the servo to rotate in the opposite direction;
int rdm = random(40, 1000); //Generate a random number.
delay(rdm); //Keep the servo moving the other direction for a random amount of time
otherServo.write(ogOtherServoSpeed); //Load back the original speed of servos.
backServo.write(ogBackServoSpeed);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.