void line_follower(Motor motorA, Motor motorB) { if (sensorA.get_color() == BLANCO && sensorB.get_color() == BLANCO) { motorA.set_sentido(ADELANTE); motorB.set_sentido(ADELANTE); } else if (sensorA.get_color() == BLANCO && sensorB.get_color() != BLANCO) { motorA.set_sentido(ATRAS); motorB.set_sentido(ADELANTE); } else if (sensorA.get_color() != BLANCO && sensorB.get_color() == BLANCO) { motorA.set_sentido(ADELANTE); motorB.set_sentido(ATRAS); } }
int main() { // PC connection Serial pc(0, 9600); // Turn all leds on while waiting Bus leds(4, LED4, LED3, LED2, LED1); leds.mode(Output); leds.write_all(1); // Config motors Motor motorB (P22, P21, P23); Motor motorA (P25, P24, P26); // Turn lights on FarolA = 1; FarolB = 1; // The interface with the remote control Serial cmdLink(1, 38400); // Read until the control says START wait_for_start(cmdLink); // Seteo velocidad y direccion del motor A motorA.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ; // Seteo velocidad y direccion del motor B motorB.set_sentido(ADELANTE); motorB.Velocidad = MARCHA_DER; while (1) { // encendemos los leds de acuerdo con los sensores //estado_sensores(); unsigned char cmd = get_command(cmdLink); leds.write(cmd); // Stop makes the car stop until the next start if (cmd == STOP) { motorA.set_sentido(DETENIDO); motorB.set_sentido(DETENIDO); wait_for_start(cmdLink); } // Go faster else if (cmd == START) { motorA.Velocidad = 0.9; motorB.Velocidad = 0.9; } // Enter line_follower mode else if (cmd == LINE_FOLLOWER) { line_follower(motorA, motorB); } // Go forward else if (cmd == (CENTER+FORWARD)) { motorA.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ; motorB.set_sentido(ADELANTE); motorB.Velocidad = MARCHA_DER; } // Go backward else if (cmd == (CENTER+BACKWARD)) { motorA.set_sentido(ATRAS); motorA.Velocidad = MARCHA_IZQ; motorB.set_sentido(ATRAS); motorB.Velocidad = MARCHA_DER; } // Go left else if (cmd > LEFT && cmd < RIGHT) { motorB.set_sentido(ADELANTE); float f = (cmd - LEFT) / 10.; if (f > 0.7) { motorA.set_sentido(ATRAS); motorA.Velocidad = MARCHA_IZQ*(f-0.2); motorB.Velocidad = MARCHA_DER*(f-0.2); } else { motorA.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ*(0.7-f); motorB.Velocidad = MARCHA_DER*(1+f); } } // Go right else if (cmd > RIGHT && cmd < BACKWARD) { motorA.set_sentido(ADELANTE); float f = (cmd - RIGHT) / 10.; if (f > 0.7) { motorB.set_sentido(ATRAS); motorA.Velocidad = MARCHA_IZQ*(f-0.2); motorB.Velocidad = MARCHA_DER*(f-0.2); } else { motorB.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ*(1+f); motorB.Velocidad = MARCHA_DER*(0.7-f); } } else { cmd -= BACKWARD; if (cmd > LEFT && cmd < RIGHT) { float f = (cmd - LEFT) / 10.; motorB.set_sentido(ATRAS); if (f > 0.7) { motorA.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ*(f-0.2); motorB.Velocidad = MARCHA_DER*(f-0.2); } else { motorA.set_sentido(ATRAS); motorA.Velocidad = MARCHA_IZQ*(0.9-f); motorB.Velocidad = MARCHA_DER*(1+f); } } else { float f = (cmd - RIGHT) / 10.; motorA.set_sentido(ATRAS); if (f > 0.7) { motorB.set_sentido(ADELANTE); motorA.Velocidad = MARCHA_IZQ*(f-0.2); motorB.Velocidad = MARCHA_DER*(f-0.2); } else { motorB.set_sentido(ATRAS); motorA.Velocidad = MARCHA_IZQ*(1+f); motorB.Velocidad = MARCHA_DER*(0.9-f); } } } } }