/* * Robotics with the BOE Shield - SlowerIrRoamingForLeaderBot */ #include // Include servo library const int irLeftPin = A0; // Set left IR sensor to analog pin A0 const int irRightPin = A5; // Set right IR sensor to analog pin A5 const int setpoint = 10; // Desired tracking distance const int stoppoint = 40; // Stop when distance is further than this const int kpl = -20; // Left wheel proportional gain const int kpr = -20; // Right wheel proportional gain Servo servoLeft; // Declare left and right servos Servo servoRight; /* * Built-in initialization block */ void setup() { Serial.begin(9600); // Start Serial port, and set baud rate to 9600 servoLeft.attach(13); // Attach left signal to pin 13 servoRight.attach(12); // Attach right signal to pin 12 } /** * Main loop auto-repeats **/ void loop() { int irLeft = irDetect(irLeftPin); // Check for object on left int irRight = irDetect(irRightPin); // Check for object on right // Serial.print(irLeft); // Print left IR sensor reading // Serial.print(" "); // Serial.println(irRight); // Print right IR sensor reading if (((irLeft != 0)||(irRight != 0)) && (min(irLeft,irRight) < stoppoint)) // Check sensor range { // Left and right proportional control calculations int driveLeft = (setpoint - irLeft) * kpl; int driveRight = (setpoint - irRight) * kpr; maneuver(driveLeft, driveRight, 50); // Drive levels set speeds } else { maneuver(0, 0, 50); } } /********** ************** ********** Functions ************** ********** **************/ /* * irDetect function take measurements from IR sensor at pin irPin * and convert the measurement to distance in cm */ int irDetect(int irPin) { int sum = 0; // Initialize sum for 50 measurements for ( int i = 0; i < 50; i++ ) // Take 50 measurements to reduce error { sum += analogRead(irPin); delay(1); } int AN_in = sum / 50; // Take average of the 50 measurements float volt= AN_in * (5.0 / 1024.0); // Convert measurement to distance in cm if (volt > 3) { return 0; } else if (volt < 0.4) { return 100; } else { int distance = (int) 245.4*exp(-4.133*volt)+43.91*exp(-0.6349*volt); return distance; } } /* * maneuver function controls servo movement */ void maneuver(int speedLeft, int speedRight, int msTime) { // speedLeft, speedRight ranges: Backward Linear Stop Linear Forward // -200 -100......0......100 200 servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed if(msTime==-1) // if msTime = -1 { servoLeft.detach(); // Stop servo signals servoRight.detach(); } delay(msTime); // Delay for msTime }