RioBotz / aldebara_com_header

Codigo do aldebaran com headers e dip switch

aldebaran_auto.cpp@0:2228de67a3ef, 2021-03-19 (annotated)

Committer:
pedro_velozo
Date:
Fri Mar 19 16:43:46 2021 +0000
Revision:
0:2228de67a3ef
Codigo do aldebaran com adicao de headers e dip switch;

Who changed what in which revision?

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