/********************************************************************
* follow.c *
* -------- *
* runs on Create Command Module *
* cover all but small opening on the front of the IR sensor *
* Create will follow a virtual wall (or any IR sending out the *
* force-field signal) and hopefully avoid obstacles in the process *
********************************************************************/

#include interrupt.h>
#include io.h>
#include
#include "oi.h"

#define TRUE 1
#define FALSE 0
#define FullSpeed 0x7FFF
#define SlowSpeed 0x0100
#define SearchSpeed 0x0100
#define ExtraAngle 10
#define SearchLeftAngle 125
#define SearchRightAngle (SearchLeftAngle - 1000)
#define CoastDistance 150
#define TraceDistance 250
#define TraceAngle 30
#define BackDistance 25
#define IRDetected (~PINB & 0x01)
//states
#define Ready 0
#define Following 1
#define WasFollowing 2
#define SearchingLeft 3
#define SearchingRight 4
#define TracingLeft 5
#define TracingRight 6
#define BackingTraceLeft 7
#define BackingTraceRight 8


// Global variables
volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;
volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];
volatile uint8_t sensors[Sen6Size];
volatile int16_t distance = 0;
volatile int16_t angle = 0;
volatile uint8_t inRange = 0;

// Functions
void byteTx(uint8_t value);
void delayMs(uint16_t time_ms);
void delayAndCheckIR(uint16_t time_ms);
void delayAndUpdateSensors(unsigned int time_ms);
void initialize(void);
void powerOnRobot(void);
void baud(uint8_t baud_code);
void drive(int16_t velocity, int16_t radius);
uint16_t randomAngle(void);
void defineSongs(void);

int main (void)
{

//state variable
uint8_t state = Ready;
int found = 0;
int wait_counter = 0;

// Set up Create and module
initialize();
LEDBothOff;
powerOnRobot();
byteTx(CmdStart);
baud(Baud28800);
byteTx(CmdControl);
byteTx(CmdFull);

// set i/o for second IR sensor
DDRB &= ~0x01; //set cargo bay ePort pin 3 to be an input
PORTB |= 0x01; //set cargo ePort pin3 pullup enabled

// program loop
while(TRUE)
{
// Stop just as a precaution
drive(0, RadStraight);

// set LEDs
byteTx(CmdLeds);
byteTx(((sensors[SenVWall])?LEDPlay:0x00) | (inRange?LEDAdvance:0x00));
byteTx(sensors[SenCharge1]);
byteTx(64);
IRDetected?LED2On:LED2Off;
inRange?LED1On:LED1Off;

//looking for user button, check often
delayAndUpdateSensors(10);
delayAndCheckIR(10);

if(UserButtonPressed) {
delayAndUpdateSensors(1000);
//active loop
while(!(UserButtonPressed)
&&(!sensors[SenCliffL])
&&(!sensors[SenCliffFL])
&&(!sensors[SenCliffFR])
&&(!sensors[SenCliffR])
) {

byteTx(CmdLeds);
byteTx(((sensors[SenVWall])?LEDPlay:0x00) | (inRange?LEDAdvance:0x00));
byteTx(sensors[SenCharge1]);
byteTx(255);
IRDetected?LED2On:LED2Off;
inRange?LED1On:LED1Off;

switch(state) {
case Ready:
if(sensors[SenVWall]) {
//check for proximity to leader
if(inRange) {
drive(0,RadStraight);
} else {
//drive straight
drive(SlowSpeed, RadStraight);
state = Following;
}
} else {
//search for the beam
angle = 0;
distance = 0;
wait_counter = 0;
found = FALSE;
drive(SearchSpeed, RadCCW);
state = SearchingLeft;
}
break;
case Following:
if(sensors[SenBumpDrop] & BumpRight) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceLeft;
} else if(sensors[SenBumpDrop] & BumpLeft) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceRight;
} else if(sensors[SenVWall]) {
//check for proximity to leader
if(inRange) {
drive(0,RadStraight);
state = Ready;
} else {
//drive straight
drive(FullSpeed, RadStraight);
state = Following;
}
} else {
//just lost the signal, keep going slowly one cycle
distance = 0;
drive(SlowSpeed, RadStraight);
state = WasFollowing;
}
break;
case WasFollowing:
if(sensors[SenBumpDrop] & BumpRight) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceLeft;
} else if(sensors[SenBumpDrop] & BumpLeft) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceRight;
} else if (sensors[SenVWall]) {
//check for proximity to leader
if(inRange) {
drive(0,RadStraight);
state = Ready;
} else {
//drive straight
drive(FullSpeed, RadStraight);
state = Following;
}
} else if (distance >= CoastDistance) {
drive(0,RadStraight);
state = Ready;
} else {
drive(SlowSpeed, RadStraight);
}
break;
case SearchingLeft:
if(found) {
if (angle >= ExtraAngle) {
drive(SlowSpeed, RadStraight);
state = Following;
} else {
drive(SearchSpeed, RadCCW);
}
} else if (sensors[SenVWall]) {
found = TRUE;
angle = 0;
if (inRange) {
drive(0,RadStraight);
state = Ready;
} else {
drive(SearchSpeed, RadCCW);
}
} else if (angle >= SearchLeftAngle) {
drive(SearchSpeed, RadCW);
wait_counter = 0;
state = SearchingRight;
} else {
drive(SearchSpeed, RadCCW);
}
break;
case SearchingRight:
if(found) {
if (-angle >= ExtraAngle) {
drive(SlowSpeed, RadStraight);
state = Following;
} else {
drive(SearchSpeed, RadCW);
}
} else if (sensors[SenVWall]) {
found = TRUE;
angle = 0;
if (inRange) {
drive(0,RadStraight);
state = Ready;
} else {
drive(SearchSpeed, RadCCW);
}
} else if(wait_counter > 0) {
wait_counter -= 20;
drive(0,RadStraight);
} else if (angle = SearchRightAngle) {
drive(0,RadStraight);
wait_counter = 5000;
angle = 0;
} else {
drive(SearchSpeed, RadCW);
}
break;
case TracingLeft:
if(sensors[SenBumpDrop] & BumpRight) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceLeft;
} else if(sensors[SenBumpDrop] & BumpLeft) {
drive(0, RadStraight);
state=Ready;
} else if (sensors[SenVWall]) {
//check for proximity to leader
if(inRange) {
drive(0,RadStraight);
state = Ready;
} else {
//drive straight
drive(SlowSpeed, RadStraight);
state = Following;
}
} else if (!(distance >= TraceDistance)) {
drive(SlowSpeed, RadStraight);
} else if (!(-angle >= TraceAngle)) {
drive(SearchSpeed, RadCW);
} else {
distance = 0;
angle = 0;
drive(SlowSpeed, RadStraight);
state = Ready;
}
break;
case TracingRight:
if(sensors[SenBumpDrop] & BumpRight) {
drive(0, RadStraight);
state=Ready;
} else if(sensors[SenBumpDrop] & BumpLeft) {
distance = 0;
angle = 0;
drive(-SlowSpeed, RadStraight);
state=BackingTraceRight;
} else if (sensors[SenVWall]) {
//check for proximity to leader
if(inRange) {
drive(0,RadStraight);
state = Ready;
} else {
//drive straight
drive(SlowSpeed, RadStraight);
state = Following;
}
} else if (!(distance >= TraceDistance)) {
drive(SlowSpeed, RadStraight);
} else if (!(angle >= TraceAngle)) {
drive(SearchSpeed, RadCCW);
} else {
distance = 0;
angle = 0;
drive(SlowSpeed, RadStraight);
state = Ready;
}
break;
case BackingTraceLeft:
if (sensors[SenVWall] && inRange) {
drive(0,RadStraight);
state = Ready;
} else if (angle >=TraceAngle) {
distance = 0;
angle = 0;
drive(SlowSpeed, RadStraight);
state = TracingLeft;
} else if (-distance >= BackDistance) {
drive (SearchSpeed, RadCCW);
} else {
drive(-SlowSpeed, RadStraight);
}
break;
case BackingTraceRight:
if (sensors[SenVWall] && inRange) {
drive(0,RadStraight);
state = Ready;
} else if (-angle >=TraceAngle) {
distance = 0;
angle = 0;
drive(SlowSpeed, RadStraight);
state = TracingRight;
} else if (-distance >= BackDistance) {
drive (SearchSpeed, RadCW);
} else {
drive(-SlowSpeed, RadStraight);
}
break;
default:
//stop
drive(0,RadStraight);
state = Ready;
break;
}
delayAndCheckIR(10);
delayAndUpdateSensors(10);
}
//cliff or userbutton detected, allow condition to stabilize (eg, button to be released)
drive(0,RadStraight);
delayAndUpdateSensors(2000);
}
}
}


// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
uint8_t temp;


temp = UDR0;

if(sensors_flag)
{
sensors_in[sensors_index++] = temp;
if(sensors_index >= Sen6Size)
sensors_flag = 0;
}
}


// Timer 1 interrupt to time delays in ms
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
if(timer_cnt)
timer_cnt--;
else
timer_on = 0;
}

// Transmit a byte over the serial port
void byteTx(uint8_t value)
{
while(!(UCSR0A & _BV(UDRE0))) ;
UDR0 = value;
}

// Delay for the specified time in ms without updating sensor values
void delayMs(uint16_t time_ms)
{
timer_on = 1;
timer_cnt = time_ms;
while(timer_on) ;
}


// Delay for the specified time in ms and check second IR detector
void delayAndCheckIR(uint16_t time_ms)
{
uint8_t timer_val = 0;
inRange = 0;
timer_on = 1;
timer_cnt = time_ms;
while(timer_on) {
if(!(timer_val == timer_cnt)) {
inRange += IRDetected;
timer_val = timer_cnt;
}
}

inRange = (inRange>=(time_ms>>1));
}

// Delay for the specified time in ms and update sensor values
void delayAndUpdateSensors(uint16_t time_ms)
{
uint8_t temp;

timer_on = 1;
timer_cnt = time_ms;
while(timer_on)
{
if(!sensors_flag)
{
for(temp = 0; temp Sen6Size; temp++)
sensors[temp] = sensors_in[temp];

// Update running totals of distance and angle
distance += (int)((sensors[SenDist1] 8) | sensors[SenDist0]);
angle += (int)((sensors[SenAng1] 8) | sensors[SenAng0]);

byteTx(CmdSensors);
byteTx(6);
sensors_index = 0;
sensors_flag = 1;
}
}
}

// Initialize the Mind Control's ATmega168 microcontroller
void initialize(void)
{
cli();

// Set I/O pins
DDRB = 0x10;
PORTB = 0xCF;
DDRC = 0x00;
PORTC = 0xFF;
DDRD = 0xE6;
PORTD = 0x7D;

// Set up timer 1 to generate an interrupt every 1 ms
TCCR1A = 0x00;
TCCR1B = (_BV(WGM12) | _BV(CS12));
OCR1A = 71;
TIMSK1 = _BV(OCIE1A);

// Set up the serial port with rx interrupt
UBRR0 = 19;
UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));

// Turn on interrupts
sei();
}

void powerOnRobot(void)
{
// If Create's power is off, turn it on
if(!RobotIsOn)
{
while(!RobotIsOn)
{
RobotPwrToggleLow;
delayMs(500); // Delay in this state
RobotPwrToggleHigh; // Low to high transition to toggle power
delayMs(100); // Delay in this state
RobotPwrToggleLow;
}
delayMs(3500); // Delay for startup
}
}

// Switch the baud rate on both Create and module
void baud(uint8_t baud_code)
{
if(baud_code = 11)
{
byteTx(CmdBaud);
UCSR0A |= _BV(TXC0);
byteTx(baud_code);
// Wait until transmit is complete
while(!(UCSR0A & _BV(TXC0))) ;

cli();

// Switch the baud rate register
if(baud_code == Baud115200)
UBRR0 = Ubrr115200;
else if(baud_code == Baud57600)
UBRR0 = Ubrr57600;
else if(baud_code == Baud38400)
UBRR0 = Ubrr38400;
else if(baud_code == Baud28800)
UBRR0 = Ubrr28800;
else if(baud_code == Baud19200)
UBRR0 = Ubrr19200;
else if(baud_code == Baud14400)
UBRR0 = Ubrr14400;
else if(baud_code == Baud9600)
UBRR0 = Ubrr9600;
else if(baud_code == Baud4800)
UBRR0 = Ubrr4800;
else if(baud_code == Baud2400)
UBRR0 = Ubrr2400;
else if(baud_code == Baud1200)
UBRR0 = Ubrr1200;
else if(baud_code == Baud600)
UBRR0 = Ubrr600;
else if(baud_code == Baud300)
UBRR0 = Ubrr300;

sei();

delayMs(100);
}
}

// Send Create drive commands in terms of velocity and radius
void drive(int16_t velocity, int16_t radius)
{
byteTx(CmdDrive);
byteTx((uint8_t)((velocity >> 8) & 0x00FF));
byteTx((uint8_t)(velocity & 0x00FF));
byteTx((uint8_t)((radius >> 8) & 0x00FF));
byteTx((uint8_t)(radius & 0x00FF));
}