/*

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 inches
// note - analog4, analog5 used for I2C by wire
library
// states and associated variables
#define INIT_STATE 0
#define LOCATE_STATE 1
#define DRIVE_STATE 2
#define TRACK_STATE 3
#define SLOWER_STATE 4
#define FOUND_STATE 5
int cur_state = -1;
int next_state = 0;
// (sonar) distance value where tracking a target
starts
#define TRACK_THRESHOLD 20
//
// 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 debug output
pinMode(LED_1, OUTPUT);
pinMode(LED_2, OUTPUT);
pinMode(LED_4, OUTPUT);
 pinMode(LED_8, OUTPUT);
// test binary counter
// four bits, 0000 (0) to 1111 (15)
for(int i = 0; i < 16; i++)
{
echoValue(i);
delay(250);
}
echoValue(0);
}
//
// main loop, update state and call approporiate
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(LOCATE_STATE):
locateState();
break;
case(DRIVE_STATE):
driveState();
break;
case(TRACK_STATE):
trackState();
break;
case (SLOWER_STATE):
slowerState();
break;
case(FOUND_STATE):
foundState();
break;
 default:
next_state = 0;
}
}
//
// wait for button press to start
// next state is LOCATE_STATE
//
void initState()
{
Serial.println("INIT_STATE");
// flash ready
for(int i = 0; i < 10; i++)
{
digitalWrite(LED_PIN, HIGH);
delay(100);
digitalWrite(LED_PIN, LOW);
delay(100);
}
// wait for button press
while(digitalRead(BUTTON_PIN) == HIGH)
{
}
// flash to indicate we're about to start
for(int i = 0; i < 10; i++)
{
digitalWrite(LED_PIN, HIGH);
delay(100);
digitalWrite(LED_PIN, LOW);
delay(100);
}
next_state = LOCATE_STATE;
}
//
// spin to locate target within threshold, or timer
expires
// exit to TRACK_STATE for target in threshold
// exit to DRIVE_STATE for timer expires
//
void locateState()
{
Serial.println("LOCATE_STATE");
// forward full speed
setSpeed(32);
delay(1000);
setSpeed(64);
delay(1000);
setSpeed(127);
delay(1000);
setSpeed(192);
delay(1000);
setSpeed(255);
delay(1000);
// stop
setSpeed(0);
delay(1000);
// reverse full speed
setSpeed(-64);
delay(1000);
setSpeed(-255);
delay(1000);
// stop
setSpeed(0);
delay(1000);
// spin CCW
setSpeed(255, -255);
delay(1000);
// stop
setSpeed(0);
delay(1000);
// spin CW
setSpeed(-255, 255);
delay(1000);
// stop
setSpeed(0);
delay(1000);
//
next_state = DRIVE_STATE;
}
//
// drive straight while watching sensors and time
// exit to LOCATE_STATE if time expires
// exit to TRACK_STATE if sonar < threshold
(stumbled across something)
// exit to FOUND_STATE if front IR on (hit
something, unlikely)
//
void driveState()
{
Serial.println("DRIVE_STATE");
setSpeed(255, 255);
next_state = TRACK_STATE;
}
//
// target in sight, drive straight toward it
// exit to LOCATE_STATE if sonar distance increases
(lost target)
// exit to FOUND_STATE if front IR on
// exit to SLOWER_STATE if sonar distance decreases
(approaching target)
//
void trackState()
{
Serial.println("TRACK_STATE");
delay(100);
next_state = SLOWER_STATE;
}
//
// reduce speed
// exit to TRACK_STATE
//
void slowerState()
{
Serial.println("SLOWER_STATE");
int spdr = 0;
int spdl = 0;
// 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)(0.7*(float)spdr);
spdl = (int)(0.7*(float)spdl);
// stop, go back to init
if(spdr < 10)
{
spdr = 0;
spdl = 0;
next_state = INIT_STATE;
}
else
{
next_state = TRACK_STATE;
}
setSpeed(spdr, spdl);
}
//
// back up, spin around flash lights - indicate
target found
// exit to LOCATE_STATE to find another target
//
void foundState()
{
Serial.println("FOUND_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;
}
// 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;
}
// set speed
// spin CCW
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();
}
//
// 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;
}