/*CoasterBot MasterBlack Dog RoboticsMAKE Coasterbot ChallengeMar-May 2010I2C master, sensor inputreads inputs, controls motor speed, etc.*/#include <Wire.h>// conversion between data types// I2C only sends bytesunion BYTE2INT {byte asBytes[2];int asInt;} byteToInt;union BYTE2FLOAT {byte asBytes[2];int asFloat;} byteToFloat;// message buffersbyte msg1[3];byte msg2[5];// define digital pin numbers#define BUTTON_PIN 10 // the number of the pushbutton pin#define LED_PIN 13 // the number of the LED pin#define LF_IR 2 // left front IR - HIGH when object in range#define RF_IR 3 // right front IR#define LR_IR 4 // left rear IR#define RR_IR 5 // right rear IR#define LED_1 9 // 1's LED#define LED_2 8 // 2's LED#define LED_4 7 // 4's LED#define LED_8 6 // 8's LED// define analog pin numbers#define SONAR 0 // low far near, high for far, n*0.5 ~ distance in inches// note - analog4, analog5 used for I2C by wire library// states and associated variables#define INIT_STATE 0#define DRIVE_STATE 1#define SLOWER_STATE 2#define FASTER_STATE 3#define AVOID_STATE 4// current and next statesint cur_state = -1;int next_state = 0;// motor speeds// these may not actually be current motor speeds// update from motor controller before useint spdr = 0;int spdl = 0;// used for speed increase/decrease callsint loopCnt = 0;

 

// time variablesunsigned long time_expires;// timeout values for states or conditions that timeout#define AVOID_TIMEOUT 750#define AVOID_BACKUPTIME 1500#define AVOID_STOPTIME 2500// max count for intermittent conditions#define LOOP_MAXCOUNT 5// (sonar) distance value where avoiding an obstacle starts#define AVOID_THRESHOLD 40#define MIN_SPEED 50#define INIT_SPEED 127#define MAX_SPEED 255#define SPIN_SPEED 127#define STOP 0#define ACCEL 1.25#define DECELL 0.75//// initialization//void setup() {Serial.begin(9600);// join i2c bus (address optional for master)Wire.begin(2);// initialize the LED pin as an output:pinMode(LED_PIN, OUTPUT);// initialize the pushbutton pin as an input:pinMode(BUTTON_PIN, INPUT);// sensor inputpinMode(LF_IR, INPUT);pinMode(RF_IR, INPUT);pinMode(LR_IR, INPUT);pinMode(RR_IR, INPUT);// state LED debug outputpinMode(LED_1, OUTPUT);pinMode(LED_2, OUTPUT);pinMode(LED_4, OUTPUT);pinMode(LED_8, OUTPUT);// use random A2D noise to seed random number generatorrandomSeed(analogRead(1));}//// main loop, update state and call approporiate state handler function//void loop(){// set current state and update debugging LEDscur_state = next_state;echoValue(cur_state);// call state functionswitch(cur_state){case(INIT_STATE):initState();break;case(DRIVE_STATE):driveState();break;

 

case (SLOWER_STATE):slowerState();break;case (FASTER_STATE):fasterState();break;case(AVOID_STATE):avoidState();break;default:next_state = 0;}}//// wait for button press to start// next state is LOCATE_STATE//void initState(){// stop motorssetSpeed(STOP);//Serial.println("INIT_STATE");// test binary counter// four bits, 0000 (0) to 1111 (15)for(int i = 0; i < 16; i++){echoValue(i);delay(75);}echoValue(0);// wait for button presswhile(digitalRead(BUTTON_PIN) == HIGH){}// flash to indicate we're about to startfor(int i = 15; i >=0; i--){echoValue(i);delay(75);}// next state is locate targetnext_state = DRIVE_STATE;}//// drive straight while watching sensors// exit to FASTER_STATE if clear// exit to SLOWER_STATE if sonar < threshold (approaching something)// exit to AVOID_STATE if front IR on (reached something)//void driveState(){Serial.println("DRIVE_STATE");loopCnt = 0;// get current speeds from motor controller// if stopped, set initial drive speedgetSpeed(&spdr, &spdl);if((spdr == 0) && (spdl == 0)){setSpeed(INIT_SPEED);