Revision 0:2228de67a3ef, committed 2021-03-19

Comitter:

pedro_velozo

Date:

Fri Mar 19 16:43:46 2021 +0000

Commit message:

Codigo do aldebaran com adicao de headers e dip switch;

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_auto.h	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,15 @@
+void Drive(int LeftSpeed, int RightSpeed);
+
+void Search();
+
+void desligar();
+
+void intHigh();
+
+void intLow();
+
+int Line_check();
+
+void Action();
+
+void AUTO_mode(); // main function
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_rc.cpp	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,100 @@
+#include "aldebaran_rc.h"
+#include "mbed.h"
+#define OFFSET 2
+
+Serial bt(p9, p10);
+
+Timer timer;
+
+DigitalOut st_led(LED1);
+DigitalOut th_led(LED2);
+DigitalOut mid_led(LED3);
+
+InterruptIn ST(p15);
+InterruptIn TH(p14);
+
+PwmOut Right(p21);
+PwmOut Left(p22);
+
+int max_st = 1500, max_th = 1500, min_st = 1500, min_th = 1500, mid_st, mid_th;
+
+int RC_value[2];
+int RC_start[2];
+
+void ST_High() { RC_start[0] = timer.read_us(); } //start the time }
+void ST_Low() { RC_value[0] = timer.read_us() - RC_start[0]; }
+
+void TH_High() { RC_start[1] = timer.read_us(); } //start the time }
+void TH_Low() { RC_value[1] = timer.read_us() - RC_start[1]; }
+
+void RC_mode()
+{
+    int st_start, th_start;
+
+    timer.start();
+
+    ST.fall(&ST_Low);
+    ST.rise(&ST_High);
+
+    TH.fall(&TH_Low);
+    TH.rise(&TH_High);
+
+    Right.period(0.02);
+    Left.period(0.02);
+
+    bt.baud(9600);
+
+    st_led = 1;
+    wait(0.5);
+    st_led = 0;
+
+    st_start = timer.read_us();
+    while (timer.read_us() - st_start < 5000000)
+    {
+        if (RC_value[0] > max_st)
+        {
+            max_st = RC_value[0];
+        }
+        if (RC_value[0] < min_st)
+        {
+            min_st = RC_value[0];
+        }
+    }
+    st_led = 1;
+    wait(1);
+    th_led = 1;
+    wait(0.5);
+    th_led = 0;
+    th_start = timer.read_us();
+    while (timer.read_us() - th_start < 5000000)
+    {
+        if (RC_value[1] > max_th)
+        {
+            max_th = RC_value[1];
+        }
+        if (RC_value[1] < min_th)
+        {
+            min_th = RC_value[1];
+        }
+    }
+    th_led = 1;
+
+    mid_led = 1;
+    wait(5);
+    mid_led = 0;
+    mid_st = RC_value[0];
+    mid_th = RC_value[1];
+    wait(1);
+    mid_led = 1;
+
+    bt.printf("Max-ST: %i Min-ST: %i Max-TH: %i Min-TH: %i\n\r", max_st, min_st, max_th, min_th);
+
+    wait(5);
+    while (1)
+    {
+        bt.printf("Left: %i  Right: %i\n\r", RC_value[1], RC_value[0]);
+
+        Right.pulsewidth_us(mid_st + (mid_st - RC_value[1]) + (mid_th - RC_value[0]));
+        Left.pulsewidth_us(mid_st + (mid_st - RC_value[1]) - (mid_th - RC_value[0]) - OFFSET);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_rc.h	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,9 @@
+void ST_High();
+
+void ST_Low();
+
+void TH_High();
+
+void TH_Low();
+
+void RC_mode(); // main function
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_semiauto.cpp	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,181 @@
+#include "aldebaran_semiauto.h"
+#include "mbed.h"
+
+#define MAX_PPM 2000       // Valor indicando que o motor deve ir para frente com 100% de acordo com o ppm
+#define MED_PPM 1500       // Valor indicando que o motor deve ficar parado de acordo com o ppm
+#define MIN_PPM 1000       // Valor indicando que o motor deve ir para tras com 100% de acordo com o ppm
+#define REVERSE_LEFT 1550  //Signal to make the robot go reverse with the left wheels.
+#define FORWARD_LEFT 1550  //Signal to make the robot go forward with the left wheels.
+#define REVERSE_RIGHT 1550 //Signal to make the robot go reverse with the left wheels.
+#define FORWARD_RIGHT 1550 //Signal to make the robot go forward with the left wheels.
+#define BACKWARDS 10
+
+Timer timer;
+InterruptIn ST(p15);
+InterruptIn TH(p14);
+
+PwmOut Right(p21);
+PwmOut Left(p22);
+
+int ch1_start;
+int ch2_start;
+int ch1_in;
+int ch2_in;
+
+void ST_High() { ch1_start = timer.read_us(); } //start the time
+void ST_Low() { ch1_in = timer.read_us() - ch1_start; }
+
+void TH_High() { ch2_start = timer.read_us(); } //start the time
+void TH_Low() { ch2_in = timer.read_us() - ch2_start; }
+//================================================================================
+//=====================  FAILSAFE - ==============================================
+//================================================================================
+InterruptIn Failsafe(p12);
+DigitalOut Led4(LED4);
+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 - =========================================
+//================================================================================
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+#define RightSensor p18
+#define LeftSensor p20
+#define PARAMETER 0.9
+
+AnalogIn RightRead(RightSensor);
+AnalogIn LeftRead(LeftSensor);
+float sensor_right;
+float sensor_left;
+int right_line;
+int left_line;
+int line;
+int READ_SENSORS()
+{
+    sensor_right = RightRead;
+    sensor_left = LeftRead;
+    if (sensor_right > PARAMETER)
+    {
+        right_line = 1;
+    } //black
+    else
+    {
+        right_line = 0;
+    } //white
+    if (sensor_left > PARAMETER)
+    {
+        left_line = 1;
+    }
+    else
+    {
+        left_line = 0;
+    }
+    if (right_line)
+    {
+        led1 = 0;
+    }
+    else
+    {
+        led1 = 1;
+    }
+    if (left_line)
+    {
+        led2 = 0;
+    }
+    else
+    {
+        led2 = 1;
+    }
+    line = left_line * 10 + right_line;
+    if (line != 11)
+    {
+        return 1;
+    } //Sensors read something
+    else
+    {
+        return 0;
+    } //Sensors read nothing
+}
+void Drive(int LeftSpeed, int RightSpeed)
+{
+    Left.pulsewidth_us(LeftSpeed);
+    Right.pulsewidth_us(RightSpeed);
+}
+void calibrate()
+{
+    Left.period_ms(16);
+    Left.pulsewidth(MAX_PPM);
+
+    Right.period_ms(16);
+    Right.pulsewidth(MAX_PPM);
+
+    wait(3);
+
+    Drive(MED_PPM, MED_PPM);
+    wait(3);
+}
+
+void semiauto()
+{
+    Drive(MED_PPM, MED_PPM); //Stop motors
+    for (int i = 0; i < BACKWARDS; i++)
+    {
+        Drive(1550, 1450);
+    }
+}
+
+void radio()
+{
+    Drive(ch1_in, ch2_in);
+}
+
+void SEMIAUTO_mode()
+{
+    timer.start();
+    calibrate();
+    Failsafe.fall(&intLow);
+    Failsafe.rise(&intHigh);
+    ST.fall(&ST_Low);
+    ST.rise(&ST_High);
+    TH.fall(&TH_Low);
+    TH.rise(&TH_High);
+    while (1)
+    {
+        if (!failsafe) //Failsafe não ativado
+        {
+            Led4 = 1;
+            if (READ_SENSORS())
+            {
+                semiauto(); //line sensors read something
+            }
+            else
+            {
+                radio();
+            }
+        }
+        else // Failsafe ativado
+        {
+            Led4 = 0;
+            Drive(MED_PPM, MED_PPM);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_semiauto.h	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,23 @@
+void ST_High();
+
+void ST_Low();
+
+void TH_High();
+
+void TH_Low();
+
+void intHigh();
+
+void intLow();
+
+int READ_SENSORS();
+
+void Drive(int LeftSpeed, int RightSpeed);
+
+void calibrate();
+
+void semiauto();
+
+void radio();
+
+void SEMIAUTO_mode();
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,23 @@
+#include "aldebaran_auto.h"
+#include "aldebaran_rc.h"
+#include "aldebaran_semiauto.h"
+#include "mbed.h"
+
+//================================================================================
+//=====================  DIP SWITCH - ==============================================
+//================================================================================
+// escolhi pinos aleatorios aqui, depois olhar melhor isso
+DigitalIn dip_1(p5); // dip_1 -> auto
+DigitalIn dip_2(p6); // dip_2 -> rc
+DigitalIn dip_3(p7); // dip_3 -> semiauto
+
+int main()
+{
+    if (dip_1)
+        AUTO_mode();
+    else if (dip_2)
+        RC_mode();
+    else if (dip_3)
+        SEMIAUTO_mode();
+    return 0;
+}