// Runs through an automatic calibration sequence
void auto_calibrate()
{
time_reset();
set_motors(60, -60);
while(get_ms() < 250)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(-60, 60);
while(get_ms() < 750)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(60, -60);
while(get_ms() < 1000)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(0, 0);
serial_send_blocking("c",1);
}
/**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;
}
void initialize()
{
unsigned int counter; // used as a simple timer
// 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);
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
lcd_goto_xy(0,0);
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);
}
// 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());
}
int main()
{
// wait
wait_with_message("Press B");
// init and calibrate light sensors
unsigned int sensors[5];
pololu_3pi_init_disable_emitter_pin(2000); // (2000 for the timeout corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor)
wait_for_button_release(BUTTON_B);
delay_ms(1000);
for(int counter=0;counter<80;counter++)
{
if(counter < 20 || counter >= 60)
set_motors(40,-40);
else
set_motors(-40,40);
calibrate_line_sensors(IR_EMITTERS_ON);
delay_ms(20);
}
set_motors(0,0);
// init IR sensors
set_analog_mode(MODE_8_BIT);
DDRC &= ~(1<< PORTC5);
PORTC &= ~(1<< PORTC5);
// wait
wait_with_message("Press B");
delay_ms(1000);
int left_speed = 110;
int right_speed = 110;
int set_point = 0;
while(1)
{
// check light sensors for our boundary
unsigned int position = read_line(sensors,IR_EMITTERS_ON);
if(position > 5 && position < 1000)
{
turn_right(55,65);
}
else if(position > 1000 && position < 1800)
{
turn_right(55,95);
}
else if(position > 1800 && position < 3000)
{
turn_left(55,95);
}
else if(position > 3000 && position < 3995)
{
turn_left(55,65);
}
// check IR sensors
int left = analog_read(6);
int right = analog_read(5);
int front = analog_read(7);
/*if((get_ms() % 300) == 0)
{
clear();
lcd_goto_xy(0,0);
print_long(left);
lcd_goto_xy(0,1);
print_long(right);
}*/
int balance = 0;
if (left > 20 || right > 20)
{
balance = right - left - 20;
}
if (set_point == 0 && front > 162)
{
set_point = 1;
set_motors(25,25);
}
else
{
set_motors(left_speed + balance,right_speed - balance);
}
}
halt();
clear();
//print("f=");
//print_long(front);
// end
while(1);
}
void auto_test_sensor_range()
{
clear();
print("AutoTest");
lcd_goto_xy(0,1);
print("B");
while(!button_is_pressed(BUTTON_B));
while(button_is_pressed(ALL_BUTTONS));
delay_ms(300);
clear();
// Auto calibrate with IR on
time_reset();
set_motors(20, -20);
while(get_ms() < 650)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(-20, 20);
while(get_ms() < 1950)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(20, -20);
while(get_ms() < 2600)
calibrate_line_sensors(IR_EMITTERS_ON);
set_motors(0, 0);
unsigned int * maximum_calibration_values = get_line_sensors_calibrated_maximum_on();
unsigned int * minimum_calibration_values = get_line_sensors_calibrated_minimum_on();
unsigned char k;
unsigned char readings_ok = 1;
unsigned char ok_array[5] = {'G','G','G','G','G'};
for (k=0;k<5;k++)
{
if ((maximum_calibration_values[k] - minimum_calibration_values[k]) > 1000)
{
if ((minimum_calibration_values[k] * 2) < maximum_calibration_values[k])
{
if (minimum_calibration_values[k] < 1600)
{
continue;
}
else
{
ok_array[k] = 'H';
}
}
else
{
ok_array[k] = 'M';
}
}
else
{
ok_array[k] = 'R';
}
readings_ok = 0;
}
if (!readings_ok)
{
clear();
print("QTR FAIL");
lcd_goto_xy(0,1);
for (k=0;k<5;k++)
{
print_character(ok_array[k]);
}
print(" C ");
while(!button_is_pressed(BUTTON_C));
while(button_is_pressed(ALL_BUTTONS));
}
}
int main()
{
pololu_3pi_init(2000);
play_mode(PLAY_CHECK);
clear();
// start receiving data at 115.2 kbaud
serial_set_baud_rate(115200);
serial_receive_ring(buffer, 100);
while(1)
{
// wait for a command
char command = read_next_byte();
// The list of commands is below: add your own simply by
// choosing a command byte and introducing another case
// statement.
switch(command)
{
case (char)0x00:
// silent error - probable master resetting
break;
case (char)0x81:
send_signature();
break;
case (char)0x86:
send_raw_sensor_values();
break;
case (char)0x87:
send_calibrated_sensor_values(1);
break;
case (char)0xB0:
send_trimpot();
break;
case (char)0xB1:
send_battery_millivolts();
break;
case (char)0xB3:
do_play();
break;
case (char)0xB4:
calibrate_line_sensors(IR_EMITTERS_ON);
send_calibrated_sensor_values(1);
break;
case (char)0xB5:
line_sensors_reset_calibration();
break;
case (char)0xB6:
send_line_position();
break;
case (char)0xB7:
do_clear();
break;
case (char)0xB8:
do_print();
break;
case (char)0xB9:
do_lcd_goto_xy();
break;
case (char)0xBA:
auto_calibrate();
break;
case (char)0xBB:
set_pid();
break;
case (char)0xBC:
stop_pid();
break;
case (char)0xC1:
m1_forward();
break;
case (char)0xC2:
m1_backward();
break;
case (char)0xC5:
m2_forward();
break;
case (char)0xC6:
m2_backward();
break;
default:
clear();
print("Bad cmd");
lcd_goto_xy(0,1);
print_hex_byte(command);
play("o7l16crc");
continue; // bad command
}
}
}