Diff: aldebaran_auto.cpp

Revision:

0:2228de67a3ef

--- /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);
+        }
+    }
+}