/**
*
* Same function as turn_clockwise but moves counter clockwise
*
* @param sensor the desired sensor to read and update
* @param degrees the amount of rotation clockwise
*/
void turn_counter_clockwise(oi_t *sensor, int degrees){
// if the current state of the iRobot has an error, return
if(errorDetection(1) == 1){
serial_puts("CAN'T MOVE!\n\r\n\r");
return;
}
int sum = 0;
oi_set_wheels(225, -225); // move forward; 150/500 speed
while (sum < degrees) {
if(errorDetection(1) == 1)
{
char error[30];
sprintf(error, "\n\rSTOPPED DUE TO SENSORS!\n\rROTATED CCW ONLY %02d CM\n\r",
sum);
serial_puts(error);
oi_set_wheels(0, 0);
break;
}
wait_ms(10);
sum += sensor->angle;
}
oi_set_wheels(0, 0); // stop
errorDetection(0);
}
/**
*
* Same function as moveForward but updates and check if there's an error
*
* @param sensor the desired sensor to read and update
* @param centimeters the longest distance the iRobot will move
*/
void moveFowardUpdate(oi_t* sensor, int centimeters){
// if the current state of the iRobot has an error, return
if(errorDetection(0) != 0){
serial_puts("CAN'T MOVE!\n\r\n\r");
return;
}
int millimeters = centimeters * 10;
int sum = 0;
oi_set_wheels(150, 150); // move forward;
while (sum < millimeters) {
// check if there's an error detectiion while moving
// if so, break out the loop and stop
if(errorDetection(0) != 0)
{
char error[30];
sprintf(error, "\n\rSTOPPED DUE TO SENSORS!\n\rMOVED ONLY %02d CM\n\r",
sum/10);
serial_puts(error);
oi_set_wheels(0, 0);
break;
}
sum += sensor->distance;
}
oi_set_wheels(0, 0); // stop
oi_free(sensor);
}
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
}
/**
* 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;
}
/**
* 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;
}
/**
* Bump sensor function to detect if an object is bumped and prints to the putty which bumper is tripped.
* @param sensor_status - the robot
**/
int check_bump_sensor(oi_t *sensor_status)
{
int bump = oi_bump_status(sensor_status);
if (bump != 0 && old_bump != bump)
{
oi_set_wheels(0,0);
switch(bump)
{
case 1:
send_string("Right Bump Sensor Detected!");
break;
case 2:
send_string("Left Bump Sensor Detected!");
break;
case 3:
send_string("Both Bump Sensors Detected!");
}
}
old_bump = bump;
return bump;
}
void handleInput(servo_data_t *servo, unsigned int *leftWheelVelocity, unsigned int *rightWheelVelocity,
int *requestIrCalibration)
{
*requestIrCalibration = 0;
char c = '\0';
if (isAvailable0())
{
c = getChar0();
switch (c)
{
case '0':
servo_set_position_deg(servo, 0);
break;
case '4':
servo_set_position_deg(servo, 45);
break;
case '9':
servo_set_position_deg(servo, 90);
break;
case '3':
servo_set_position_deg(servo, 135);
break;
case '8':
servo_set_position_deg(servo, 180);
break;
case '+':
servo_increment_degrees(servo, 4);
break;
case '-':
servo_decrement_degrees(servo, 4);
break;
case 'e':
*leftWheelVelocity += 50;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
break;
case 'd':
*leftWheelVelocity -= 50;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
break;
case 'r':
*rightWheelVelocity += 50;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
break;
case 'f':
*rightWheelVelocity -= 50;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
break;
case 'F':
oi_full_mode();
printf0("Full Mode \r\n");
break;
case ' ':
*rightWheelVelocity = 0;
*leftWheelVelocity = 0;
oi_set_wheels(0, 0);
break;
case 'b':
*rightWheelVelocity = -50;
*leftWheelVelocity = -50;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
_delay_ms(4000); //
*rightWheelVelocity = 0;
*leftWheelVelocity = 0;
oi_set_wheels(*leftWheelVelocity, *rightWheelVelocity);
break;
case 'c':
*requestIrCalibration = 1;
break;
case 's':
oi_safe_mode();
printf0("safe mode\r\n");
break;
case 'R':
printf0("reset\r\n");
oi_reset();
break;
default:
break;
}
}
}
list_t *create_ir_lookup_table_from_ping(servo_data_t *servo, timer_prescaler_t prescaler)
{
int maxVoltage = 1023;
int minVoltage = 130;
int avg = 5;
int speed = 50; //50 mm per second
int delay_for_5_mm = 100;
oi_set_wheels(0, 0);
servo_set_position_deg(servo, 90);
printf0("Put roomba in front of flat surface like a wall. Press 'c' when ready...\r\n");
while (getChar0() != 'c')
printf0("Put roomba in front of flat surface like a wall. Press 'c' when ready...\r\n");
unsigned voltage = ir_read_voltage_avg(avg);
printf0("start: %u\r\n", voltage);
while (voltage != maxVoltage)
{
printf0("%u\r\n", voltage);
oi_set_wheels(-speed, -speed);
_delay_ms(2 * delay_for_5_mm); //1 cm
oi_set_wheels(0, 0);
voltage = ir_read_voltage_avg(avg);
}
_delay_ms(100);
unsigned peakVoltage_distance_mm = ping_mm_busy_wait(prescaler);
while (voltage == maxVoltage)
{
printf0("[%u v], %u p_mm \r\n", voltage, peakVoltage_distance_mm);
oi_set_wheels(-speed, -speed);
_delay_ms(delay_for_5_mm * 2);
oi_set_wheels(0, 0);
_delay_ms(100);
peakVoltage_distance_mm = ping_mm_busy_wait(prescaler);
voltage = ir_read_voltage_avg(avg);
}
list_t *ret = lalloc();
ladd(ret, (void *) new_ir_measurement(maxVoltage, peakVoltage_distance_mm));
unsigned oldMm = peakVoltage_distance_mm;
while (voltage > minVoltage)
{
_delay_ms(100);
unsigned mm = ping_mm_busy_wait(prescaler);
printf0("reading [%u v], %u mm >? %u oldMm \r\n", voltage, mm, oldMm);
while (mm <= oldMm)
{
printf0("Non increasing ping value: [OLD: %u] [NEW: %u]\r\n", oldMm, mm);
_delay_ms(100);
mm = ping_mm_busy_wait(prescaler);
oi_set_wheels(-speed, -speed);
_delay_ms(delay_for_5_mm); //5mm
oi_set_wheels(0, 0);
}
printf0(" adding [%u v], %u mm \r\n", voltage, mm);
ladd(ret, (void *) new_ir_measurement(voltage, mm));
oi_set_wheels(-speed, -speed);
_delay_ms(delay_for_5_mm * 2); //1cm
oi_set_wheels(0, 0);
voltage = ir_read_voltage_avg(avg);
oldMm = mm;
}
printf0("sorting...\r\n");
lmergesort(ret, 0, ret->length - 1, (int (*)(const void *, const void *)) compare_ir_measurements);
printf0("done\r\n");
return ret;
}
/**
*
* 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;
}
