An interesting thing about these IR detectors is that their outputs are just like the whiskers. When no object is detected, the output is high; when an object is detected, the output is low. In this activity, the example sketch RoamingWithWhiskers is modified so that it works with the IR detectors; all it takes is few simple modifications. Here are the steps:
- Save the sketch RoamingWithWhiskers as RoamingWithIr
- Add the irDetect function.
int irDetect(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 8); delay(1); int ir = digitalRead(irReceiverPin); delay(1); return ir; }
- Replace these digitalRead calls:
byte wLeft = digitalRead(5); byte wRight = digitalRead(7);
…with these calls to irDetect:
int irLeft = irDetect(9, 10, 38000); int irRight = irDetect(2, 3, 38000);
- Replace all instances of wLeft with irLeft and wRight with irRight.
- Update the /*…*/ and // comments.
Example Sketch – RoamingWithIr
- Open RoamingWithWhiskers.
- Save it as RoamingWithIr.
- Modify it so that it matches the sketch below.
- Save it; then, upload it into the Arduino.
- Disconnect the BOE Shield-Bot from its programming cable.
- Reconnect the battery pack and move the 3-position switch to position 2.
- Place your BOE Shield-Bot somewhere where it can roam and avoid obstacles.
- Verify that it behaves like RoamingWithWhiskers (aside from the fact that there’s no contact required).
/* * Robotics with the BOE Shield - RoamingWithIr * Adaptation of RoamingWithWhiskers with IR object detection instead of * contact switches. */ #include <Servo.h> // Include servo library Servo servoLeft; // Declare left and right servos Servo servoRight; void setup() // Built-in initialization block { pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver tone(4, 3000, 1000); // Play tone for 1 second delay(1000); // Delay to finish tone servoLeft.attach(13); // Attach left signal to pin 13 servoRight.attach(12); // Attach right signal to pin 12 } void loop() // Main loop auto-repeats { int irLeft = irDetect(9, 10, 38000); // Check for object on left int irRight = irDetect(2, 3, 38000); // Check for object on right if((irLeft == 0) && (irRight == 0)) // If both sides detect { backward(1000); // Back up 1 second turnLeft(800); // Turn left about 120 degrees } else if(irLeft == 0) // If only left side detects { backward(1000); // Back up 1 second turnRight(400); // Turn right about 60 degrees } else if(irRight == 0) // If only right side detects { backward(1000); // Back up 1 second turnLeft(400); // Turn left about 60 degrees } else // Otherwise, no IR detected { forward(20); // Forward 1/50 of a second } } int irDetect(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 8); // IRLED 38 kHz for at least 1 ms delay(1); // Wait 1 ms int ir = digitalRead(irReceiverPin); // IR receiver -> ir variable delay(1); // Down time before recheck return ir; // Return 1 no detect, 0 detect } void forward(int time) // Forward function { servoLeft.writeMicroseconds(1700); // Left wheel counterclockwise servoRight.writeMicroseconds(1300); // Right wheel clockwise delay(time); // Maneuver for time ms } void turnLeft(int time) // Left turn function { servoLeft.writeMicroseconds(1300); // Left wheel clockwise servoRight.writeMicroseconds(1300); // Right wheel clockwise delay(time); // Maneuver for time ms } void turnRight(int time) // Right turn function { servoLeft.writeMicroseconds(1700); // Left wheel counterclockwise servoRight.writeMicroseconds(1700); // Right wheel counterclockwise delay(time); // Maneuver for time ms } void backward(int time) // Backward function { servoLeft.writeMicroseconds(1300); // Left wheel clockwise servoRight.writeMicroseconds(1700); // Right wheel counterclockwise delay(time); // Maneuver for time ms }