/**
*
* Function to stop the robot when finished with the course
*/
void finish()
{
// TODO
// blink led power light 3 times
// play victory song (Final Fantasy
oi_play_song(2);
// print to putty victory message
serial_puts("SWEET, SWEET, SWEET VICTORY");
}
/**
*
* Function to let the robot be in idle until we start
*/
void start()
{
// stay in while loop until ready when 1 is entered
while(1)
{
if(serial_getc() == '1')
break;
}
// play introduction song (Lost Woods)
oi_play_song(3);
}
/**
* Runs the main part of the program. Note it is an infinite loop.
* @author Group B1
* @date 12/4/2012
*/
void run_robot(oi_t *oi)
{
int x = STARTING_X_POS;
int y = STARTING_Y_POS;
int o = STARTING_ORENTATION;
int msg = 0;
while(1){
char c = read_one_char();
oi_update(oi);
printf("11 %d\n", oi->virtual_wall);
lprintf("\n Group B1\n\n\n\n");
lprintf("%d %d %d %d\n%d", oi->cliff_left_signal , oi->cliff_frontleft_signal , oi->cliff_frontright_signal , oi->cliff_right_signal, oi->virtual_wall );
if( oi->cliff_left_signal > 1000 && oi->cliff_frontleft_signal > 1000 && oi->cliff_frontright_signal > 1000 && oi->cliff_right_signal > 1000 ){
printf("10\n");
}
if(c == FORWARD)
{
if(move_forward(oi, DEFAULT_DISTANCE) >= DEFAULT_DISTANCE){
if(o == NORTH){
y = y + 1 * STEP;
}
else if(o == EAST){
x = x + 1 * STEP;
}
else if(o == SOUTH){
y = y - 1 * STEP;
}
else if(o == WEST){
x = x - 1 * STEP;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
}
else if(c == RIGHT_TURN)
{
turn_clockwise();
if(o == NORTH){
o = WEST;
}
else if(o == EAST){
o = NORTH;
}
else if(o == SOUTH){
o = EAST;
}
else if(o == WEST){
o = SOUTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == LEFT_TURN)
{
turn_counter_clockwise();
if(o == NORTH){
o = EAST;
}
else if(o == EAST){
o = SOUTH;
}
else if(o == SOUTH){
o = WEST;
}
else if(o == WEST){
o = NORTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == RESET)
{
move_servo(0);
x = STARTING_X_POS;
y = STARTING_Y_POS;
o = STARTING_ORENTATION;
msg = 0;
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == MUSIC)
{
load_song();
oi_play_song(JAWS);
}
else if(c == SCAN)
{
oi_update(oi);
printf("1 %d %d %d %d %d ", oi->virtual_wall ,x,y,o,msg);
for(int i = 0 ; i <= 1800 ;i = i + 1)
{
move_servo(i * 1);
int x = ir_distance();
printf("%d ", x);
}
printf("\n");
move_servo(0);
}
}
}
/**
*
* Function to control the iRobot depending on the character pressed, call it like keyboardInput(serial_getc());
* @param c the character that would determine if the iRobot moves, uses serial_getc()
*/
void keyboardInput(char c)
{
// toggle precision mode, if activated, move is 5 cm and 5 degrees
if(c == 'T')
{
if(precision == 0){
serial_puts("PRECISION ACTIVATED\n\r\n\r");
precision = 1;
}
else{
serial_puts("PRECISION DEACTIVATED\n\r\n\r");
precision = 0;
}
}
// move the iRobot forward, 10 cm
else if(c == 'W')
{
if(precision)
{
serial_puts("MOVING FORWARD 5 CM\n\r\n\r");
moveFowardUpdate(sensor_data, 5);
}
else
{
serial_puts("MOVING FORWARD 10 CM\n\r\n\r");
moveFowardUpdate(sensor_data, 10);
}
wait_ms(100);
}
// move the iRobot backwards, 10 cm
else if(c == 'S')
{
if(precision)
{
serial_puts("MOVING BACKWARD 5 CM\n\r\n\r");
moveBackward(sensor_data, 5);
}
else
{
serial_puts("MOVING BACKWARD 10 CM\n\r\n\r");
moveBackward(sensor_data, 10);
}
wait_ms(100);
}
// rotate the iRobot counter clockwise, 15 degrees
else if(c == 'A')
{
if(precision)
{
serial_puts("TURNING COUNTER CLOCKWISE 5 DEGREES\n\r\n\r");
turn_counter_clockwise(sensor_data, 5); // TODO
}
else
{
serial_puts("TURNING COUNTER CLOCKWISE 15 DEGREES\n\r\n\r");
turn_counter_clockwise(sensor_data, 15); // TODO
}
wait_ms(100);
}
// rotate the iRobot clockwise, 15 degrees
else if(c == 'D')
{
if(precision)
{
serial_puts("TURNING CLOCKWISE 5 DEGREEES\n\r\n\r");
turn_clockwise(sensor_data, 5); // TODO
}
else
{
serial_puts("TURNING CLOCKWISE 15 DEGREEES\n\r\n\r");
turn_clockwise(sensor_data, 15); // TODO
}
wait_ms(100);
}
// start sweeping for ir and sonar data
else if(c == ' ')
{
oi_play_song(0);
serial_puts("SWEEPING FOR OBJECTS\n\r");
smallestObjectSweep();
wait_ms(100);
}
// clear screen
else if(c == '-')
{
clearScreen();
wait_ms(100);
}
// finish command
else if(c == 'f')
{
if(serial_getc == 'i')
{
if(serial_getc == 'n')
{
if(serial_getc == 'i')
{
if(serial_getc == 's')
{
if(serial_getc == 'h')
{
isFinished = 1;
}
}
}
}
}
}
// if any other key is pressed, nothing happens
}
/**
* Used to control the robot.
* Receive and transmit data, measure the distance from object and navigate to the retrieval zone.
**/
int main(void)
{
lcd_init();
timer3_init();
timer_init();
ADC_init();
USART_Init();
oi_t *sensor_data = oi_alloc();
oi_init(sensor_data);//should turn the iRobot Create's power LED yellow
lcd_init();
serial_puts("Start");
//USART_Transmit(13);
//USART_Transmit(10);
int TempAngle[4] = {0,0,0,0};
int TempIR[4] = {0,0,0,0};
int pos[4] = {0,0,0,0};
int AddIR[4] = {0,0,0,0};
int count[4] = {0,0,0,0};
int found = 0;
int x1 = 0;
int x2 = 0;
int x3 = 0;
int x4 = 0;
unsigned angle = 0;
unsigned char IR = 0;
volatile int i=0;
volatile int x = 0;
char command;
char display[100];
char display1[20];
char display2[20];
char display3[20];
char display4[20];
char display5[100];
char display6[100];
while (1)
{
command = USART_Recieve();
USART_Transmit(command);
//USART_Transmit(13);
//USART_Transmit(10);
if (command == '1')
{
found = 0;
angle = 0;
int t;
int TempAngle[6] = {0, 0,0,0,0,0};
int TempIR[6] = {0, 0,0,0,0,0};
int pos[6] = {0, 0,0,0,0,0};
int AddIR[6] = {0, 0, 0,0,0,0};
int count[6] = {0, 0, 0,0,0,0};
for (angle = 0;angle < 181;angle++)
{
move_servo(angle);
wait_ms(20);
IR = 0;
IR = 42800*pow(ADC_read(2),-1.23);
sprintf(display6, "Angle: %5d IR: %5d",angle,IR);
serial_puts(display6);
if (IR < 80)
{
TempAngle[found]++;
count[found]++;
AddIR[found]+=IR;
TempIR[found]=AddIR[found]/count[found];
}
else
{
if(TempAngle[found] < 5)
{
TempAngle[found] = 0;
}
else
{
pos[found] = angle- TempAngle[found]/2;
if (TempIR[found]*TempAngle[found]< 460)
{
USART_Transmit(13);
USART_Transmit(10);
for (int i = 0;i<strlen(s8);i++)
{
USART_Transmit(s8[i]);
}
sprintf(display5, "object position: %5d",pos[found]);
serial_puts(display5);
}
sprintf(display, "object position: %5d IR: %5d object size: %5d",pos[found],TempIR[found],TempAngle[found]);
serial_puts(display);
USART_Transmit(13);
USART_Transmit(10);
found++;
}
}
}
OCR3B = 1000-1; //return to 0 degree
}
if (command == 'w')
{
move_forward(sensor_data,20);
}
if (command == 's')
{
move_backforward(sensor_data,20);
}
if (command == 'a')
{
turn_clockwise(sensor_data,82);
}
if (command == 'd')
{
turn_counterclockwise(sensor_data,82);
}
if (command == 'q')
{
turn_clockwise(sensor_data,38);
}
if (command == 'e')
{
turn_counterclockwise(sensor_data, 38);
}
if (command == '8')
{
move_forward(sensor_data,5);
}
if (command == '5')
{
move_backforward(sensor_data,5);
}
if (command == 'p')
{
oi_t* sensor = oi_alloc();
oi_init(sensor);
load_songs();
oi_play_song(songings);
}
if(command == 'k')
{
oi_update(sensor_data);
x1 = sensor_data->cliff_left_signal;
x2 = sensor_data->cliff_right_signal;
x3 = sensor_data->cliff_frontleft_signal;
x4 = sensor_data->cliff_frontright_signal;
sprintf (display1, "left = %d",x1);
sprintf (display2, "right = %d",x2);
sprintf (display3, "front left = %d",x3);
sprintf (display4, "front right = %d",x4);
USART_Transmit(13);
USART_Transmit(10);
serial_puts(display1);
serial_puts(display3);
serial_puts(display4);
serial_puts(display2);
if (x1>500||x2>500||x3>500||x4>500)
{
USART_Transmit(13);
USART_Transmit(10);
for (int i=0;i<strlen(s6);i++)
{
USART_Transmit(s6[i]);
}
}
}
}
}
