/** readSensorsBranches ******************************************
* Read the IR line sensors and return an interpreted result.
*
* Sensors are numbered 0 - 4 starting at the left. This version
* returns a value as for readLineSensors(), but also sets isLeft
* and isRight to indicate if there is a line under sensor 0 (isLeft) and/or
* sensor 4 (isRight). If either isLeft or isRight is true then the value returned
* shouldn't be trusted for steering since
*
* @modifies isLeft is true if and only if there is a line detected directly
* under the left sensor (0)
* isRight is true if and only if there is a line detected directly
* under the right sensor (4)
* @returns if !(isLeft || isRight) then the value can be interpreted as follows:
* 0 -- line sensed under leftmost sensor (0) only
* or no line detected
* 1 - 999 -- line sensed under left two sensors (0 and 1)
* 1000 - 1999 -- line sensed under sensors 1 and 2
* 2000 -- line sensed under sensors 1, 2 and 3
* 2001 - 2999 -- line sensed under sensors 2 and 3
* 3000 - 3999 -- line sensed under sensors 3 and 4
* 4000 -- line sensed under rightmost sensor (4) only
* or no line detected
*/
int readSensorsBranches(bool& isLeft, bool& isRight) {
unsigned int sensors[5];
int result = read_line(sensors, IR_EMITTERS_ON);
isLeft = sensors[0] > 500;
isRight = sensors[4] > 500;
display_readings(sensors);
return result;
}
/**lineCalibration*****************************************
* @descrip: a function to calibrate the sensors and display
* the results in the form of a bar graph. It waits for a button
* to be pressed.
* @param: none
* @returns: BUTTON_A, BUTTON_B or BUTTON_C depending on which was
* pressed after the calibration
*
***********************************************************/
unsigned char lineCalibration() {
const int TURN_SPEED = 40; // motor speed for turning
const int TURN_STEPS = 20; // number of samples in a half turn
const int SAMPLE_DELAY = 20; // ms between samples
unsigned char button; // which button is pressed at the end
set_motors(-TURN_SPEED, TURN_SPEED); // start turning left
for (int i = 0; i < TURN_STEPS; i++) {// some number of times
calibrate_line_sensors(IR_EMITTERS_ON);
delay_ms(SAMPLE_DELAY); // wait 20ms
}
set_motors(TURN_SPEED, -TURN_SPEED); // start turning right
for (int i = 0; i < 2*TURN_STEPS; i++) {// twice as many times
calibrate_line_sensors(IR_EMITTERS_ON);
delay_ms(SAMPLE_DELAY); // wait 20ms
}
set_motors(-TURN_SPEED, TURN_SPEED); // start turning left
for (int i = 0; i < TURN_STEPS; i++) {// some number of times
calibrate_line_sensors(IR_EMITTERS_ON);
delay_ms(SAMPLE_DELAY); // wait 20ms
}
set_motors(0,0);
// Display calibrated values as a bar graph.
unsigned int sensor[5]; // place to store sensor readings
while ((button = button_is_pressed(ANY_BUTTON)) == 0) {// as long as a button is not pressed
unsigned int position = read_line(sensor, IR_EMITTERS_ON); // read the sensors
clear();
print_long(position); // display the value on the top line
display_readings(sensor); // display the bar graph on the bottom line
delay_ms(100); // wait a bit
}
play_from_program_space(beep_button_b); // beep for B button
while (button_is_pressed(button)) ; // empty loop - wait until button released
delay_ms(200); // wait a bit more
return button;
}
// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.
void initialize()
{
unsigned int counter; // used as a simple timer
unsigned int sensors[5]; // an array to hold sensor values
// This must be called at the beginning of 3pi code, to set up the
// sensors. We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
pololu_3pi_init(2000);
load_custom_characters(); // load the custom characters
// Play welcome music and display a message
print_from_program_space(welcome_line1);
lcd_goto_xy(0,1);
print_from_program_space(welcome_line2);
play_from_program_space(welcome);
delay_ms(1000);
clear();
print_from_program_space(demo_name_line1);
lcd_goto_xy(0,1);
print_from_program_space(demo_name_line2);
delay_ms(1000);
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
int bat = read_battery_millivolts();
clear();
print_long(bat);
print("mV");
lcd_goto_xy(0,1);
print("Press B");
delay_ms(100);
}
// Always wait for the button to be released so that 3pi doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_B);
delay_ms(1000);
// Auto-calibration: turn right and left while calibrating the
// sensors.
for(counter=0;counter<80;counter++)
{
if(counter < 20 || counter >= 60)
set_motors(40,-40);
else
set_motors(-40,40);
// This function records a set of sensor readings and keeps
// track of the minimum and maximum values encountered. The
// IR_EMITTERS_ON argument means that the IR LEDs will be
// turned on during the reading, which is usually what you
// want.
calibrate_line_sensors(IR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);
// Display calibrated values as a bar graph.
while(!button_is_pressed(BUTTON_B))
{
// Read the sensor values and get the position measurement.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);
// Display the position measurement, which will go from 0
// (when the leftmost sensor is over the line) to 4000 (when
// the rightmost sensor is over the line) on the 3pi, along
// with a bar graph of the sensor readings. This allows you
// to make sure the robot is ready to go.
clear();
print_long(position);
lcd_goto_xy(0,1);
display_readings(sensors);
delay_ms(100);
}
wait_for_button_release(BUTTON_B);
clear();
print("Go!");
// Play music and wait for it to finish before we start driving.
play_from_program_space(go);
while(is_playing());
}
/** readLineSensors ******************************************
* Read the IR line sensors and return an interpreted result.
*
* Sensors are numbered 0 - 4 starting at the left. The interpretation
* assumes that the robot is following a 3/4" wide black line with no
* sharp turns and no branches.
*
* @returns 0 -- line sensed under leftmost sensor (0) only
* or no line detected
* 1 - 999 -- line sensed under left two sensors (0 and 1)
* 1000 - 1999 -- line sensed under sensors 1 and 2
* 2000 -- line sensed under sensors 1, 2 and 3
* 2001 - 2999 -- line sensed under sensors 2 and 3
* 3000 - 3999 -- line sensed under sensors 3 and 4
* 4000 -- line sensed under rightmost sensor (4) only
* or no line detected
*/
int readLineSensors() {
unsigned int sensors[5];
int result = read_line(sensors, IR_EMITTERS_ON);
display_readings(sensors);
return result;
}