+/**********************************************************

+MyRobot.ino

+

+Initial sketch structured using tabs

+

+Michael Margolis 4 July 2012

+***********************************************************/

+

+#include <AFMotor.h> // adafruit motor shield library

+#include "RobotMotor.h" // 2wd or 4wd motor library

+

+#include "globalDefines.h" // global defines

+

+// Setup runs at startup and is used configure pins and init system variables

+void setup()

+{

+ Serial.begin(9600);

+ blinkNumber(8); // open port while flashing. Needed for Leonardo only

+

+ motorBegin(MOTOR_LEFT);

+ motorBegin(MOTOR_RIGHT);

+

+ irSensorBegin(); // initialize sensors

+ pinMode(LED_PIN, OUTPUT); // enable the LED pin for output

+

+ Serial.println("Waiting for a sensor to detect blocked reflection");

+}

+

+void loop()

+{

+ // call a function when reflection blocked on left side

+ if(lookForObstacle(OBST_LEFT_EDGE) == true) {

+ calibrateRotationRate(DIR_LEFT,360); // calibrate ccw rotation

+ }

+ // as above for right sensor

+ if(lookForObstacle(OBST_RIGHT_EDGE) == true) {

+ calibrateRotationRate(DIR_RIGHT, 360); // calibrate CW rotation

+ }

+}

+

+// function to indicate numbers by flashing the built-in LED

+void blinkNumber( byte number) {

+ pinMode(LED_PIN, OUTPUT); // enable the LED pin for output

+ while(number--) {

+ digitalWrite(LED_PIN, HIGH); delay(100);

+ digitalWrite(LED_PIN, LOW); delay(400);

+ }

+}

+

+/**********************

+ code to look for obstacles

+**********************/

+

+// returns true if the given obstacle is detected

+boolean lookForObstacle(int obstacle)

+{

+ switch(obstacle) {

+ case OBST_FRONT_EDGE: return irEdgeDetect(DIR_LEFT) ||

+ irEdgeDetect(DIR_RIGHT);

+ case OBST_LEFT_EDGE: return irEdgeDetect(DIR_LEFT);

+ case OBST_RIGHT_EDGE: return irEdgeDetect(DIR_RIGHT);

+ }

+ return false;

+}

+

+/*************************************

+ functions to rotate the robot

+*************************************/

+

+// return the time in milliseconds to turn the given angle at the given speed

+long rotationAngleToTime( int angle, int speed)

+{

+int fullRotationTime; // time to rotate 360 degrees at given speed

+

+ if(speed < MIN_SPEED)

+ return 0; // ignore speeds slower then the first table entry

+

+ angle = abs(angle);

+

+ if(speed >= 100)

+ fullRotationTime = rotationTime[NBR_SPEEDS-1]; // the last entry is 100%

+ else

+ {

+ int index = (speed - MIN_SPEED) / SPEED_TABLE_INTERVAL ; // index into speed and time tables

+ int t0 = rotationTime[index];

+ int t1 = rotationTime[index+1]; // time of the next higher speed

+ fullRotationTime = map(speed, speedTable[index], speedTable[index+1], t0, t1);

+ // Serial.print("index= "); Serial.print(index); Serial.print(", t0 = "); Serial.print(t0); Serial.print(", t1 = "); Serial.print(t1);

+ }

+ // Serial.print(" full rotation time = "); Serial.println(fullRotationTime);

+ long result = map(angle, 0,360, 0, fullRotationTime);

+ return result;

+}

+

+// rotate the robot from MIN_SPEED to 100% increasing by SPEED_TABLE_INTERVAL

+void calibrateRotationRate(int sensor, int angle)

+{

+ Serial.print(locationString[sensor]);

+ Serial.println(" calibration" );

+ for(int speed = MIN_SPEED; speed <= 100; speed += SPEED_TABLE_INTERVAL)

+ {

+ delay(1000);

+ blinkNumber(speed/10);

+

+ if( sensor == DIR_LEFT)

+ { // rotate left

+ motorReverse(MOTOR_LEFT, speed);

+ motorForward(MOTOR_RIGHT, speed);

+ }

+ else if( sensor == DIR_RIGHT)

+ { // rotate right

+ motorForward(MOTOR_LEFT, speed);

+ motorReverse(MOTOR_RIGHT, speed);

+ }

+ else

+ Serial.println("Invalid sensor");

+

+ int time = rotationAngleToTime(angle, speed);

+

+ Serial.print(locationString[sensor]);

+ Serial.print(": rotate ");

+ Serial.print(angle);

+ Serial.print(" degrees at speed ");

+ Serial.print(speed);

+ Serial.print(" for ");

+ Serial.print(time);

+ Serial.println("ms");

+ delay(time);

+ motorStop(MOTOR_LEFT);

+ motorStop(MOTOR_RIGHT);

+ delay(2000); // two second delay between speeds

+ }

+}

+