Diff: aldebaran_semiauto.cpp

Revision:

0:2228de67a3ef

--- /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