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);
}
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);
}
// 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);
}
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);
clear();
}
/*==========la fonction main()=======*/
int main(void) {
lcd_init_printf();
pololu_3pi_init(2000);
play_mode(PLAY_CHECK);
clear();
print("Hello!");
play("L16 ceg>c");
// start receiving data at 9600 baud
serial_set_baud_rate(9600);
serial_receive_ring(buffer, 100);
int i=0;
char dirct,chaine[4], comp='C',*recuper = NULL, *ok;
long val, veri=0;
char command;
/* la boucle qui permet de recuperer la trame caractere par caractere */
while(1){
for(i=0;i<4;i++){
command = read_next_byte();
if (command)
{
chaine[i] = command;
}
}
/*recuperation de la lettre recu dans la trame */
dirct = chaine[0];
/*recuperation du reste de la trame en chaine de caractere */
recuper = strchr(chaine,chaine[1]);
/*conversion de cette chaine recuperer en entier (type long)*/
val = strtol(recuper, &ok,10);
/* cette condition permet d'eviter l'execution de la meme trame plusieurs fois*/
if(dirct != comp || veri != val)
{
clear();
printf("%s",chaine);
switch(dirct)
{
case 'A':
avancer(val);
break;
case 'R':
reculer(val);
break;
case 'D':
droit(val);
break;
case 'G':
gauche(val);
break;
case 'M':
melodie();
break;
default:
set_motors(0,0);
break;
}
comp = dirct;
veri = val;
}
}
return 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() {
//holds sensor values
unsigned int sensors[5];
//hold min and max sensor values for calibration
unsigned int minv[5] = {65500, 65500, 65500, 65500, 65500}, maxv[5] = {0, 0, 0, 0, 0};
//holds the previous value so that we can make sure the sensor didn't report a crap value
int range[5];
//set up the 3pi
pololu_3pi_init(2000);
//calibrate the stuff
calibrate(sensors, minv, maxv);
//set our range from our calibrated readings so that it doesn't break the other readings when we start moving
int i;
for (i = 0; i < 5; i++)
range[i] = getCalibratedSensor(sensors[i], minv[i], maxv[i]);
//set the speed
int const speed = 255;
//holds the deriv
int deriv;
//holds the integral
int integ = 0;
//holds the last position
int lastProp = 0;
//line position relative to center
int position = 0;
long last = millis();
//run in circles
while(1) {
//read the line sensor values
read_line_sensors(sensors, IR_EMITTERS_ON);
//compute line positon
position = line_position(sensors, minv, maxv, range);
//get the middle sensors to = 0
int prop = position - 250;
//save the running time
long now = millis();
long diff = now - last;
//calc the derivative
deriv = ((prop - lastProp) * 10) / diff;
//if the robot has changed directions, clear the integral
if ((lastProp < 0 && prop > 0) || (prop < 0 && lastProp > 0))
integ = 0;
else
integ += prop * diff;
//get a proportional speed
int propSpeed = (prop * 2) + (integ / 6500) + (deriv * 23);
//set our last run time
last = now;
//get a proportional speed
int left = speed+propSpeed;
int right = speed-propSpeed;
//make sure the motors are never off / going negative
if (left <= 0) {
int diff = 0 - left;
right += diff - 30;
left = 30;
}
if (right <= 0) {
int diff = 0 - right;
left += diff - 30;
right = 30;
}
//limit the motors to their maxes
if (left > 255)
left = 255;
if (right > 255)
right = 255;
lastProp = prop;
set_motors(left, right);
}
}
/** setup **********************************************************
* All the initialization goes here. It should be called once when
* the program starts up.
*
*/
void setup() {
pololu_3pi_init(2000); // library function that initializes sensors
load_custom_characters();
}
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
}
}
}
