/*cm2pc.c
This is the final build for my project. It incorporates everything that the supporting
programs have outlined. It walks through how to set up the USb port for debugging purposes,
shows how to properly initialize all the pins on create, shows all the original functions,
walks you through the supporting functions I wrote for my algorithm and finally, walks
through the main algorithm.

To upload a program to the create:

1) Tools > [WinAVR] Make Clean

2) Tools > [WinAVR] Make All, this runs the compiler.

3) Tools > [WinAVR] Program, this uploads to the create

Important things to remember:
1) in the "makefile" on line 59, the TARGET should always be the current file
2) in the "makefile" on line 203, AVRDUDE_PORT should always be the correct port
3) when the create is connected via the USB hit the red "reset" button before uploading
4) to run a debugging program with the USB cord attached, hold the "black" button while you
press the "reset" button
5) to run a program without the cord attached, just hit "reset"
*/

// Includes
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include "oi.h" //all the constants that are used throughout the code (e.g.,[CmdSensors])

// Constants
#define START_SONG 0
#define ALARM_SONG 1

#define USB 1
#define CR8 2 // toggle between usb and create on CM serial processor

// Global variables
volatile uint16_t timer_cnt = 0; //original variables
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;
//added variables
volatile uint8_t posA, posB, posC; //prepare variables to hold light values
volatile uint8_t prevreading=0; //used in the FindMax() function
volatile uint8_t min=0; //receives the minimum light value from the sensors
volatile int16_t direction=0; //receives the new direction for the robot to drive
volatile uint16_t timer=500; //used in the delayMs() function
volatile uint8_t sensevalue; //holds wheel drop and cliff sense data
volatile uint8_t boolflag=0; //helps to steer the robot more accurately

// Functions
void byteTx(uint8_t value);/*------------------------built in function used for issuing commands.
it can send data to parts of the robot, or
back through the USB port*/
void delayMs(uint16_t time_ms);/*--------------------built in delay function*/
void delayAndUpdateSensors(unsigned int time_ms);/*-built in function, checks sensors*/
void initialize(void);/*-----------------------------built in function but highly modified by me.
see function for spefics*/
void powerOnRobot(void);/*---------------------------built in, self explanatory*/
void baud(uint8_t baud_code);/*----------------------built in, changes baud rate. need to change
baud rate when sending data back through USB*/
void drive(int8_t velocityhigh, int8_t velocitylow, int8_t radiushigh, int8_t radiuslow);
/*this is my modified drive function. it is the
same as the old one, however you have more
control when setting speeds. there are two
velocity bits and two radius bits. changing
these values can cause the create to turn in
very specific ways*/
void olddrive(int16_t velocity, int16_t radius);/*---build in function, new name. this function
gives basic drive instructions. the modified
function makes controlling easier*/
uint16_t randomAngle(void);/*------------------------built in, sends back random angle*/
void defineSongs(void);/*------------------------- --built in, sets up songs*/
void writeChar(char c, uint8_t com);/*------------ --taken from command modual manual. sends data
to computer via the USB cable*/
uint8_t GetLightReading(uint8_t position); /*------ --returns the light reading for the requested
sensor*/
uint8_t FindMin(uint8_t posA, uint8_t posB, uint8_t posC);
/*returns the lowest value from the three
light sensors*/
uint8_t FindMax(void);/*-----------------------------returns a bool. this function determines
in what direction the light is brightest*/
int16_t FindDirection(void);/*-----------------------returns the new direction for the robot to
head.*/
uint8_t byteRx(void);/*------------------------------reads from the serial port*/
void delayAndUpdateSensors2(unsigned int time_ms);

int main (void)
{
// Set up Command module. All are original function calls
initialize();
LEDBothOff;
powerOnRobot();
byteTx(CmdStart);
baud(Baud28800); //28800 for normal operations
defineSongs();
byteTx(CmdControl);
byteTx(CmdFull);

// Play the reset song and wait while it plays
byteTx(CmdPlay);
byteTx(START_SONG);
delayAndUpdateSensors(750);

//This is where the algorithm happens, the infinite for loop runs until the power is shut off
for(;;){

//start by getting the light readings and determining which direction to head
posA = GetLightReading(0x05);
posB = GetLightReading(0x02);
posC = GetLightReading(0x03);
min = FindMin(posA, posB, posC);
direction = FindDirection();

delayAndUpdateSensors2(20); //request bump sensor status

//it bumped into something
if(sensevalue > 1){ //i could get specific, but if its not zero, then theres a bump

sensevalue = 0; //clear the old value so it doesn't freeze
//it bumped into something so back up, spin and find the max light
olddrive(0,RadStraight);
delayMs(1000);
olddrive(-200,RadStraight);
delayMs(1000);
olddrive(0,RadStraight);
delayMs(1000);

if(posB > posC){//turn right
while(FindMax()){
olddrive(120,-1);
delayAndUpdateSensors(500);
}
}//end if

else {//turn left
while(FindMax()){
olddrive(120,1);
delayAndUpdateSensors(500);
}
}//end else


boolflag = 0;


}
else{
//the main part of the seek algorithm.
//keeps searching until it runs into something
if (boolflag != 0) {

if(direction > 0){
if(direction > 150)
boolflag=0;
else{
timer = 1000;
direction = (direction/2);
drive(0,100,255,(uint8_t)direction);
delayAndUpdateSensors(20);
}

}
else {
if(direction < -150)
boolflag=0;
else{
timer = 1000;
direction = (direction/2);
drive(0,100,1,(uint8_t)direction);
delayAndUpdateSensors(20);
}

}

}//end if

//the angle is positive so turn clockwise
else if (direction >= 0){

if(direction <= 40){ //don't turn too sharp, it's almost straight on
timer = 1000;
direction = (direction/2);
drive(0,100,255,(uint8_t)direction);
boolflag = 1;
}
else if (direction >= 155) { //spin, you're the wrong way
timer = 20;
//determine where the light is brightest
while(FindMax()){
olddrive(120,-1);
delayAndUpdateSensors(500);
}
}
else{ //turn at a median rate, it's near the sides
timer = 500;
drive(0,100,255,3*((uint8_t)direction)); //veer right

//FOR DEBUGGING PURPOSES
//need the highest Baud rate to send data back through the USB
//baud(Baud115200);
//writeChar(((uint8_t)direction), USB); sends the data to the computer
//writeChar(posA, USB);
}
}//end else if

//the direction is negative so turn counter clockwise
else {

if (direction >= -40){ //don't turn too sharp, it's almost straight on
timer = 1000;
direction = (direction/2);
drive(0,100,1,(uint8_t)direction);
boolflag = 1;
}
else if (direction <= -155) { //spin, you're the wrong way
timer = 20;
while(FindMax()){
olddrive(120,1);
delayAndUpdateSensors(500);
}
}
else { //simply veer, it's between -120 and -30 degrees
timer = 500;
drive(0,100,1,3*((uint8_t)direction)); //veer left

}
}//end else

delayAndUpdateSensors(timer);

}//end else

}//end for
}

//********************************************************************************************
/*the following three functions came directly from the command module manual
they change the flow of data so that when byteTx is called it sends data
from the create to the computer through the USB cable. when it is done sending
data it returns the com port to its original state, giving commands to the create.
*/
//********************************************************************************************

void setSerial(uint8_t com)
{
if(com == USB)
PORTB |= 0x10;
else if(com == CR8)
PORTB &= ~0x10;
}

uint8_t getSerialDestination(void)
{
if (PORTB & 0x10)
return USB;
else
return CR8;
}

//write char is called whenever you want to send data to the computer
void writeChar(char c, uint8_t com)
{
uint8_t originalDestination = getSerialDestination();
if (com != originalDestination)
{
setSerial(com);
delayMs(5);
}
byteTx((uint8_t)(c));
delayMs(2); // Allow char to xmt.
if (com != originalDestination)
{
setSerial(originalDestination);
delayMs(5);
}
}

uint8_t byteRx(void)
{
while(!(UCSR0A & 0x80)) ;
/* wait until a byte is received */
return UDR0;
}

//********************************************************************************************
/*the next section of functions are from the original program. they are used throughout
the code. they are reliable functions so i have not gone into great detail explaining
how they work
*/
//********************************************************************************************

// Serial receive interupt 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 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;
}
}
}

// MY DELAY FUNCTION THAT SENDS BACK ONLY THE BUMP DATA
void delayAndUpdateSensors2(uint16_t time_ms)
{

timer_on = 1;
timer_cnt = time_ms;
while(timer_on)
{

byteTx(CmdSensors); //op code to request sensor data
byteTx(7); // ask specifically for bumps (it gives wheel drops too)
sensevalue = byteRx(); //store the bump data

}
}

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
// ***
// the baud rate must be cranked up to 115200 whenever sending data to the computer
// ***
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);
}
}

//original drive function
void olddrive(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));
}

// Return an angle value in the range 53 to 180 (degrees)
uint16_t randomAngle(void)
{
return (53 + ((uint16_t)(random() & 0xFF) >> 1));
}

// Define songs to be played later
void defineSongs(void)
{
// Start song
byteTx(CmdSong);
byteTx(START_SONG);
byteTx(5);
byteTx(60);
byteTx(24);
byteTx(79);
byteTx(24);
byteTx(75);
byteTx(24);
byteTx(72);
byteTx(24);
byteTx(76);
byteTx(48);

// Alarm song
byteTx(CmdSong);
byteTx(ALARM_SONG);
byteTx(7);
byteTx(86);
byteTx(24);
byteTx(83);
byteTx(24);
byteTx(86);
byteTx(24);
byteTx(83);
byteTx(24);
byteTx(86);
byteTx(24);
byteTx(83);
byteTx(24);
byteTx(0);
byteTx(96);
}
//********************************************************************************************
/*The next section is all the functions i wrote. i will go through in detail what all of them
do and how they perform the desired task.
*/
//********************************************************************************************

// modified drive function
/*1st:vel high 2: vel low, 3rd: radius high, 4th: radius low
When the 3rd byte is 255, it turns the create right. 1, turns it left*/
void drive(int8_t velocityhigh, int8_t velocitylow, int8_t radiushigh, int8_t radiuslow)
{
byteTx(CmdDrive);
byteTx(velocityhigh);
byteTx(velocitylow);
byteTx(radiushigh);
byteTx(radiuslow);
}

/*This function uses the ADMUX to get the light reading from the requested sensor*/
uint8_t GetLightReading(uint8_t position){

ADMUX &= ~0X0F; //clear the old ADC channel
ADMUX |= position; //select channel (position:left,right,center) SENSOR
ADCSRA |= 0x40; //gets the light reading
while(ADCSRA & 0x40) ; //waits until a reading is ready (not noticable)
return (ADC); //bring it back

}

/*This function finds the minimum light value across all three sensors
it looks like a big mess, but it performs flawlessly*/
uint8_t FindMin(uint8_t posA, uint8_t posB, uint8_t posC)
{
if(posA <= posB){
if(posA <= posC)
return (posA);
else if (posC <= posB)
return (posC);
}
else if(posB <= posC){
return (posB);
}
//else
return (posC);

}

uint8_t FindMax(void)
{
delayAndUpdateSensors(20);
posA = GetLightReading(0x05);
if(posA >= (prevreading-3)){ //the minus gives it a buffer cuz of the fluctuations
prevreading = posA;
return(1);//keep turning
}
else{
prevreading = posA;
return(0);//stop turning, the max has been reached
}
}

/*Finds the new direction of travel. developed with help from DCP, this algorithm takes the
minimum light value and finds the direction accordingly. posA is in the front of the
robot, posB is on the right side and posC is on the left side. they are positioned 120
degrees apart from eachother */
int16_t FindDirection(void){
//c is min, light source is between a and b
if (posC == min)
direction = 120 * ((double)posB/((double)posA + (double)posB));

//a is min, light source is between b and c
if (posA == min)
direction = 120+(120 * ((double)posC/((double)posB + (double)posC)));

//b is min, light source is between a and c
if (posB == min)
direction = 240+(120 * ((double)posA/((double)posA + (double)posC)));

//if the direction is greater than 180 its faster to turn left rather than swing way right
if (direction > 180)
direction = direction - 360;

return(direction);
}

/* Initialize the Mind Control's ATmega168 microcontroller
This is a built in function that has some extremely important modifications by me.
i also went in and explained what some of the lines of code do and how important some are*/

void initialize(void)
{
cli();

// Set I/O pins - i did not modify any of the I/O pins, these are there original settings
DDRB = 0x10;
PORTB = 0xCF;
DDRC = 0x00; /*sets everything as inputs, the commented out step flips only the inputs used
if you were to put hook up a motor or anything else that was used as output
you would need to be more specific in which pins are input and which are
output. the following line is more specific.
DDRC &= ~0xC2C; set C2, C3 and C5 as inputs, this is more specific, works same as all 0's*/
PORTC = 0xFF;
DDRD = 0xE6;
PORTD = 0x7D;

// Set up timer 1 to generate an interrupt every 1 ms- i did not modify any of this code
TCCR1A = 0x00;
TCCR1B = (_BV(WGM12) | _BV(CS12));
OCR1A = 71;
TIMSK1 = _BV(OCIE1A);

UBRR0 = 19;
UCSR0B = 0x18;
UCSR0C = 0x06;

//The next lines are extremely important
DIDR0 |= 0x2C; //disable digital input on C2, C3 and C5. we are only concerned with analog
/*
DIDR0 0 0 1 0 1 1 0 0
C5 C3 C2
*/
//The next steps are all critical and never change when dealing with different pins
PRR &= ~_BV(PRADC); // Turn off ADC power save
ADCSRA = (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0)); // Enabled, prescaler = 128
ADMUX = (_BV(REFS0) );//voltage reference for a light sensor, different with different device
// Turn on interrupts
sei();
}