/*

CoasterBot Master
Black Dog Robotics
MAKE Coasterbot Challenge
Mar-May 2010
I2C master, sensor input
reads inputs, controls motor speed, etc.
*/
#include <Wire.h>
// conversion between data types
// I2C only sends bytes
union BYTE2INT {
byte asBytes[2];
int asInt;
} byteToInt;
union BYTE2FLOAT {
byte asBytes[2];
int asFloat;
} byteToFloat;
// message buffers
byte 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 inc
hes
// 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 states
int cur_state = -1;
int next_state = 0;
// motor speeds
// these may not actually be current motor speeds
// update from motor controller before use
int spdr = 0;
int spdl = 0;
// used for speed increase/decrease calls
int loopCnt = 0;
// time variables
unsigned 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 input
pinMode(LF_IR, INPUT);
pinMode(RF_IR, INPUT);
pinMode(LR_IR, INPUT);
pinMode(RR_IR, INPUT);
// state LED debug output
pinMode(LED_1, OUTPUT);
pinMode(LED_2, OUTPUT);
pinMode(LED_4, OUTPUT);
pinMode(LED_8, OUTPUT);
// use random A2D noise to seed random number generator
randomSeed(analogRead(1));
}
//
// main loop, update state and call approporiate state handler function
//
void loop()
{
// set current state and update debugging LEDs
cur_state = next_state;
echoValue(cur_state);
// call state function
switch(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 motors
setSpeed(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 press
while(digitalRead(BUTTON_PIN) == HIGH)
{
}
// flash to indicate we're about to start
for(int i = 15; i >=0; i--)
{
echoValue(i);
delay(75);
}
// next state is locate target
next_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 speed
getSpeed(&spdr, &spdl);
if((spdr == 0) && (spdl == 0))
{
setSpeed(INIT_SPEED);
 }
// continue until slow to approach obstacle, or stop to avoid obstacle
while(true)
{
// delay between samples
delay(10);
// front IR triggered, obstacle reached
// go to AVOID_STATE
if((digitalRead(LF_IR) == LOW) || (digitalRead(RF_IR) == LOW))
{
Serial.println(" front IR triggered - stop");
setSpeed(STOP);
next_state = AVOID_STATE;
break;
}
// sonar in range, approaching an obstacle
// go to SLOWER_STATE to slow down
if(analogRead(SONAR) < AVOID_THRESHOLD)
{
Serial.print(" sonar in range");
loopCnt++;
if(loopCnt > LOOP_MAXCOUNT)
{
next_state = SLOWER_STATE;
Serial.println(" - slower");
break;
}
Serial.println("");
continue;
}
// nothing in the way, speed up a bit
// go to FASTER_STATE
Serial.print(" clear");
loopCnt++;
if(loopCnt > LOOP_MAXCOUNT)
{
next_state = FASTER_STATE;
Serial.println(" - faster");
break;
}
Serial.println("");
}
}
//
// reduce speed
// exit to DRIVE_STATE
//
void slowerState()
{
Serial.println("SLOWER_STATE");
// get current speeds from motor controller
// we could save them in this code somewhere, but only
// storing them on the motor control separates functionality better
getSpeed(&spdr, &spdl);
// reduce speed
spdr = (int)(DECELL*(float)spdr);
spdl = (int)(DECELL*(float)spdl);
// can't go slower than min
// use abs() funtion to handle forward and backward speeds
if(abs(spdr) < MIN_SPEED)
 {
if(spdr > 0)
{
spdr = MIN_SPEED;
}
else
{
spdr = -1 * MIN_SPEED;
}
if(spdl > 0)
{
spdl = MIN_SPEED;
}
else
{
spdl = -1 * MIN_SPEED;
}
}
Serial.print(" new speeds ");
Serial.print(spdr);
Serial.print(" ");
Serial.println(spdl);
setSpeed(spdr, spdl);
next_state = DRIVE_STATE;
}
//
// increase speed
// exit to DRIVE_STATE
//
void fasterState()
{
Serial.println("FASTER_STATE");
// get current speeds from motor controller
// we could save them in this code somewhere, but only
// storing them on the motor control separates functionality better
getSpeed(&spdr, &spdl);
// reduce speed
spdr = (int)(ACCEL*(float)spdr);
spdl = (int)(ACCEL*(float)spdl);
// can't go faster than max
// use abs() funtion to handle forward and backward speeds
if(abs(spdr) > MAX_SPEED)
{
if(spdr > 0)
{
spdr = MAX_SPEED;
}
else
{
spdr = -1 * MAX_SPEED;
}
if(spdl > 0)
{
spdl = MAX_SPEED;
}
else
{
spdl = -1 * MAX_SPEED;
}
}
 Serial.print(" new speeds ");
Serial.print(spdr);
Serial.print(" ");
Serial.println(spdl);
setSpeed(spdr, spdl);
next_state = DRIVE_STATE;
}
//
// back up, turn right or left to avoid obstacle
// exit to DRIVE_STATE to continue
//
void avoidState()
{
Serial.println("AVOID_STATE");
//
// stop in front of obstacle and flash lights
//
time_expires = millis() + AVOID_STOPTIME;
setSpeed(STOP);
while(time_expires > millis())
{
// flash a random value 0-15 on status LEDs
echoValue(random(15));
delay(50);
}
//
// back up until time expires or rear triggers hit
//
Serial.println(" reverse");
setSpeed(-1 * INIT_SPEED);
time_expires = millis() + AVOID_BACKUPTIME + random(AVOID_BACKUPTIME);
while((time_expires > millis()) &&
((digitalRead(LR_IR) == HIGH) && (digitalRead(RR_IR) == HIGH)))
{
// flash a random value 0-15 on status LEDs
echoValue(random(15));
delay(50);
}
setSpeed(STOP);
// set turn time
Serial.println(" spin");
time_expires = millis() + AVOID_TIMEOUT + random(AVOID_TIMEOUT);
//
// turn right or left
//
if(random(0, 2) != 0)
{
spinCW(SPIN_SPEED);
}
else
{
spinCCW(SPIN_SPEED);
}
while(time_expires > millis())
{
// flash a random value 0-15 on status LEDs
echoValue(random(15));
delay(50);
}
// done showing off, stop spin, reset status LEDs, go to DRIVE_STATE
 setSpeed(STOP);
echoValue(cur_state);
next_state = DRIVE_STATE;
}
//
// display an integer in binary on 4 debug LEDs
//
void echoValue(int n)
{
digitalWrite(LED_1, (n & 0x01)==0 ? LOW : HIGH);
digitalWrite(LED_2, (n & 0x02)==0 ? LOW : HIGH);
digitalWrite(LED_4, (n & 0x04)==0 ? LOW : HIGH);
digitalWrite(LED_8, (n & 0x08)==0 ? LOW : HIGH);
}
//
// set speed for both motors to the same value
//
void setSpeed(int spd)
{
// range check
if(spd < -255)
{
spd = -255;
}
if(spd > 255)
{
spd = 255;
}
Serial.print("Setting equal speeds ");
Serial.println(spd);
// set speed
byteToInt.asInt = spd;
msg1[0] = 1;
msg1[1] = byteToInt.asBytes[0];
msg1[2] = byteToInt.asBytes[1];
// send 3 byte message to 0x04 via I2C
Wire.beginTransmission(4);
Wire.send(msg1, 3);
Wire.endTransmission();
}
//
// set speed for both motors to the unique values
//
void setSpeed(int spdr, int spdl)
{
// range check
if(spdr < -255)
{
spdr = -255;
}
if(spdr > 255)
{
spdr = 255;
}
if(spdl < -255)
{
spdl = -255;
}
if(spdl > 255)
{
 spdl = 255;
}
//
Serial.print("Setting speeds ");
Serial.print(spdr);
Serial.print(" ");
Serial.println(spdl);
// set speed
byteToInt.asInt = spdr;
msg2[0] = 2;
msg2[1] = byteToInt.asBytes[0];
msg2[2] = byteToInt.asBytes[1];
byteToInt.asInt = spdl;
msg2[3] = byteToInt.asBytes[0];
msg2[4] = byteToInt.asBytes[1];
// send 5 byte message to 0x04 via I2C
Wire.beginTransmission(4);
Wire.send(msg2, 5);
Wire.endTransmission();
}
//
// spin CW
//
void spinCW(int spd)
{
// range check
if(spd < -255)
{
spd = -255;
}
if(spd > 255)
{
spd = 255;
}
setSpeed(spd, -1*spd);
}
//
// spin CCW
//
void spinCCW(int spd)
{
// range check
if(spd < -255)
{
spd = -255;
}
if(spd > 255)
{
spd = 255;
}
setSpeed(-1 * spd, spd);
}
//
// request current motor speeds from motor controller
// via I2C connection
//
void getSpeed(int* spdr, int* spdl)
{
// send 1 byte mode message to 0x04 via I2C
// mode 3 is a speed request to follow
 byte msg1 = 0x03;
Wire.beginTransmission(4);
Wire.send(msg1);
Wire.endTransmission();
// delay to let slave handle message
// if not here, communications doesn't work
// need to play with delay, or have slave send a 'ready' message
delay(50);
// request 4 bytes for speed
Wire.requestFrom(4, 4);
int i = 0;
byte rcv[4];
// again, delay for slave to process
delay(50);
while(Wire.available())
{
rcv[i] = Wire.receive();
i++;
if(i > 3)
break;
}
// move from bytes to int values to return to caller
byteToInt.asBytes[0] = rcv[0];
byteToInt.asBytes[1] = rcv[1];
*spdr = byteToInt.asInt;
byteToInt.asBytes[0] = rcv[2];
byteToInt.asBytes[1] = rcv[3];
*spdl = byteToInt.asInt;
}