void time_test()
{
static long elapsed_time = 0;
static long last_read = 0;
static long is_ticking = 0;
static char a_is_pressed = 0;
static char c_is_pressed = 0;
static char last_seconds = 0;
long current_time = get_ms();
if(is_ticking)
elapsed_time += current_time - last_read;
last_read = current_time;
if(button_is_pressed(BUTTON_A) && !a_is_pressed)
{
// reset
a_is_pressed = 1;
is_ticking = 0;
elapsed_time = 0;
if(!is_playing()) // only play once
play_from_program_space(beep_button_a);
}
// find the end of the button press without stopping
if(!button_is_pressed(BUTTON_A))
a_is_pressed = 0;
if(button_is_pressed(BUTTON_C) && !c_is_pressed)
{
// start/stop
c_is_pressed = 1;
is_ticking = !is_ticking;
play_from_program_space(beep_button_c);
}
// find the end of the button press without stopping
if(!button_is_pressed(BUTTON_C))
c_is_pressed = 0;
print_long((elapsed_time/1000/60/10)%10); // tens of minutes
print_long((elapsed_time/1000/60)%10); // minutes
print_character(':');
print_long((elapsed_time/1000)%60/10); // tens of seconds
char seconds = ((elapsed_time/1000)%60)%10;
print_long(seconds); // seconds
print_character('.');
print_long((elapsed_time/100)%10); // tenths of seconds
print_long((elapsed_time/10)%10); // hundredths of seconds
// beep every second
if(seconds != last_seconds && elapsed_time != 0 && !is_playing())
play_from_program_space(timer_tick);
last_seconds = seconds;
}
// waits for a button, plays the appropriate beep, and returns the
// button or buttons that were pressed
char wait_for_button_and_beep()
{
char button = wait_for_button_press(ANY_BUTTON);
if(button & BUTTON_A)
play_from_program_space(beep_button_a);
else if(button & BUTTON_B)
play_from_program_space(beep_button_b);
else
play_from_program_space(beep_button_c);
wait_for_button_release(button);
return button;
}
// process_received_byte: Responds to a byte that has been received on
// USB_COMM. If you are writing your own serial program, you can
// replace all the code in this function with your own custom behaviors.
void process_received_byte(char byte)
{
clear(); // clear LCD
print("RX: ");
print_character(byte);
lcd_goto_xy(0, 1); // go to start of second LCD row
switch(byte)
{
// If the character 'G' or 'g' is received, toggle the green LED.
case 'G':
case 'g':
green_led(TOGGLE);
print("green LED");
break;
// If the character 'R' or 'r' is received, toggle the red LED.
case 'R':
case 'r':
red_led(TOGGLE);
print("red LED");
break;
// If the character 'C' or 'c' is received, play the note C.
case 'C':
case 'c':
play_from_program_space(PSTR("c16"));
print("play note C");
break;
// If the character 'D' or 'd' is received, play the note D.
case 'D':
case 'd':
play_from_program_space(PSTR("d16"));
print("play note D");
break;
// If any other character is received, change its capitalization and
// send it back.
default:
wait_for_sending_to_finish();
send_buffer[0] = byte ^ 0x20;
serial_send(USB_COMM, send_buffer, 1);
print("TX: ");
print_character(send_buffer[0]);
break;
}
}
void music_test()
{
static char fugue_title_pos = 0;
static long last_shift = 0;
char c,i;
if(get_ms() - last_shift > 250)
{
for(i=0; i<8; i++)
{
c = pgm_read_byte(fugue_title + fugue_title_pos + i);
print_character(c);
}
last_shift = get_ms();
fugue_title_pos ++;
if(fugue_title_pos + 8 >= sizeof(fugue_title))
fugue_title_pos = 0;
}
if(!is_playing())
{
play_from_program_space(fugue);
}
delay_ms(100);
}
static portTASK_FUNCTION( vBlockingQueueProducer, pvParameters )
{
unsigned short usValue = 0;
xBlockingQueueParameters *pxQueueParameters;
short sErrorEverOccurred = pdFALSE;
pxQueueParameters = ( xBlockingQueueParameters * ) pvParameters;
for( ;; )
{
if( xQueueSend( pxQueueParameters->xQueue, ( void * ) &usValue, pxQueueParameters->xBlockTime ) != pdPASS )
{
sErrorEverOccurred = pdTRUE;
}
else
{
/* We have successfully posted a message, so increment the variable
used to check we are still running. */
if( sErrorEverOccurred == pdFALSE )
{
( *pxQueueParameters->psCheckVariable )++;
}
/* Increment the variable we are going to post next time round. The
consumer will expect the numbers to follow in numerical order. */
++usValue;
play_from_program_space(PSTR("d"));
while (is_playing()){};
red_led(1); // Turn on the red LED.
delay_ms(20); // Wait for 20 ms.
}
}
}
void loop() // run over and over again
{
// wait here for one of the three buttons to be pushed
unsigned char button = wait_for_button(ANY_BUTTON);
clear();
if (button == TOP_BUTTON)
{
play_from_program_space(fugue);
print("Fugue!");
lcd_goto_xy(0, 1);
print("flash ->");
}
if (button == MIDDLE_BUTTON)
{
play("! V8 cdefgab>cbagfedc");
print("C Major");
lcd_goto_xy(0, 1);
print("RAM ->");
}
if (button == BOTTOM_BUTTON)
{
if (is_playing())
{
stop_playing();
print("stopped");
}
else
{
play_note(A(5), 200, 15);
print("note A5");
}
}
}
/** waitForButton **********************************************
* Pause until a button is pressed.
* @params button -- identifier of button to wait for
* tune -- sequence of notes played when button is pressed
*/
void waitForButton(unsigned char button, const char *tune) {
while(!button_is_pressed(button)) {
delay_ms(100);
}
play_from_program_space(tune);
while (button_is_pressed(button)) ; // wait for button up
delay_ms(200);
}
/** displayWelcome ******************************************
* Output welcome message, play tune and delay 2s.
* @modifies - lcd displays welcome message, tune is played
*/
void displayWelcome() {
clear();
print_from_program_space(welcome_line1);
lcd_goto_xy(0,1);
print_from_program_space(welcome_line2);
play_from_program_space(welcome);
delay_ms(2000);
}
int main()
{
TCCR0A = 0; // configure timer0 to run at 78 kHz
TCCR0B = 0x04; // and overflow when TCNT0 = 256 (~3 ms)
play_from_program_space(rhapsody);
while(1)
{
// allow the sequence to keep playing automatically through the following delays
#ifndef ALWAYS_CHECK
play_mode(PLAY_AUTOMATIC);
#else
play_mode(PLAY_CHECK);
#endif
lcd_goto_xy(0, 0);
print("blink!");
int i;
for (i = 0; i < 8; i++)
{
#ifdef ALWAYS_CHECK
play_check();
#endif
red_led(1);
delay_ms(500);
red_led(0);
delay_ms(500);
}
lcd_goto_xy(0, 0);
print("timing");
lcd_goto_xy(0, 1);
print(" "); // clear bottom LCD line
// turn off automatic playing so that our time-critical code won't be interrupted by
// the buzzer's long timer1 interrupt. Otherwise, this interrupt could throw off our
// timing measurements. Instead, we will now use playCheck() to keep the sequence
// playing in a way that won't throw off our measurements.
#ifndef ALWAYS_AUTOMATIC
play_mode(PLAY_CHECK);
#endif
unsigned char maxTime = 0;
for (i = 0; i < 8000; i++)
{
TCNT0 = 0;
while (TCNT0 < 20) // time for ~250 us
;
if (TCNT0 > maxTime)
maxTime = TCNT0; // if the elapsed time is greater than the previous max, save it
#ifndef ALWAYS_AUTOMATIC
play_check(); // check if it's time to play the next note and play it if so
#endif
}
lcd_goto_xy(0, 1);
print("max=");
print_long((unsigned int)maxTime);
print(" "); // overwrite any left over characters
}
}
void rob_wait_play_from_program_space (const char * pSequence)
{
vTaskSuspendAll();
{
play_from_program_space (pSequence);
while (is_playing())
{
}
}
xTaskResumeAll();
}
// Displays a welcome message and plays the initial music.
void initialize()
{
load_custom_characters(); // load the custom characters
play_from_program_space(welcome);
print_two_lines_delay_1s(welcome_line1,welcome_line2);
print_two_lines_delay_1s(demo_name_line1,demo_name_line2);
print_two_lines_delay_1s(instructions_line1,instructions_line2);
clear();
print_from_program_space(instructions_line3);
lcd_goto_xy(0,1);
print_from_program_space(instructions_line4);
while(!(wait_for_button_and_beep() & BUTTON_B));
play_from_program_space(thank_you_music);
print_two_lines_delay_1s(thank_you_line1,thank_you_line2);
}
void initialize()
{
play_from_program_space(welcome);
#ifdef DEBUG
// start receiving data at 9600 baud.
serial_set_baud_rate(9600);
serial_receive_ring(buffer, 100);
#endif
// initialize your QTR sensors
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2};
// unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2, IO_C3, IO_C4, IO_C5, IO_D7, IO_D4};
unsigned char qtr_rc_pins[] = {IO_D4, IO_D7, IO_C5, IO_C4, IO_C3, IO_C2, IO_C1, IO_C0};
qtr_rc_init(qtr_rc_pins, 8, 2000, IO_D2); // 800 us timeout, emitter pin PD2
#ifdef DEBUG
serial_send_blocking("Press Button A to start calibrating...\n", 39);
#endif
wait_for_button_press(BUTTON_A);
// Always wait for the button to be released so that the robot doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_A);
delay_ms(800);
// then start calibration phase and move the sensors over both
// reflectance extremes they will encounter in your application:
// We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
unsigned int counter; // used as a simple timer
for(counter = 0; counter < 82; counter++)
{
if(counter < 20 || counter >= 60)
set_motors(60,-60);
else
set_motors(-60,60);
qtr_calibrate(QTR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);
#ifdef DEBUG
serial_send_blocking("Press Button A to start line following...\n", 42);
#endif
wait_for_button_press(BUTTON_A);
wait_for_button_release(BUTTON_A);
}
int main() {
setup();
displayWelcome();
displayBattery();
lineCalibration();
followLineLaps(4);
play_from_program_space(success);
clear();
print("Done.");
while (true); // prevent exit from main
return 0; // dead code
}
// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.
void initialize()
{
// 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_from_program_space(welcome);
print_two_lines_delay_1s(welcome_line1,welcome_line2);
print_two_lines_delay_1s(demo_name_line1,demo_name_line2);
print_two_lines_delay_1s(instructions_line1,instructions_line2);
clear();
print_from_program_space(instructions_line3);
lcd_goto_xy(0,1);
print_from_program_space(instructions_line4);
while(!(wait_for_button_and_beep() & BUTTON_B));
play_from_program_space(thank_you_music);
print_two_lines_delay_1s(thank_you_line1,thank_you_line2);
}
int main()
{
play_from_program_space(PSTR(">g32>>c32")); // Play welcoming notes.
while(1)
{
// Print battery voltage (in mV) on LCD.
clear();
print_long(read_battery_millivolts_sv());
red_led(1); // Turn on the red LED.
delay_ms(200); // Wait for 200 ms.
red_led(0); // Turn off the red LED.
delay_ms(200); // Wait for 200 ms.
}
}
int main()
{
play_from_program_space(PSTR(">g32>>c32")); // Play welcoming notes.
while(1)
{
// Get battery voltage (in mV) from the auxiliary processor
// and print it on the the LCD.
clear();
print_long(read_battery_millivolts_svp());
red_led(1); // Turn on the red LED.
delay_ms(200); // Wait for 200 ms.
red_led(0); // Turn off the red LED.
delay_ms(200); // Wait for 200 ms.
}
}
int main()
{
// If button A is pressed, test the PD0/PD1 outputs
if(button_is_pressed(BUTTON_A))
test_outputs();
// If button C is not pressed down, go to the demo.
if(!button_is_pressed(BUTTON_C) && !test_pushbutton_tries())
demo();
// Load bar graph characters.
// Together with space and the solid block at 255, this makes almost
// all possible bar heights, with two to spare.
unsigned char i;
for(i=0;i<6;i++)
{
lcd_load_custom_character(bars+i,i);
}
clear();
pololu_3pi_init(TEST_LINE_SENSOR_TIMEOUT);
if(test_pushbutton_tries())
goto pushbuttons;
play_from_program_space(welcome_test);
print_two_lines_delay_1s_test(welcome_test_line1,welcome_test_line2);
print_two_lines_delay_1s_test(test_name_line1,test_name_line2);
clear();
test_battery();
test_qtr();
test_motors();
test_pot();
pushbuttons:
test_pushbuttons();
clear();
print("Success");
play("O5 c16");
while(1);
}
/** makeTurn *******************************************
* makes a turn either left or right
*
* @params left -- when true will turn left, else turn right
*/
void makeTurn(bool leftTurn) {
unsigned int sensors[5];
int sensorValue = 0;
// turn one way for a left turn, other way for right
if (leftTurn) {
set_motors(-motor_speed, motor_speed);
}
else {
set_motors(motor_speed, -motor_speed);
}
// take an initial reading of the sensors
// could use a do..while loop here
read_line(sensors, IR_EMITTERS_ON);
// keep turning until we are off the line
while (sensors[center_sensor] > 500) {
read_line(sensors, IR_EMITTERS_ON);
}
// give it a small chance to ensure it is off the line
delay_ms(10);
// keep turning until we find a line again
while (sensors[center_sensor] < 500) {
read_line(sensors, IR_EMITTERS_ON);
}
play_from_program_space(beep_button_a);
// keep turning until we center on the line again, fixes bug with 3pi
// being unable to center before hitting an intersection if the
// intersection is shortly after a dead end
while (sensorValue > 2050 || sensorValue < 1950) {
sensorValue = read_line(sensors, IR_EMITTERS_ON);
}
// all stop captain
set_motors(0, 0);
}
/** displayBattery ****************************************
* Output battery level, wait for button B
* @modifies lcd display has battery level output.
*/
void displayBattery() {
int bat;
const int LOW_BATTERY = 3500;
const int FULL_BATTERY = 6000;
double batPer;
while (!button_is_pressed(BUTTON_B)) {// as long as B is not pressed
bat = read_battery_millivolts();// read battery voltage
batPer = (double)(bat - LOW_BATTERY)/(FULL_BATTERY-LOW_BATTERY)*100; // compute percent of full value
clear();
print_long(batPer); // display value
print("%");
lcd_goto_xy(0,1);
print("Press B");
delay_ms(100); // wait a little bit
}
play_from_program_space(beep_button_b);
while (button_is_pressed(BUTTON_B)) ; // wait for button up
delay_ms(200);
}
static portTASK_FUNCTION( vBlockingQueueConsumer, pvParameters )
{
unsigned short usData, usExpectedValue = 0;
xBlockingQueueParameters *pxQueueParameters;
short sErrorEverOccurred = pdFALSE;
pxQueueParameters = ( xBlockingQueueParameters * ) pvParameters;
for( ;; )
{
if( xQueueReceive( pxQueueParameters->xQueue, &usData, pxQueueParameters->xBlockTime ) == pdPASS )
{
if( usData != usExpectedValue )
{
/* Catch-up. */
usExpectedValue = usData;
sErrorEverOccurred = pdTRUE;
}
else
{
/* We have successfully received a message, so increment the
variable used to check we are still running. */
if( sErrorEverOccurred == pdFALSE )
{
( *pxQueueParameters->psCheckVariable )++;
}
/* Increment the value we expect to remove from the queue next time
round. */
++usExpectedValue;
play_from_program_space(PSTR("c"));
while (is_playing()){};
red_led(0); // Turn off the red LED.
delay_ms(20); // Wait for 20 ms.
}
}
}
}
/**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()
{
// Set PC5 as an input with internal pull-up disabled
DDRC5 &= ~(1<< PORTC5); //port 5 is an input
PORTC &= ~(1<< PORTC5);
// 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(name_line1);
lcd_goto_xy(0,1);
print_from_program_space(name_line2);
delay_ms(1000);
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
clear();
print_long(read_battery_millivolts());
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);
clear();
print("Go!");
// Play music and wait for it to finish before we start driving.
play_from_program_space(go);
while(is_playing());
}
void menu_select()
{
static int menu_index = 0;
print_two_lines_delay_1s(main_menu_intro_line1,main_menu_intro_line2);
while(1)
{
clear();
lcd_goto_xy(0,1);
print_from_program_space(menu_line2);
lcd_goto_xy(0,0);
print_from_program_space(main_menu_options[menu_index]);
lcd_show_cursor(CURSOR_BLINKING);
// the cursor will be blinking at the end of the option name
// wait for all buttons to be released, then a press
while(button_is_pressed(ANY_BUTTON));
char button = wait_for_button_press(ANY_BUTTON);
if(button & BUTTON_A)
{
play_from_program_space(beep_button_a);
menu_index --;
}
else if(button & BUTTON_B)
{
lcd_hide_cursor();
clear();
play_from_program_space(beep_button_b);
wait_for_button_release(button);
while(!button_is_pressed(BUTTON_B))
{
lcd_goto_xy(0,1);
print_from_program_space(back_line2);
lcd_goto_xy(0,0);
main_menu_functions[menu_index]();
}
set_motors(0,0);
stop_playing();
m1_speed = 0;
m2_speed = 0;
red_led(0);
green_led(0);
play_from_program_space(beep_button_b);
return;
}
else if(button & BUTTON_C)
{
play_from_program_space(beep_button_c);
menu_index ++;
}
if(menu_index < 0)
menu_index = main_menu_length-1;
if(menu_index >= main_menu_length)
menu_index = 0;
}
}
/** followSegment ****************************************
* follow segment until a turn is found, an end of the line
* or the target
*
* @params left -- by reference if a left branches was found
* right -- by reference if a right branches was found
* straight -- by reference if a straight line was found
*/
void followSegment(bool& left, bool& right, bool& straight) {
// the value returned from the line sensors
int sensorValue = 2000;
// the raw sensor values from the 3pi
unsigned int sensors[5];
// the change in speed
signed int delta_speed = 0;
// inital read of the sensor value
sensorValue = read_line(sensors, IR_EMITTERS_ON);
left = right = false;
// while we have a line at center and not at a branch
while (!left && !right) {
// could have this in the while but this way is slightly easier to read
// if we go off the line stop we break out of the loop
if (sensorValue > 2000 + sensor_threshold || sensorValue < 2000 - sensor_threshold) {
break;
}
// normalize the sensor value to a motor delta value and turn motors on
// delta is between 0 and motor_speed
delta_speed = ((motor_speed - (-motor_speed)) * (sensorValue - 0.0))/(4000.0 - 0.0) + (-motor_speed);
set_motors(motor_speed + delta_speed, motor_speed - delta_speed);
// we need to update the sensor values of the 3pi will just repeat forever
sensorValue = read_line(sensors, IR_EMITTERS_ON);
left = sensors[left_sensor] > 500;
right = sensors[right_sensor] > 500;
// if we have a reading to the left or right keep moving forward a bit
// this is to fix a big where the 3pi would read one part of a branch but
// miss the other because it read one side too fast
if (left || right) {
// move forward some and read the sensors again
set_motors(motor_speed, motor_speed);
delay_ms(55);
sensorValue = read_line(sensors, IR_EMITTERS_ON);
left = sensors[left_sensor] > 500;
right = sensors[right_sensor] > 500;
}
// if we are at target we stop and return from the function
if (isTarget()) {
set_motors(0,0);
done = true;
return;
}
}
set_motors(motor_speed, motor_speed);
// if we have left and right sensor values we move forward until they are both clear of a line
// if only left we wait for the left sensor to clear
// if only right we wait for the right sensor to clear
// we always break out when we move off the center of the line
if (left && right) {
while (sensors[center_sensor] > 500 && ((sensors[left_sensor] > 500) == left) && ((sensors[right_sensor] > 500) == right)) {
read_line(sensors, IR_EMITTERS_ON);
}
}
else if (left) {
while (sensors[center_sensor] > 500 && ((sensors[left_sensor] > 500) == left)) {
read_line(sensors, IR_EMITTERS_ON);
}
}
else if (right) {
while (sensors[center_sensor] > 500 && ((sensors[right_sensor] > 500) == right)) {
read_line(sensors, IR_EMITTERS_ON);
}
}
// check straight now since we are off the intersection
straight = (sensors[center_sensor] > 500);
// let us know what you are doing Mr. 3pi
play_from_program_space(beep_button_b);
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());
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
unsigned int sensors[8]; // an array to hold sensor values
unsigned int slow = 30;
#ifdef DEBUG
unsigned int counter = 0;
#endif
// set up the 3pi
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
// Get the position of the line. Note that we *must* provide
// the "sensors" argument to read_line() here, even though we
// are not interested in the individual sensor readings.
unsigned int position = qtr_read_line(sensors,QTR_EMITTERS_ON);
position = position - 50;
/*
// If I lift the robot, it should stop the motors. When the
// sensors are no longer near a surface, they will all read 1000,
// and the position given by read_line functions will be 3500.
//if (sensors[0] + sensors[1] + sensors[2] + sensors[3] + sensors[4]
// + sensors[5] + sensors[6] + sensors[7] == 7000)
if (sensors[0] + sensors[3] + sensors[4] + sensors[7] == 4000) // Shorter and quicker
{
set_motors(0,0);
break;
}
if(position < 1000)
{
// We are far to the right of the line: turn left.
// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 255, so we are driving
// it at just about 40% of the max.
set_motors(0,170);
#ifdef DEBUG
if (counter > 100)
serial_send_blocking("LEFT \n", 6);
#endif
}
else if(position < 2000)
{
set_motors(60,150);
#ifdef DEBUG
if (counter > 100)
serial_send_blocking("left \n", 6);
#endif
}
else if (position < 3000)
{
set_motors(90,150);
}
else if (position < 3300) // CENTER = 3500
{
set_motors(130,150);
}
else if (position < 3700)
{
// We are somewhat close to being centered on the line:
// drive straight.
set_motors(160,160);
#ifdef DEBUG
if (counter > 100)
serial_send_blocking("OK \n", 6);
#endif
}
else if (position < 4000)
{
set_motors(150,130);
}
else if (position < 5000)
{
set_motors(150,90);
#ifdef DEBUG
if (counter > 100)
serial_send_blocking("right\n", 6);
#endif
}
else if (position < 6000)
{
set_motors(150,60);
}
else // (position < 7000)
{
// We are far to the left of the line: turn right.
set_motors(170,0);
#ifdef DEBUG
if (counter > 100)
serial_send_blocking("RIGHT\n", 6);
#endif
}
#ifdef DEBUG
if (counter > 100)
counter = 0;
else
counter++;
#endif
*/
if(position < 1000)
{
// We are far to the right of the line: turn left.
// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 255, so we are driving
// it at just about 40% of the max.
set_motors(0, 180 - slow);
}
else if(position < 2000)
{
set_motors(40 - slow, 150 - slow);
}
else if (position < 3000)
{
set_motors(90 - slow, 150 - slow);
}
else if (position < 3300) // CENTER = 3500
{
set_motors(130 - slow, 150 - slow);
}
else if (position < 3700)
{
// We are somewhat close to being centered on the line:
// drive straight.
set_motors(160 - slow, 160 - slow);
}
else if (position < 4000)
{
set_motors(150 - slow, 130 - slow);
}
else if (position < 5000)
{
set_motors(150 - slow, 90 - slow);
}
else if (position < 6000)
{
set_motors(150 - slow, 40 - slow);
}
else // (position < 7000)
{
// We are far to the left of the line: turn right.
set_motors(180 - slow, 0);
}
}
// This part of the code is never reached. A robot should
// never reach the end of its program, or unpredictable behavior
// will result as random code starts getting executed. If you
// really want to stop all actions at some point, set your motors
// to 0,0 and run the following command to loop forever:
play_from_program_space(bye);
set_motors(0,0);
while(1);
}
// *** triggered by top button ***
// This function plays a melody from flash in the background while the two
// user LEDs alternately fade in and out.
unsigned char melodyTest()
{
unsigned char button;
int i = 0;
// the following function does not block execution
play_from_program_space(fugue); // play music from flash in the background
red_led(0); // turn red and green LEDs off
green_led(0);
clear(); // clear the LCD, go to the start of the first LCD line
print("melody:"); // print to the LCD
lcd_goto_xy(0, 1); // go to the start of the second LCD line
print("fugue"); // print to the LCD
time_reset(); // reset the internal millisecond timer count to zero
while (1) // loop here until we detect a button press and return
{
if (get_ms() >= 5) // if 5 or more milliseconds have elapsed
{
time_reset(); // reset timer count to zero
// check if middle or bottom buttons have been pressed
button = button_is_pressed(MIDDLE_BUTTON | BOTTOM_BUTTON);
if (button != 0) // if so, stop melody and return the button ID
{
stop_playing(); // stop playing music
return button;
}
i += 5; // increase our state variable based on the time
if (i >= 1000) // once a second has elapsed, reset the state var
i = 0;
}
// the following code alternately flashes the red and green LEDs,
// fading them in and out over time as the music plays in the
// background. This is accomplished by using high-frequency PWM
// signals on the LED lines. Essentially, each LED is flickering
// on and off very quickly, which gives it the apperance of variable
// brightness depending on the percentage of the cycle the LED is on.
// Each LED flicker cycle takes a total approximately 251 us.
if (i < 250) // phase 1: ramp up the red LED brightness
{ // as i increases over time
red_led(1); // turn the red LED on
delay_us(i + 1); // microsecond delay
red_led(0); // turn the red LED off
delay_us(250 - i); // microsecond delay
}
else if (i < 500) // phase 2: ramp down the red LED brightness
{
red_led(1); // turn the red LED on
delay_us(500 - i); // microsecond delay
red_led(0); // turn the red LED off
delay_us(i - 249); // microsecond delay
}
else if (i < 750) // phase 3: ramp up the green LED brightness
{
green_led(1); // turn the green LED on
delay_us(i - 499); // microsecond delay
green_led(0); // turn the green LED off
delay_us(750 - i); // microsecond delay
}
else // phase 4: ramp down the green LED brightness
{
green_led(1); // turn the green LED on
delay_us(1000 - i); // microsecond delay
green_led(0); // turn the green LED off
delay_us(i - 749); // microsecond delay
}
}
}
