Codigo do aldebaran com headers e dip switch
Diff: aldebaran_auto.cpp
- Revision:
- 0:2228de67a3ef
diff -r 000000000000 -r 2228de67a3ef aldebaran_auto.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/aldebaran_auto.cpp Fri Mar 19 16:43:46 2021 +0000 @@ -0,0 +1,262 @@ +#include "aldebaran_auto.h" +#include "mbed.h" +#include "PwmOut.h" +Serial bt(p9, p10); +//================================================================================ +//========================= MOTORS CONTROL - ==================================== +//================================================================================ +PwmOut Right(p21); +PwmOut Left(p22); +#define MED_PPM 1500 // Valor indicando que o motor deve ficar parado de acordo com o ppm +int LSpeedGlobal; +int RSpeedGlobal; + +void Drive(int LeftSpeed, int RightSpeed) +{ + Left.pulsewidth_us(LeftSpeed); + Right.pulsewidth_us(RightSpeed); + LSpeedGlobal = LeftSpeed; + RSpeedGlobal = RightSpeed; +} +void calibrate() +{ + Drive(MED_PPM, MED_PPM); + wait(3); +} +//================================================================================ +//===================== DISTANCE SENSORS - ===================================== +//================================================================================ +//********** DEFINICOES - ************ +DigitalIn Sensor1(p26); // Sensor digital localizado na esquerda do robo +DigitalIn Sensor2(p27); //Sensor digital localizado na diagonal esquerda do robo +DigitalIn Sensor3(p28); //Sensor digital localizado na frente do robo +DigitalIn Sensor4(p25); //Sensor digital localizado na diagonal direita do robo +DigitalIn Sensor5(p30); //Sensor digital localizado na direita do robo + +int measure[5] = {0, 0, 0, 0, 0}; // We define variable for storing sensor output. +int total = 0; //{left, left_diagonal, front, right_diagonal, right} + +void Search() +{ + measure[0] = Sensor1.read(); + measure[1] = Sensor2.read(); + measure[2] = Sensor3.read(); + measure[3] = Sensor4.read(); + measure[4] = Sensor5.read(); + + total = measure[4] * 10000 + measure[3] * 1000 + measure[2] * 100 + measure[1] * 10 + measure[0]; + return; +} +//================================================================================ +//===================== SWITCH INICIALIZATION - ================================= +//================================================================================ +InterruptIn SWITCH(p11); +DigitalOut Led4(LED4); +void desligar() +{ + Drive(MED_PPM, MED_PPM); +} +//================================================================================ +//===================== FAILSAFE - ============================================== +//================================================================================ +InterruptIn Failsafe(p12); +Timer timer; +DigitalOut Led3(LED3); +int RC_value; +int RC_start; +int failsafe = 0; +#define FAILSAFE_SIGNAL 1400 +void intHigh() +{ + RC_start = timer.read_us(); +} +void intLow() +{ + RC_value = timer.read_us() - RC_start; + if (RC_value > FAILSAFE_SIGNAL) + { + failsafe = 0; //failsafe não ativado + } + else + { + failsafe = 1; //failsafe ativado + } +} +//================================================================================ +//===================== LINE SENSORS - ========================================= +//================================================================================ +#define PARAMETER 0.908 +#define RightSensor p18 +#define LeftSensor p20 +DigitalOut Led1(LED1); +DigitalOut Led2(LED2); +AnalogIn RightRead(RightSensor); +AnalogIn LeftRead(LeftSensor); +bool right_line = 1, left_line = 1; +int result = 0, anterior = 0; +int Line_check() +{ + if (RightRead.read() > PARAMETER) + { + right_line = 1; + } + else + { + right_line = 0; //white + } + if (LeftRead.read() > PARAMETER) + { + left_line = 1; + } + else + { + left_line = 0; + } + result = right_line * 10 + left_line; + if (result == 0 && anterior == 0) + { + return 0; + } + if (result == 0 && anterior == 1) + { + anterior = 0; + wait_ms(0); + return 0; + } + switch (result) + { + case 10: + { //Sensor de linha da direita viu a linha + Led2 = 1; + Led1 = 0; + Drive(1500 + 5 * (50), 1500 - 5 * (50)); //Para tras com velocidade máxima por 50ms + wait(1); + Drive(1500 - 5 * (35), 1500 - 5 * (35)); //Para esquerda com velocidade máxima por 100ms + wait(0.5); + anterior = 1; + break; + } + + case 1: + { //Sensor de linha da esquerda viu a linha + Led2 = 0; + Led1 = 1; + Drive(1500 + 5 * (50), 1500 - 5 * (50)); //Para tras com velocidade máxima por 100ms + wait(1); + Drive(1500 + 5 * (35), 1500 + 5 * (35)); //Para direita com velocidade máxima por 50ms + wait(0.5); + anterior = 1; + break; + } + + case 11: + { //Sensor de linha da esquerda e da direita viram a linha + Led2 = 1; + Led1 = 1; + Drive(1500 + 5 * (50), 1500 - 5 * (50)); //Para tras + wait(1); + Drive(1500 + 5 * (35), 1500 + 5 * (35)); //Para direita + wait(0.5); + anterior = 1; + break; + } + default: + { + anterior = 1; + Led2 = 0; + Led1 = 0; + break; + } + } + return 1; +} +void Action() +{ + + switch (total) + { + + case 0: //caso nao veja nenhum dos sensores + Drive(LSpeedGlobal, RSpeedGlobal); + break; + + case 1: //vira para a esquerda + Drive(1500 - 5 * (40), 1500 - 5 * (40)); + break; + + case 10: //vira para a esquerda + Drive(1500 - 5 * (30), 1500 - 5 * (30)); + break; + + case 11: //vira para a esquerda + Drive(1500 - 5 * (30), 1500 - 5 * (30)); + break; + + case 100: //vai reto + Drive(1500 - 5 * (60), 1500 + 5 * (60)); + break; + + case 110: //vai reto + Drive(1500 - 5 * (60), 1500 + 5 * (60)); + break; + + case 1000: //vira para a direita + Drive(1500 + 5 * (30), 1500 + 5 * (30)); + break; // + + case 1100: //vai reto + Drive(1500 - 5 * (60), 1500 + 5 * (60)); + break; + + case 1110: //vai reto (GRANDE CHIFRE) + Drive(1500 - 5 * (100), 1500 + 5 * (100)); + break; + + case 10000: //vira para direita + Drive(1500 + 5 * (40), 1500 + 5 * (40)); + break; + + case 11000: //vira para a direita + Drive(1500 + 5 * (30), 150 + 5 * (30)); + break; + + //todos os outros casos que são impossíveis + + default: //para//////////// + Drive(MED_PPM, MED_PPM); + break; + } +} + +void AUTO_mode() +{ + timer.start(); + SWITCH.fall(&desligar); + Failsafe.fall(&intLow); + Failsafe.rise(&intHigh); + calibrate(); + LSpeedGlobal = 1500 - 5 * (30); + RSpeedGlobal = 1500 + 5 * (30); + + while (SWITCH.read() == 0) + { + Search(); + Led4 = 1; + } + Led4 = 0; + while (1) + { + if (!failsafe && SWITCH.read()) + { //Failsafe não ativado + Led3 = 1; + Action(); + //Line_check(); + Search(); + } + else + { // Failsafe ativado + Led3 = 0; + Drive(MED_PPM, MED_PPM); + } + } +}