Diff: main.cpp

Revision:

0:3e0de9e209d1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 06 19:28:07 2020 +0000
@@ -0,0 +1,241 @@
+#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()// Função necessária para dar o PPM inicial nos controladores. 
+{
+    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}; // Variável para guardar os valores dos sensores de distância.
+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); // Interrupt on digital pushbutton input p12
+Timer timer;
+DigitalOut Led3(LED3);
+int RC_value;
+int RC_start;
+int failsafe=0;
+#define FAILSAFE_SIGNAL 1400 //Valor em microssegundos do sinal do failsafe
+void intHigh()
+{
+    RC_start=timer.read_us();//Começa o timer
+}
+void intLow()
+{
+    RC_value=timer.read_us()- RC_start;//Reads ppm signal from RC receiver.  
+    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*(50),1500+5*(50));
+            break;
+
+        case 1000: //vira para a direita
+            Drive(1500+5*(30),1500+5*(30));
+            break;//
+
+        case 1100://vai reto
+            Drive(1500-5*(50),1500+5*(50));
+            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),1500+5*(30));
+            break;
+
+        //todos os outros casos que são impossíveis
+
+        default://para////////////
+            Drive(MED_PPM,MED_PPM);
+            break;
+    }
+}
+int main()
+{
+    timer.start();
+    SWITCH.fall(&desligar);//Attach interrupt function request when falling edge is occurred
+    Failsafe.fall(&intLow);//Attach interrupt function request when falling edge is occurred
+    Failsafe.rise(&intHigh);//Attach interrupt function request when rising edge is occurred
+    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();
+            Search();
+            }
+        else { // Failsafe ativado
+            Led3=0;
+            Drive(MED_PPM,MED_PPM);
+        }
+    }
+}