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);
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
// Set all five user LEDs as outputs driven low.
// Otherwise, the LEDs are driven by the LCD and can appear
// on or partially on.
red_led(0);
green_led(0);
red_led2(0);
green_led2(0);
yellow_led(0);
clear();
delay(10);
// if any button is pressed, go into the old version of the test code
if(button_is_pressed(ANY_BUTTON))
{
print("Simple Test");
wait_for_button_release(button_is_pressed(ANY_BUTTON));
test(); // activate the simpler test code
}
// set up the robot
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
menu_select();
}
}
int main()
{
clear(); // clear the LCD
print("Send serial");
lcd_goto_xy(0, 1); // go to start of second LCD row
print("or press B");
// Set the baud rate to 9600 bits per second. Each byte takes ten bit
// times, so you can get at most 960 bytes per second at this speed.
serial_set_baud_rate(USB_COMM, 9600);
// Start receiving bytes in the ring buffer.
serial_receive_ring(USB_COMM, receive_buffer, sizeof(receive_buffer));
while(1)
{
// USB_COMM is always in SERIAL_CHECK mode, so we need to call this
// function often to make sure serial receptions and transmissions
// occur.
serial_check();
// Deal with any new bytes received.
check_for_new_bytes_received();
// If the user presses the middle button, send "Hi there!"
// and wait until the user releases the button.
if (button_is_pressed(MIDDLE_BUTTON))
{
wait_for_sending_to_finish();
memcpy_P(send_buffer, PSTR("Hi there!\r\n"), 11);
serial_send(USB_COMM, send_buffer, 11);
send_buffer[11] = 0; // terminate the string
clear(); // clear the LCD
lcd_goto_xy(0, 1); // go to start of second LCD row
print("TX: ");
print(send_buffer);
// Wait for the user to release the button. While the processor is
// waiting, the OrangutanSerial library will not be able to receive
// bytes from the USB_COMM port since this requires calls to the
// serial_check() function, which could cause serial bytes to be
// lost. It will also not be able to send any bytes, so the bytes
// bytes we just queued for transmission will not be sent until
// after the following blocking function exits once the button is
// released. If any of this is a concern, you can replace the
// following line with:
// do
// {
// while (button_is_pressed(MIDDLE_BUTTON))
// serial_check(); // receive and transmit as needed
// delay_ms(10); // debounce the button press/release
// }
// while (button_is_pressed(MIDDLE_BUTTON));
wait_for_button_release(MIDDLE_BUTTON);
}
}
}
// 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;
}
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()
{
while(1)
{
clear();
print("Waiting");
// wait for either the top or bottom buttons to be pressed
// store the value of the pressed button in the variable 'button'
unsigned char button = wait_for_button_press(TOP_BUTTON | BOTTOM_BUTTON);
clear();
if (button == TOP_BUTTON) // display the button that was pressed
print("top down");
else
print("bot down");
wait_for_button_release(button); // wait for that button to be released
clear();
print("released"); // display that the button was released
delay_ms(1000);
}
}
int main() // run once, when the sketch starts
{
currentIdx = 0;
print("Music!");
while(1) // run over and over again
{
// if we haven't finished playing the song and
// the buzzer is ready for the next note, play the next note
if (currentIdx < MELODY_LENGTH && !is_playing())
{
// play note at max volume
play_note(note[currentIdx], duration[currentIdx], 15);
// optional LCD feedback (for fun)
lcd_goto_xy(0, 1); // go to start of the second LCD line
if(note[currentIdx] != 255) // display blank for rests
print_long(note[currentIdx]); // print integer value of the current note
print(" "); // overwrite any left over characters
currentIdx++;
}
// Insert some other useful code here...
// the melody will play normally while the rest of your code executes
// as long as it executes quickly enough to keep from inserting delays
// between the notes.
// For example, let the top user pushbutton function as a stop/reset melody button
if (button_is_pressed(TOP_BUTTON))
{
stop_playing(); // silence the buzzer
if (currentIdx < MELODY_LENGTH)
currentIdx = MELODY_LENGTH; // terminate the melody
else
currentIdx = 0; // restart the melody
wait_for_button_release(TOP_BUTTON); // wait here for the button to be released
}
}
}
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 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();
}
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;
}
}
// 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);
}
int main()
{
initialize();
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);
unsigned int sensores[5];
int medio = 2000;
int m_max = 200;
int m_moderada = 190;
int umbral_zona_moderada = 1200;
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(m_max,m_max);
int extremos = sensores[0] + sensores[4];
if (extremos < 1700)
{
set_motors(0,0);
delay_ms(2000);
break;
}
}
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
int todos = 0;
for (int x = 0; x < 5; x++)
todos += sensores[x];
if (todos == 5000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
int posicion_linea_centrada = ((int)posicion_linea) - medio; // Si la linea esta en el centro el valor es 0, en los sensores extremos -2000 o 2000.
float pendiente_zona_dastica = ((float)(m_max - m_moderada)/(medio - umbral_zona_moderada)); //
int b = m_max - pendiente_zona_dastica*medio;
int diferencia_motores = 0; // m1 - m2
int m1 = 0;
int m2 = 0;
if (posicion_linea_centrada > umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada + b;
}
else if (posicion_linea_centrada < -umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada - b;
} else {
double x = ((double)posicion_linea_centrada)/(umbral_zona_moderada);
diferencia_motores = m_moderada*x*x*x;
}
if(diferencia_motores > 0)
{
m1 = m_max;
m2 = m_max - diferencia_motores;
}
else if (diferencia_motores < 0)
{
m2 = m_max;
m1 = m_max + diferencia_motores;
} else
{
m1 = m_max;
m2 = m_max;
}
clear();
print_long(m1);
print(" ");
print_long(m2);
lcd_goto_xy(0,1);
print_long(diferencia_motores);
set_motors(m1,m2);
}
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
/* int todos = 0;
for (int x = 0; x < 5; x++)
todos += sensores[x];
if (todos < 5000)
{
break;
}*/
int extremos = sensores[0] + sensores[4];
if (extremos < 2000)
{
set_motors(m_max,m_max);
delay_ms(20);
break;
}
int posicion_linea_centrada = ((int)posicion_linea) - medio; // Si la linea esta en el centro el valor es 0, en los sensores extremos -2000 o 2000.
float pendiente_zona_dastica = ((float)(m_max - m_moderada)/(medio - umbral_zona_moderada)); //
int b = m_max - pendiente_zona_dastica*medio;
int diferencia_motores = 0; // m1 - m2
int m1 = 0;
int m2 = 0;
if (posicion_linea_centrada > umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada + b;
}
else if (posicion_linea_centrada < -umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada - b;
} else {
double x = ((double)posicion_linea_centrada)/(umbral_zona_moderada);
diferencia_motores = m_moderada*x*x*x;
}
if(diferencia_motores > 0)
{
m1 = m_max;
m2 = m_max - diferencia_motores;
}
else if (diferencia_motores < 0)
{
m2 = m_max;
m1 = m_max + diferencia_motores;
} else
{
m1 = m_max;
m2 = m_max;
}
clear();
print_long(m1);
print(" ");
print_long(m2);
lcd_goto_xy(0,1);
print_long(diferencia_motores);
set_motors(m1,m2);
}
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
int extremos = sensores[0] + sensores[4];
if (extremos == 2000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
int posicion_linea_centrada = ((int)posicion_linea) - medio; // Si la linea esta en el centro el valor es 0, en los sensores extremos -2000 o 2000.
float pendiente_zona_dastica = ((float)(m_max - m_moderada)/(medio - umbral_zona_moderada)); //
int b = m_max - pendiente_zona_dastica*medio;
int diferencia_motores = 0; // m1 - m2
int m1 = 0;
int m2 = 0;
if (posicion_linea_centrada > umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada + b;
}
else if (posicion_linea_centrada < -umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada - b;
} else {
double x = ((double)posicion_linea_centrada)/(umbral_zona_moderada);
diferencia_motores = m_moderada*x*x*x;
}
if(diferencia_motores > 0)
{
m1 = m_max;
m2 = m_max - diferencia_motores;
}
else if (diferencia_motores < 0)
{
m2 = m_max;
m1 = m_max + diferencia_motores;
} else
{
m1 = m_max;
m2 = m_max;
}
clear();
print_long(m1);
print(" ");
print_long(m2);
lcd_goto_xy(0,1);
print_long(diferencia_motores);
set_motors(m1,m2);
}
while (1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(m_max,0);
if (sensores[0] < 1000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
}
while (1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(m_max,0);
if (posicion_linea < 2000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
}
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
int laterales = sensores[0] + sensores[1] + sensores[2];
if (laterales >= 2500)
{
set_motors(0,0);
delay_ms(1000);
break;
}
int posicion_linea_centrada = ((int)posicion_linea) - medio; // Si la linea esta en el centro el valor es 0, en los sensores extremos -2000 o 2000.
float pendiente_zona_dastica = ((float)(m_max - m_moderada)/(medio - umbral_zona_moderada)); //
int b = m_max - pendiente_zona_dastica*medio;
int diferencia_motores = 0; // m1 - m2
int m1 = 0;
int m2 = 0;
if (posicion_linea_centrada > umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada + b;
}
else if (posicion_linea_centrada < -umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada - b;
} else {
double x = ((double)posicion_linea_centrada)/(umbral_zona_moderada);
diferencia_motores = m_moderada*x*x*x;
}
if(diferencia_motores > 0)
{
m1 = m_max;
m2 = m_max - diferencia_motores;
}
else if (diferencia_motores < 0)
{
m2 = m_max;
m1 = m_max + diferencia_motores;
} else
{
m1 = m_max;
m2 = m_max;
}
clear();
print_long(m1);
print(" ");
print_long(m2);
lcd_goto_xy(0,1);
print_long(diferencia_motores);
set_motors(m1,m2);
}
while(1){
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(0,m_max);
if ( (sensores[0] + sensores[1]) < 1000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
}
while(1){
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(0,m_max);
if ( sensores[2] >= 900)
{
set_motors(0,0);
delay_ms(1000);
break;
}
}
while(1)
{
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
int todos = 0;
for (int x = 0; x < 5; x++)
todos += sensores[x];
if (todos == 5000)
{
set_motors(0,0);
delay_ms(1000);
break;
}
int posicion_linea_centrada = ((int)posicion_linea) - medio; // Si la linea esta en el centro el valor es 0, en los sensores extremos -2000 o 2000.
float pendiente_zona_dastica = ((float)(m_max - m_moderada)/(medio - umbral_zona_moderada)); //
int b = m_max - pendiente_zona_dastica*medio;
int diferencia_motores = 0; // m1 - m2
int m1 = 0;
int m2 = 0;
if (posicion_linea_centrada > umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada + b;
}
else if (posicion_linea_centrada < -umbral_zona_moderada)
{
diferencia_motores = pendiente_zona_dastica*posicion_linea_centrada - b;
} else {
double x = ((double)posicion_linea_centrada)/(umbral_zona_moderada);
diferencia_motores = m_moderada*x*x*x;
}
if(diferencia_motores > 0)
{
m1 = m_max;
m2 = m_max - diferencia_motores;
}
else if (diferencia_motores < 0)
{
m2 = m_max;
m1 = m_max + diferencia_motores;
} else
{
m1 = m_max;
m2 = m_max;
}
clear();
print_long(m1);
print(" ");
print_long(m2);
lcd_goto_xy(0,1);
print_long(diferencia_motores);
set_motors(m1,m2);
}
while(1){
unsigned int posicion_linea = read_line(sensores,IR_EMITTERS_ON);
set_motors(m_max,m_max);
if (sensores[2] < 100)
{
set_motors(0,0);
break;
}
}
}
