/// Initialize the Create
void oi_init(oi_t *self)
{
lcd_clear();
lcd_puts("Starting OI...");
// Setup USART1 to communicate to the iRobot Create using serial (baud = 57600)
UBRR1L = 16; // UBRR = (FOSC/16/BAUD-1);
UCSR1B = (1 << RXEN) | (1 << TXEN);
UCSR1C = (3 << UCSZ10);
// Starts the SCI. Must be sent first
oi_byte_tx(OI_OPCODE_START);
oi_byte_tx(OI_OPCODE_BAUD);
oi_byte_tx(8); // baud code for 28800
wait_ms(100);
// Set the baud rate on the Cerebot II to match the Create's baud
UBRR1L = 33; // UBRR = (FOSC/16/BAUD-1);
// Use Full mode, unrestricted control
oi_byte_tx(OI_OPCODE_FULL);
oi_set_leds(1, 1, 7, 255);
oi_update(self);
oi_update(self); // call twice to clear distance/angle
lcd_clear();
}
void turn_counterclockwise(oi_t *sensor, int degrees)
{
int sum = 0;
oi_set_wheels(500, -500); // move forward; full speed
while (sum < degrees) {
oi_update(sensor_data);
sum += sensor_data->angle;
}
oi_set_wheels(0, 0); // stop
}
void move_backward(oi_t *sensor, int centimeters)
{
int sum = 0;
oi_set_wheels(-500, -500); // move forward; full speed
while (sum < centimeters) {
oi_update(sensor_data);
sum -= sensor_data->distance;
}
oi_set_wheels(0, 0); // stop
}
/**
* Virtual wall sensor function to detect the edge and prints to the putty that the wall was detected.
* @param sensor_status - the robot
**/
int check_virtual_wall(oi_t *sensor_status)
{
oi_update(sensor_status);
int status = sensor_status->virtual_wall;
if (status == 1 && old_wall == 0)
{
oi_set_wheels(0,0);
send_string("Virtual Wall Detected!");
}
old_wall = status;
return status;
}
void rotate_hare(int angle, int scale) {
OBJAFF *oa_curr= &oa_buffer[0];
OBJAFF *oa_new= &oa_buffer[2];
oa_identity(oa_curr);
oa_identity(oa_new);
oa_scale(oa_curr, scale, scale);
oa_rotate(oa_new, angle & SIN_MASK);
oa_postmultiply(oa_curr, oa_new);
oi_update(0, 1);
oa_update(0, 3);
}
/**
* Checks each of the cliff sensors and returns 1 if the sensor is tripped and prints to putty which sensor was tripped.
* @param sensor_status - the robot
**/
int check_cliff(oi_t *sensor_status)
{
int stop = 0;
oi_update(sensor_status);
if(old_cl != sensor_status->cliff_left)
{
old_cl = sensor_status->cliff_left;
if(old_cl == 1)
{
oi_set_wheels(0, 0);
stop = 1;
send_string("Cliff Left Detected!");
}
}
else if(old_cfl != sensor_status->cliff_frontleft)
{
old_cfl = sensor_status->cliff_frontleft;
if(old_cfl == 1)
{
oi_set_wheels(0, 0);
stop = 1;
send_string("Cliff Front Left Detected!");
}
}
else if(old_cfr != sensor_status->cliff_frontright)
{
old_cfr = sensor_status->cliff_frontright;
if(old_cfr == 1)
{
oi_set_wheels(0, 0);
stop = 1;
send_string("Cliff Front Right Detected!");
}
}
else if(old_cr != sensor_status->cliff_right)
{
old_cr = sensor_status->cliff_right;
if(old_cr == 1)
{
oi_set_wheels(0, 0);
stop = 1;
send_string("Cliff Right Detected!");
}
}
return stop;
}
/*
*@apram sensor_status (structor that holds off os the info about the robot)
*/
int update_cliff(oi_t *sensor_status)
{
int stop = 0;
oi_update(sensor_status);
if(old_cl != sensor_status->cliff_left)
{
old_cl = sensor_status->cliff_left;
if(old_cl == 1)
{
stop = 1;
oi_set_wheels(0, 0);
}
}
else if(old_cfl != sensor_status->cliff_frontleft)
{
old_cfl = sensor_status->cliff_frontleft;
if(old_cfl == 1)
{
stop = 1;
oi_set_wheels(0, 0);
}
}
else if(old_cfr != sensor_status->cliff_frontright)
{
old_cfr = sensor_status->cliff_frontright;
if(old_cfr == 1)
{
stop = 1;
oi_set_wheels(0, 0);
}
}
else if(old_cr != sensor_status->cliff_right)
{
old_cr = sensor_status->cliff_right;
if(old_cr == 1)
{
stop = 1;
oi_set_wheels(0, 0);
}
}
return stop;
}
//Handler for OI, moved from control.
void oi_system()
{
movement_data_t movement_data;
rotation_data_t rotation_data;
enum {
oi_command_init = 0,
oi_command_move = 1,
oi_command_rotate = 2,
oi_command_play_song = 3,
oi_command_dump = 4,
oi_command_end_sequence = 5
} oi_command = usart_rx();
txq_enqueue(oi_command);
switch (oi_command) {
case oi_command_init:
oi_init(&(control.oi_state));
break;
case oi_command_move:
if(rx_frame()) {
r_error(error_frame,"Move should not have multiple frames.");
}
struct {
uint16_t speed;
uint16_t dist;
bool stream;
} *move_data = (void *) &control.data;
struct {
uint16_t dist;
uint8_t flag;
} *response_move = (void *) &control.data;
#warning "Stream functionality to be implemented later."
//Stream returns the distance traveled
//lcd_puts ("In OI subsystem"); //debug
movement_data = move_dist(&(control.oi_state), move_data->dist, move_data->speed);
response_move->dist = movement_data.travelled;
response_move->flag = movement_data.flag;
if (movement_data.flag != 0) {
movement_data = move_dist(&(control.oi_state), -20, move_data->speed);
response_move->dist += movement_data.travelled;
}
control.data_len = 3;
tx_frame(false);
break;
case oi_command_rotate:
if (rx_frame()) {
r_error(error_bad_message, "Rotate should only have one data frame.");
}
int16_t *angle = &(control.data[0]); // TODO: test if this is the right number of bytes
if (control.data_len != 2/*sizeof(*angle)*/) { // TODO: hardcoding to debug
r_error(error_bad_message, "Received too much data with rotate "
"message.");
}
struct {
uint16_t rotation;
} *response_rotation = (void *) &control.data;
rotation_data = turn(&(control.oi_state), *angle);
response_rotation->rotation = rotation_data.rotated;
control.data_len = 2;
tx_frame(false);
break;
//Sing me a song.
case oi_command_play_song:
#warning "oi_command_play_song is deprecated"
;
//assuming that we get two data frames, the first containing the notes and the second containing the durations.
int j;
struct {
uint8_t n; //The number of notes present in the song
//char data[n];
uint8_t index;
} *song_data = (void *) &control.data;
// int j;
while(rx_frame()) { //this should happen twice please
char tmp_notes[song_data->n];
char tmp_durs[song_data->n];
for(j =0; j<song_data->n; j++) {
//tmp_notes[n]; TODO: broken
#warning "oi_command_play_song not implemented"
}
}
break;
case oi_command_dump:
//copies all of the data from OI_UPDATE and transmits to Control.
oi_update(&(control.oi_state));
memcpy(control.data, &control.oi_state, sizeof(control.oi_state));
control.data_len = sizeof(control.oi_state);
tx_frame(false);
break;
case oi_command_end_sequence:
#warning "oi_command_end_sequence not implemented"
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED to orange
oi_set_leds(1, 1, 170, 255);
wait_ms(50);
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED to yellow
oi_set_leds(1, 1, 70, 255);
wait_ms(50);
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED back to default state (from oi_init())
oi_set_leds(1, 1, 7, 255);
// Play song
songs_load(RICK_ROLL);
break;
default:
r_error(error_bad_message, "Bad OI Command");
break;
}
}
/**
* 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);
}
}
}
/**
* The main loop which waits for commands via blue tooth and
* then acts based on commands received.
*
* @author Jacob Moyer,
* Ed Droesch,
* Aaron Pederson, &
* Matthew Backes
*
* @date 12/13/2013
*/
void main()
{
///Initializes the open_interface struct to be used throughout the program.
oi_t *sensor_data = oi_alloc();
oi_init(sensor_data);
initializer();
char key =0;
///Waits for commands from the USART connection,
///Then performs the command and returns that the command was performed.
while(1)
{
char message[32] = "";
key = USART_Receive();
///Returns what the current tape sensor is reading.
if (key == 'p')
{
oi_update(sensor_data);
sprintf(message,"sensor->signal =%d\r\n",(int)sensor_data->cliff_frontleft_signal);
string_tran(message);
}
///Moves the servo motor to the 0 degree position.
if(key == 'c')
{
calibrate_servo(0);
}
///Scans 0 - 180 degrees in front of the robot and returns the readings,
///it also guesses where objects are and returns their angle and width.
if(key == 'j')
{
fullScan();
string_tran("Scan Complete.\r\n");
}
///Moves FORWARD_DISTANCE centimeters forward.
if(key =='w')
{
sprintf(message,"Moved %d\r\n",move(sensor_data, FORWARD_DISTANCE,&sensors));
string_tran(message);
}
///Moves BACKWARD_DISTANCE centimeters backwards.
if(key=='s')
{
sprintf(message,"Moved %d\r\n",move(sensor_data, BACKWARD_DISTANCE,&sensors));
string_tran(message);
}
///Rotates ROTATE_DEGREES counterclockwise.
if(key=='a')
{
turn_CCW(sensor_data,ROTATE_DEGREES);
sprintf(message,"Rotated %d\r\n",ROTATE_DEGREES);
string_tran(message);
}
///Rotates ROTATE_DEGREES clockwise.
if(key=='d')
{
turn_CCW(sensor_data,-ROTATE_DEGREES);
sprintf(message,"Rotated %d\r\n",-ROTATE_DEGREES);
string_tran(message);
}
///Plays the Morrowind theme song.
if(key=='r')
{
play_morrowind();
string_tran("Playing song.");
}
}
}
/**
*
* Function that detects the error and updates the state of the iRobot
*
* @param isRotating 0 if it's not rotating, 1 if it is
* @return returns 1 if there's an error detected, 2 if it's only a light sensor, else 0
*/
int errorDetection(int isRotating)
{
// update the sensors
oi_update(sensor_data);
// if there's an error, set to 1
int detection = 0;
/// light errors///
/*
// left sensor to detect white tape
if(sensor_data->cliff_left_signal > WHITE_MIN)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_LEFT_SIGNAL] = 1;
}
// left sensor to detect black tape
else if(sensor_data->cliff_left_signal < BLACK_MAX)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_LEFT_SIGNAL] = 2;
}
else
sensorArray[CLIFF_LEFT_SIGNAL] = 0;
// front left sensor to detect white tape
if(sensor_data->cliff_frontleft_signal > WHITE_MIN)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_FRONT_LEFT_SIGNAL] = 1;
}
// front left sensor to detect black tape
else if(sensor_data->cliff_frontleft_signal < BLACK_MAX)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_FRONT_LEFT_SIGNAL] = 2;
}
else
sensorArray[CLIFF_FRONT_LEFT_SIGNAL] = 0;
// front right sensor to detect white tape
if(sensor_data->cliff_frontright_signal > WHITE_MIN)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_FRONT_RIGHT_SIGNAL] = 1;
}
// front right sensor to detect black tape
else if(sensor_data->cliff_frontright_signal < BLACK_MAX)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_FRONT_RIGHT_SIGNAL] = 2;
}
else
sensorArray[CLIFF_FRONT_RIGHT_SIGNAL] = 0;
// right sensor to detect white tape
if(sensor_data->cliff_right_signal > WHITE_MIN)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_RIGHT_SIGNAL] = 1;
}
// right sensor to detect black tape
else if(sensor_data->cliff_right_signal < BLACK_MAX)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 2;
sensorArray[CLIFF_RIGHT_SIGNAL] = 2;
}
else
sensorArray[CLIFF_RIGHT_SIGNAL] = 0;
*/
/// bumper errors ///
// left bumper
if(sensor_data->bumper_left == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[LEFT_BUMPER] = 1;
}
else
sensorArray[LEFT_BUMPER] = 0;
// right bumper
if(sensor_data->bumper_right == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[RIGHT_BUMPER] = 1;
}
else
sensorArray[RIGHT_BUMPER] = 0;
/// cliff errors ///
// left cliff
if(sensor_data->cliff_left == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[CLIFF_LEFT] = 1;
}
else
sensorArray[CLIFF_LEFT] = 0;
// front left cliff
if(sensor_data->cliff_frontleft == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[CLIFF_FRONT_LEFT] = 1;
}
else
sensorArray[CLIFF_FRONT_LEFT] = 0;
// front right cliff
if(sensor_data->cliff_frontright == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[CLIFF_FRONT_RIGHT] = 1;
}
else
sensorArray[CLIFF_FRONT_RIGHT] = 0;
// right cliff
if(sensor_data->cliff_right == 1)
{
// stop the iRobot
oi_set_wheels(0, 0);
// set detection to 1
detection = 1;
sensorArray[CLIFF_RIGHT] = 1;
}
else
sensorArray[CLIFF_RIGHT] = 0;
// if there's an error, print the status of the sensors
if(detection != 0 && isRotating == 0);
printSensorStatus(sensorArray);
return detection;
}
/**
* 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]);
}
}
}
}
}
