RioBotz / Mbed 2 deprecated AldebaranAutoIRFinal

Código Aldebaran+

Dependencies:   mbed

main.cpp@0:3e0de9e209d1, 2020-05-06 (annotated)

Committer:
Mateusjesus
Date:
Wed May 06 19:28:07 2020 +0000
Revision:
0:3e0de9e209d1
'

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mateusjesus 0:3e0de9e209d1 1 #include"mbed.h"
Mateusjesus 0:3e0de9e209d1 2 #include"PwmOut.h"
Mateusjesus 0:3e0de9e209d1 3 Serial bt(p9, p10);
Mateusjesus 0:3e0de9e209d1 4 //================================================================================
Mateusjesus 0:3e0de9e209d1 5 //========================= MOTORS CONTROL - ====================================
Mateusjesus 0:3e0de9e209d1 6 //================================================================================
Mateusjesus 0:3e0de9e209d1 7 PwmOut Right(p21);
Mateusjesus 0:3e0de9e209d1 8 PwmOut Left(p22);
Mateusjesus 0:3e0de9e209d1 9 #define MED_PPM 1500 // Valor indicando que o motor deve ficar parado de acordo com o ppm
Mateusjesus 0:3e0de9e209d1 10 int LSpeedGlobal;
Mateusjesus 0:3e0de9e209d1 11 int RSpeedGlobal;
Mateusjesus 0:3e0de9e209d1 12
Mateusjesus 0:3e0de9e209d1 13 void Drive(int LeftSpeed,int RightSpeed)
Mateusjesus 0:3e0de9e209d1 14 {
Mateusjesus 0:3e0de9e209d1 15 Left.pulsewidth_us(LeftSpeed);
Mateusjesus 0:3e0de9e209d1 16 Right.pulsewidth_us(RightSpeed);
Mateusjesus 0:3e0de9e209d1 17 LSpeedGlobal = LeftSpeed;
Mateusjesus 0:3e0de9e209d1 18 RSpeedGlobal = RightSpeed;
Mateusjesus 0:3e0de9e209d1 19 }
Mateusjesus 0:3e0de9e209d1 20 void calibrate()// Função necessária para dar o PPM inicial nos controladores.
Mateusjesus 0:3e0de9e209d1 21 {
Mateusjesus 0:3e0de9e209d1 22 Drive(MED_PPM, MED_PPM);
Mateusjesus 0:3e0de9e209d1 23 wait(3);
Mateusjesus 0:3e0de9e209d1 24 }
Mateusjesus 0:3e0de9e209d1 25 //================================================================================
Mateusjesus 0:3e0de9e209d1 26 //===================== DISTANCE SENSORS - =====================================
Mateusjesus 0:3e0de9e209d1 27 //================================================================================
Mateusjesus 0:3e0de9e209d1 28 //**************************** DEFINICOES - ************************************
Mateusjesus 0:3e0de9e209d1 29 DigitalIn Sensor1 (p26);// Sensor digital localizado na esquerda do robo
Mateusjesus 0:3e0de9e209d1 30 DigitalIn Sensor2 (p27);//Sensor digital localizado na diagonal esquerda do robo
Mateusjesus 0:3e0de9e209d1 31 DigitalIn Sensor3 (p28);//Sensor digital localizado na frente do robo
Mateusjesus 0:3e0de9e209d1 32 DigitalIn Sensor4 (p25); //Sensor digital localizado na diagonal direita do robo
Mateusjesus 0:3e0de9e209d1 33 DigitalIn Sensor5 (p30); //Sensor digital localizado na direita do robo
Mateusjesus 0:3e0de9e209d1 34
Mateusjesus 0:3e0de9e209d1 35 int measure[5]= {0,0,0,0,0}; // Variável para guardar os valores dos sensores de distância.
Mateusjesus 0:3e0de9e209d1 36 int total = 0; //{left, left_diagonal, front, right_diagonal, right}
Mateusjesus 0:3e0de9e209d1 37
Mateusjesus 0:3e0de9e209d1 38 void Search ()
Mateusjesus 0:3e0de9e209d1 39 {
Mateusjesus 0:3e0de9e209d1 40 measure[0] = Sensor1.read();
Mateusjesus 0:3e0de9e209d1 41 measure[1] = Sensor2.read();
Mateusjesus 0:3e0de9e209d1 42 measure[2] = Sensor3.read();
Mateusjesus 0:3e0de9e209d1 43 measure[3] = Sensor4.read();
Mateusjesus 0:3e0de9e209d1 44 measure[4] = Sensor5.read();
Mateusjesus 0:3e0de9e209d1 45
Mateusjesus 0:3e0de9e209d1 46 total = measure[4]*10000 + measure[3]*1000 + measure[2]*100 + measure[1]*10 + measure[0];
Mateusjesus 0:3e0de9e209d1 47 return;
Mateusjesus 0:3e0de9e209d1 48 }
Mateusjesus 0:3e0de9e209d1 49 //================================================================================
Mateusjesus 0:3e0de9e209d1 50 //===================== SWITCH INICIALIZATION - =================================
Mateusjesus 0:3e0de9e209d1 51 //================================================================================
Mateusjesus 0:3e0de9e209d1 52 InterruptIn SWITCH(p11);
Mateusjesus 0:3e0de9e209d1 53 DigitalOut Led4(LED4);
Mateusjesus 0:3e0de9e209d1 54 void desligar()
Mateusjesus 0:3e0de9e209d1 55 {
Mateusjesus 0:3e0de9e209d1 56 Drive(MED_PPM,MED_PPM);
Mateusjesus 0:3e0de9e209d1 57 }
Mateusjesus 0:3e0de9e209d1 58 //================================================================================
Mateusjesus 0:3e0de9e209d1 59 //===================== FAILSAFE - ==============================================
Mateusjesus 0:3e0de9e209d1 60 //================================================================================
Mateusjesus 0:3e0de9e209d1 61 InterruptIn Failsafe(p12); // Interrupt on digital pushbutton input p12
Mateusjesus 0:3e0de9e209d1 62 Timer timer;
Mateusjesus 0:3e0de9e209d1 63 DigitalOut Led3(LED3);
Mateusjesus 0:3e0de9e209d1 64 int RC_value;
Mateusjesus 0:3e0de9e209d1 65 int RC_start;
Mateusjesus 0:3e0de9e209d1 66 int failsafe=0;
Mateusjesus 0:3e0de9e209d1 67 #define FAILSAFE_SIGNAL 1400 //Valor em microssegundos do sinal do failsafe
Mateusjesus 0:3e0de9e209d1 68 void intHigh()
Mateusjesus 0:3e0de9e209d1 69 {
Mateusjesus 0:3e0de9e209d1 70 RC_start=timer.read_us();//Começa o timer
Mateusjesus 0:3e0de9e209d1 71 }
Mateusjesus 0:3e0de9e209d1 72 void intLow()
Mateusjesus 0:3e0de9e209d1 73 {
Mateusjesus 0:3e0de9e209d1 74 RC_value=timer.read_us()- RC_start;//Reads ppm signal from RC receiver.
Mateusjesus 0:3e0de9e209d1 75 if(RC_value > FAILSAFE_SIGNAL) {
Mateusjesus 0:3e0de9e209d1 76 failsafe=0; //failsafe não ativado
Mateusjesus 0:3e0de9e209d1 77 } else {
Mateusjesus 0:3e0de9e209d1 78 failsafe=1; //failsafe ativado
Mateusjesus 0:3e0de9e209d1 79 }
Mateusjesus 0:3e0de9e209d1 80 }
Mateusjesus 0:3e0de9e209d1 81
Mateusjesus 0:3e0de9e209d1 82 //================================================================================
Mateusjesus 0:3e0de9e209d1 83 //===================== LINE SENSORS - =========================================
Mateusjesus 0:3e0de9e209d1 84 //================================================================================
Mateusjesus 0:3e0de9e209d1 85 #define PARAMETER 0.908
Mateusjesus 0:3e0de9e209d1 86 #define RightSensor p18
Mateusjesus 0:3e0de9e209d1 87 #define LeftSensor p20
Mateusjesus 0:3e0de9e209d1 88 DigitalOut Led1 (LED1);
Mateusjesus 0:3e0de9e209d1 89 DigitalOut Led2 (LED2);
Mateusjesus 0:3e0de9e209d1 90 AnalogIn RightRead(RightSensor);
Mateusjesus 0:3e0de9e209d1 91 AnalogIn LeftRead(LeftSensor);
Mateusjesus 0:3e0de9e209d1 92 bool right_line=1,left_line=1;
Mateusjesus 0:3e0de9e209d1 93 int result=0,anterior=0;
Mateusjesus 0:3e0de9e209d1 94 int Line_check ()
Mateusjesus 0:3e0de9e209d1 95 {
Mateusjesus 0:3e0de9e209d1 96 if(RightRead.read()>PARAMETER) {
Mateusjesus 0:3e0de9e209d1 97 right_line=1;
Mateusjesus 0:3e0de9e209d1 98 }
Mateusjesus 0:3e0de9e209d1 99 else {
Mateusjesus 0:3e0de9e209d1 100 right_line=0; //white
Mateusjesus 0:3e0de9e209d1 101 }
Mateusjesus 0:3e0de9e209d1 102 if(LeftRead.read()>PARAMETER) {
Mateusjesus 0:3e0de9e209d1 103 left_line=1;
Mateusjesus 0:3e0de9e209d1 104 } else {
Mateusjesus 0:3e0de9e209d1 105 left_line=0;
Mateusjesus 0:3e0de9e209d1 106 }
Mateusjesus 0:3e0de9e209d1 107 result=right_line*10+left_line;
Mateusjesus 0:3e0de9e209d1 108 if(result==0 && anterior==0) {
Mateusjesus 0:3e0de9e209d1 109 return 0;
Mateusjesus 0:3e0de9e209d1 110 }
Mateusjesus 0:3e0de9e209d1 111 if(result==0 && anterior==1) {
Mateusjesus 0:3e0de9e209d1 112 anterior=0;
Mateusjesus 0:3e0de9e209d1 113 wait_ms(0);
Mateusjesus 0:3e0de9e209d1 114 return 0;
Mateusjesus 0:3e0de9e209d1 115 }
Mateusjesus 0:3e0de9e209d1 116 switch(result) {
Mateusjesus 0:3e0de9e209d1 117 case 10: { //Sensor de linha da direita viu a linha
Mateusjesus 0:3e0de9e209d1 118 Led2=1;
Mateusjesus 0:3e0de9e209d1 119 Led1=0;
Mateusjesus 0:3e0de9e209d1 120 Drive(1500+5*(50),1500-5*(50)); //Para tras com velocidade máxima por 50ms
Mateusjesus 0:3e0de9e209d1 121 wait(1);
Mateusjesus 0:3e0de9e209d1 122 Drive(1500-5*(35),1500-5*(35));//Para esquerda com velocidade máxima por 100ms
Mateusjesus 0:3e0de9e209d1 123 wait(0.5);
Mateusjesus 0:3e0de9e209d1 124 anterior=1;
Mateusjesus 0:3e0de9e209d1 125 break;
Mateusjesus 0:3e0de9e209d1 126 }
Mateusjesus 0:3e0de9e209d1 127
Mateusjesus 0:3e0de9e209d1 128 case 1: { //Sensor de linha da esquerda viu a linha
Mateusjesus 0:3e0de9e209d1 129 Led2=0;
Mateusjesus 0:3e0de9e209d1 130 Led1=1;
Mateusjesus 0:3e0de9e209d1 131 Drive(1500+5*(50),1500-5*(50));//Para tras com velocidade máxima por 100ms
Mateusjesus 0:3e0de9e209d1 132 wait(1);
Mateusjesus 0:3e0de9e209d1 133 Drive(1500+5*(35),1500+5*(35)); //Para direita com velocidade máxima por 50ms
Mateusjesus 0:3e0de9e209d1 134 wait(0.5);
Mateusjesus 0:3e0de9e209d1 135 anterior=1;
Mateusjesus 0:3e0de9e209d1 136 break;
Mateusjesus 0:3e0de9e209d1 137 }
Mateusjesus 0:3e0de9e209d1 138
Mateusjesus 0:3e0de9e209d1 139 case 11: { //Sensor de linha da esquerda e da direita viram a linha
Mateusjesus 0:3e0de9e209d1 140 Led2=1;
Mateusjesus 0:3e0de9e209d1 141 Led1=1;
Mateusjesus 0:3e0de9e209d1 142 Drive(1500+5*(50),1500-5*(50));//Para tras
Mateusjesus 0:3e0de9e209d1 143 wait(1);
Mateusjesus 0:3e0de9e209d1 144 Drive(1500+5*(35),1500+5*(35)); //Para direita
Mateusjesus 0:3e0de9e209d1 145 wait(0.5);
Mateusjesus 0:3e0de9e209d1 146 anterior=1;
Mateusjesus 0:3e0de9e209d1 147 break;
Mateusjesus 0:3e0de9e209d1 148
Mateusjesus 0:3e0de9e209d1 149 }
Mateusjesus 0:3e0de9e209d1 150 default: {
Mateusjesus 0:3e0de9e209d1 151 anterior=1;
Mateusjesus 0:3e0de9e209d1 152 Led2=0;
Mateusjesus 0:3e0de9e209d1 153 Led1=0;
Mateusjesus 0:3e0de9e209d1 154 break;
Mateusjesus 0:3e0de9e209d1 155 }
Mateusjesus 0:3e0de9e209d1 156 }
Mateusjesus 0:3e0de9e209d1 157 return 1;
Mateusjesus 0:3e0de9e209d1 158 }
Mateusjesus 0:3e0de9e209d1 159 void Action ()
Mateusjesus 0:3e0de9e209d1 160 {
Mateusjesus 0:3e0de9e209d1 161
Mateusjesus 0:3e0de9e209d1 162 switch(total) {
Mateusjesus 0:3e0de9e209d1 163
Mateusjesus 0:3e0de9e209d1 164 case 0: //caso nao veja nenhum dos sensores
Mateusjesus 0:3e0de9e209d1 165 Drive(LSpeedGlobal,RSpeedGlobal);
Mateusjesus 0:3e0de9e209d1 166 break;
Mateusjesus 0:3e0de9e209d1 167
Mateusjesus 0:3e0de9e209d1 168 case 1: //vira para a esquerda
Mateusjesus 0:3e0de9e209d1 169 Drive(1500-5*(40),1500-5*(40));
Mateusjesus 0:3e0de9e209d1 170 break;
Mateusjesus 0:3e0de9e209d1 171
Mateusjesus 0:3e0de9e209d1 172 case 10: //vira para a esquerda
Mateusjesus 0:3e0de9e209d1 173 Drive(1500-5*(30),1500-5*(30));
Mateusjesus 0:3e0de9e209d1 174 break;
Mateusjesus 0:3e0de9e209d1 175
Mateusjesus 0:3e0de9e209d1 176 case 11://vira para a esquerda
Mateusjesus 0:3e0de9e209d1 177 Drive(1500-5*(30),1500-5*(30));
Mateusjesus 0:3e0de9e209d1 178 break;
Mateusjesus 0:3e0de9e209d1 179
Mateusjesus 0:3e0de9e209d1 180 case 100: //vai reto
Mateusjesus 0:3e0de9e209d1 181 Drive(1500-5*(60),1500+5*(60));
Mateusjesus 0:3e0de9e209d1 182 break;
Mateusjesus 0:3e0de9e209d1 183
Mateusjesus 0:3e0de9e209d1 184 case 110://vai reto
Mateusjesus 0:3e0de9e209d1 185 Drive(1500-5*(50),1500+5*(50));
Mateusjesus 0:3e0de9e209d1 186 break;
Mateusjesus 0:3e0de9e209d1 187
Mateusjesus 0:3e0de9e209d1 188 case 1000: //vira para a direita
Mateusjesus 0:3e0de9e209d1 189 Drive(1500+5*(30),1500+5*(30));
Mateusjesus 0:3e0de9e209d1 190 break;//
Mateusjesus 0:3e0de9e209d1 191
Mateusjesus 0:3e0de9e209d1 192 case 1100://vai reto
Mateusjesus 0:3e0de9e209d1 193 Drive(1500-5*(50),1500+5*(50));
Mateusjesus 0:3e0de9e209d1 194 break;
Mateusjesus 0:3e0de9e209d1 195
Mateusjesus 0:3e0de9e209d1 196 case 1110://vai reto (GRANDE CHIFRE)
Mateusjesus 0:3e0de9e209d1 197 Drive(1500-5*(100),1500+5*(100));
Mateusjesus 0:3e0de9e209d1 198 break;
Mateusjesus 0:3e0de9e209d1 199
Mateusjesus 0:3e0de9e209d1 200 case 10000: //vira para direita
Mateusjesus 0:3e0de9e209d1 201 Drive(1500+5*(40),1500+5*(40));
Mateusjesus 0:3e0de9e209d1 202 break;
Mateusjesus 0:3e0de9e209d1 203
Mateusjesus 0:3e0de9e209d1 204 case 11000://vira para a direita
Mateusjesus 0:3e0de9e209d1 205 Drive (1500+5*(30),1500+5*(30));
Mateusjesus 0:3e0de9e209d1 206 break;
Mateusjesus 0:3e0de9e209d1 207
Mateusjesus 0:3e0de9e209d1 208 //todos os outros casos que são impossíveis
Mateusjesus 0:3e0de9e209d1 209
Mateusjesus 0:3e0de9e209d1 210 default://para////////////
Mateusjesus 0:3e0de9e209d1 211 Drive(MED_PPM,MED_PPM);
Mateusjesus 0:3e0de9e209d1 212 break;
Mateusjesus 0:3e0de9e209d1 213 }
Mateusjesus 0:3e0de9e209d1 214 }
Mateusjesus 0:3e0de9e209d1 215 int main()
Mateusjesus 0:3e0de9e209d1 216 {
Mateusjesus 0:3e0de9e209d1 217 timer.start();
Mateusjesus 0:3e0de9e209d1 218 SWITCH.fall(&desligar);//Attach interrupt function request when falling edge is occurred
Mateusjesus 0:3e0de9e209d1 219 Failsafe.fall(&intLow);//Attach interrupt function request when falling edge is occurred
Mateusjesus 0:3e0de9e209d1 220 Failsafe.rise(&intHigh);//Attach interrupt function request when rising edge is occurred
Mateusjesus 0:3e0de9e209d1 221 calibrate();
Mateusjesus 0:3e0de9e209d1 222 LSpeedGlobal = 1500-5*(30);
Mateusjesus 0:3e0de9e209d1 223 RSpeedGlobal = 1500+5*(30);
Mateusjesus 0:3e0de9e209d1 224
Mateusjesus 0:3e0de9e209d1 225 while(SWITCH.read()==0) {
Mateusjesus 0:3e0de9e209d1 226 Search();
Mateusjesus 0:3e0de9e209d1 227 Led4=1;
Mateusjesus 0:3e0de9e209d1 228 }
Mateusjesus 0:3e0de9e209d1 229 Led4=0;
Mateusjesus 0:3e0de9e209d1 230 while(1) {
Mateusjesus 0:3e0de9e209d1 231 if(!failsafe && SWITCH.read()) { //Failsafe não ativado
Mateusjesus 0:3e0de9e209d1 232 Led3=1;
Mateusjesus 0:3e0de9e209d1 233 Action();
Mateusjesus 0:3e0de9e209d1 234 Search();
Mateusjesus 0:3e0de9e209d1 235 }
Mateusjesus 0:3e0de9e209d1 236 else { // Failsafe ativado
Mateusjesus 0:3e0de9e209d1 237 Led3=0;
Mateusjesus 0:3e0de9e209d1 238 Drive(MED_PPM,MED_PPM);
Mateusjesus 0:3e0de9e209d1 239 }
Mateusjesus 0:3e0de9e209d1 240 }
Mateusjesus 0:3e0de9e209d1 241 }