/** * * 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; }