Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
main.cpp@0:b803244146f7, 2019-10-16 (annotated)
- Committer:
- bispomat
- Date:
- Wed Oct 16 22:35:17 2019 +0000
- Revision:
- 0:b803244146f7
- Child:
- 1:444af82e35cb
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bispomat | 0:b803244146f7 | 1 | //========================= BIBLIOTECAS =========================// |
bispomat | 0:b803244146f7 | 2 | |
bispomat | 0:b803244146f7 | 3 | #include "mbed.h" |
bispomat | 0:b803244146f7 | 4 | |
bispomat | 0:b803244146f7 | 5 | //========================= DEFINICOES =========================// |
bispomat | 0:b803244146f7 | 6 | |
bispomat | 0:b803244146f7 | 7 | PwmOut pwm(p25); |
bispomat | 0:b803244146f7 | 8 | DigitalOut dir (p26); |
bispomat | 0:b803244146f7 | 9 | PwmOut pwm1(p24); |
bispomat | 0:b803244146f7 | 10 | DigitalOut dir1 (p23); |
bispomat | 0:b803244146f7 | 11 | //DigitalIn button(p18); |
bispomat | 0:b803244146f7 | 12 | //DigitalOut led(p20); |
bispomat | 0:b803244146f7 | 13 | |
bispomat | 0:b803244146f7 | 14 | Serial bt(p28, p27); |
bispomat | 0:b803244146f7 | 15 | |
bispomat | 0:b803244146f7 | 16 | //InterruptIn A (p21); |
bispomat | 0:b803244146f7 | 17 | //DigitalIn B (p22); |
bispomat | 0:b803244146f7 | 18 | //InterruptIn C (p29); |
bispomat | 0:b803244146f7 | 19 | //DigitalIn D (p30); |
bispomat | 0:b803244146f7 | 20 | |
bispomat | 0:b803244146f7 | 21 | //Ticker END; |
bispomat | 0:b803244146f7 | 22 | //Ticker END1; |
bispomat | 0:b803244146f7 | 23 | |
bispomat | 0:b803244146f7 | 24 | DigitalIn s1(p5); |
bispomat | 0:b803244146f7 | 25 | DigitalIn s2(p6); |
bispomat | 0:b803244146f7 | 26 | DigitalIn s3(p7); |
bispomat | 0:b803244146f7 | 27 | DigitalIn s4(p8); |
bispomat | 0:b803244146f7 | 28 | DigitalIn s5(p9); |
bispomat | 0:b803244146f7 | 29 | DigitalIn s6(p10); |
bispomat | 0:b803244146f7 | 30 | DigitalIn s7(p12); |
bispomat | 0:b803244146f7 | 31 | DigitalIn s8(p13); |
bispomat | 0:b803244146f7 | 32 | |
bispomat | 0:b803244146f7 | 33 | //#define Kp_Roda 1 |
bispomat | 0:b803244146f7 | 34 | //#define Kd_Roda 1 |
bispomat | 0:b803244146f7 | 35 | //#define Ki_Roda 1 |
bispomat | 0:b803244146f7 | 36 | |
bispomat | 0:b803244146f7 | 37 | //============== VARIAVEIS MAIS IMPORTANTES ================// |
bispomat | 0:b803244146f7 | 38 | //Separadas do resto para facilitar a alteração |
bispomat | 0:b803244146f7 | 39 | |
bispomat | 0:b803244146f7 | 40 | float ParametroCorrecao = 5.3; |
bispomat | 0:b803244146f7 | 41 | float Kp_Sensor = 0.145; //0.14; //0.2 |
bispomat | 0:b803244146f7 | 42 | float Kd_Sensor = 1.64; //1.75; //1.15 |
bispomat | 0:b803244146f7 | 43 | float BaseSpeed = 0.17; |
bispomat | 0:b803244146f7 | 44 | |
bispomat | 0:b803244146f7 | 45 | //==========================================================// |
bispomat | 0:b803244146f7 | 46 | //#define VMAX_PWM 100 |
bispomat | 0:b803244146f7 | 47 | //#define VMAX_Encoder 100 |
bispomat | 0:b803244146f7 | 48 | |
bispomat | 0:b803244146f7 | 49 | //#define TESTE_MOTOR |
bispomat | 0:b803244146f7 | 50 | //#define TESTE_SENSOR |
bispomat | 0:b803244146f7 | 51 | |
bispomat | 0:b803244146f7 | 52 | Timeout END; |
bispomat | 0:b803244146f7 | 53 | //========================= VARIAVEIS GLOBAIS ========================= |
bispomat | 0:b803244146f7 | 54 | |
bispomat | 0:b803244146f7 | 55 | int Position; |
bispomat | 0:b803244146f7 | 56 | int Error=0; |
bispomat | 0:b803244146f7 | 57 | int Correction; |
bispomat | 0:b803244146f7 | 58 | int TotalError = 0; |
bispomat | 0:b803244146f7 | 59 | int LastSensor = 0; |
bispomat | 0:b803244146f7 | 60 | int SET_POINT = 45; |
bispomat | 0:b803244146f7 | 61 | |
bispomat | 0:b803244146f7 | 62 | //float Turbo = BaseSpeed; |
bispomat | 0:b803244146f7 | 63 | |
bispomat | 0:b803244146f7 | 64 | //short LimiteFim; |
bispomat | 0:b803244146f7 | 65 | //short ContaFim = 0; |
bispomat | 0:b803244146f7 | 66 | //bool LastRead = false; |
bispomat | 0:b803244146f7 | 67 | //bool Preto[15]; |
bispomat | 0:b803244146f7 | 68 | //int Threshold = 600; |
bispomat | 0:b803244146f7 | 69 | |
bispomat | 0:b803244146f7 | 70 | int LastError = 0; |
bispomat | 0:b803244146f7 | 71 | //int Integral_Esquerda = 0, Integral_Direita = 0; |
bispomat | 0:b803244146f7 | 72 | int MAX_INTEGRAL = 50; |
bispomat | 0:b803244146f7 | 73 | int MIN_INTEGRAL = -50; |
bispomat | 0:b803244146f7 | 74 | float Correcao_Sensor; |
bispomat | 0:b803244146f7 | 75 | |
bispomat | 0:b803244146f7 | 76 | //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1; |
bispomat | 0:b803244146f7 | 77 | //int Esquerda_LastError = 0, Direita_LastError = 0; |
bispomat | 0:b803244146f7 | 78 | float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida; |
bispomat | 0:b803244146f7 | 79 | float Direita_Esperada, Direita_Real, Direita_Corrigida; |
bispomat | 0:b803244146f7 | 80 | //int Esquerda_realSpeed, Direita_realSpeed; |
bispomat | 0:b803244146f7 | 81 | //int Direita_Count = 0, Direita_LastCount = 0; |
bispomat | 0:b803244146f7 | 82 | //int Esquerda_Count = 0, Esquerda_LastCount = 0; |
bispomat | 0:b803244146f7 | 83 | //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed; |
bispomat | 0:b803244146f7 | 84 | |
bispomat | 0:b803244146f7 | 85 | /*================================================= |
bispomat | 0:b803244146f7 | 86 | -------------- CODIGO DOS SENSORES ---------------- |
bispomat | 0:b803244146f7 | 87 | =================================================*/ |
bispomat | 0:b803244146f7 | 88 | |
bispomat | 0:b803244146f7 | 89 | //------------------ FUNCAO ----------------------- |
bispomat | 0:b803244146f7 | 90 | |
bispomat | 0:b803244146f7 | 91 | void Setup_Sensores (void){ //Define o modo de cada sensor como PullUp |
bispomat | 0:b803244146f7 | 92 | s1.mode(PullUp); |
bispomat | 0:b803244146f7 | 93 | s2.mode(PullUp); |
bispomat | 0:b803244146f7 | 94 | s3.mode(PullUp); |
bispomat | 0:b803244146f7 | 95 | s4.mode(PullUp); |
bispomat | 0:b803244146f7 | 96 | s5.mode(PullUp); |
bispomat | 0:b803244146f7 | 97 | s6.mode(PullUp); |
bispomat | 0:b803244146f7 | 98 | s7.mode(PullUp); |
bispomat | 0:b803244146f7 | 99 | s8.mode(PullUp); |
bispomat | 0:b803244146f7 | 100 | } |
bispomat | 0:b803244146f7 | 101 | |
bispomat | 0:b803244146f7 | 102 | void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição |
bispomat | 0:b803244146f7 | 103 | |
bispomat | 0:b803244146f7 | 104 | if (s4.read()==0 && s5.read()==0) {Position = 45;} //Se ler os dois do meio, está no SetPoint |
bispomat | 0:b803244146f7 | 105 | else if (s7.read()==0) {Position = 70;} //Ele só vai ler os sensores das extremidades |
bispomat | 0:b803244146f7 | 106 | else if (s8.read()==0) {Position = 80;} //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos) |
bispomat | 0:b803244146f7 | 107 | if (s1.read()==0) {Position = 10;} |
bispomat | 0:b803244146f7 | 108 | if (s2.read()==0) {Position = 20;} |
bispomat | 0:b803244146f7 | 109 | if (s3.read()==0) {Position = 30;} |
bispomat | 0:b803244146f7 | 110 | if (s4.read()==0) {Position = 40;} |
bispomat | 0:b803244146f7 | 111 | if (s5.read()==0) {Position = 50;} |
bispomat | 0:b803244146f7 | 112 | if (s6.read()==0) {Position = 60;} |
bispomat | 0:b803244146f7 | 113 | |
bispomat | 0:b803244146f7 | 114 | //bt.printf("\n\rPosicao: %d", Position); //Print de teste |
bispomat | 0:b803244146f7 | 115 | } |
bispomat | 0:b803244146f7 | 116 | |
bispomat | 0:b803244146f7 | 117 | /*================================================= |
bispomat | 0:b803244146f7 | 118 | --------------- CODIGO DOS MOTORES ---------------- |
bispomat | 0:b803244146f7 | 119 | =================================================*/ |
bispomat | 0:b803244146f7 | 120 | |
bispomat | 0:b803244146f7 | 121 | //------------------ FUNCOES ---------------------- |
bispomat | 0:b803244146f7 | 122 | |
bispomat | 0:b803244146f7 | 123 | void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); } |
bispomat | 0:b803244146f7 | 124 | |
bispomat | 0:b803244146f7 | 125 | void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda |
bispomat | 0:b803244146f7 | 126 | |
bispomat | 0:b803244146f7 | 127 | if (VelocidadeEsquerda <= 0){ |
bispomat | 0:b803244146f7 | 128 | VelocidadeEsquerda = abs(VelocidadeEsquerda); |
bispomat | 0:b803244146f7 | 129 | dir = 0; |
bispomat | 0:b803244146f7 | 130 | pwm.write(VelocidadeEsquerda); |
bispomat | 0:b803244146f7 | 131 | } |
bispomat | 0:b803244146f7 | 132 | else { dir = 1; pwm.write(VelocidadeEsquerda); } |
bispomat | 0:b803244146f7 | 133 | |
bispomat | 0:b803244146f7 | 134 | if (VelocidadeDireita <= 0){ |
bispomat | 0:b803244146f7 | 135 | VelocidadeDireita = abs(VelocidadeDireita); |
bispomat | 0:b803244146f7 | 136 | dir1 = 0; |
bispomat | 0:b803244146f7 | 137 | pwm1.write(VelocidadeDireita); |
bispomat | 0:b803244146f7 | 138 | } |
bispomat | 0:b803244146f7 | 139 | else { dir1 = 1; pwm1.write(VelocidadeDireita); } |
bispomat | 0:b803244146f7 | 140 | } |
bispomat | 0:b803244146f7 | 141 | |
bispomat | 0:b803244146f7 | 142 | /*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado |
bispomat | 0:b803244146f7 | 143 | |
bispomat | 0:b803244146f7 | 144 | void Volta_Pra_Linha (){ //Faz o LF voltar para o último lado em que leu a linha, caso tenha pertido (função não funcional, precisa alterar a velocidade) |
bispomat | 0:b803244146f7 | 145 | if (Position == 0){ |
bispomat | 0:b803244146f7 | 146 | if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; } |
bispomat | 0:b803244146f7 | 147 | else { Esquerda_Esperada = 80; Direita_Esperada = -50; } |
bispomat | 0:b803244146f7 | 148 | } |
bispomat | 0:b803244146f7 | 149 | else { LastSensor = Error; } |
bispomat | 0:b803244146f7 | 150 | } |
bispomat | 0:b803244146f7 | 151 | */ |
bispomat | 0:b803244146f7 | 152 | void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso |
bispomat | 0:b803244146f7 | 153 | /*================================================= |
bispomat | 0:b803244146f7 | 154 | --------------- PID DOS SENSORES ----------------- |
bispomat | 0:b803244146f7 | 155 | =================================================*/ |
bispomat | 0:b803244146f7 | 156 | |
bispomat | 0:b803244146f7 | 157 | float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção |
bispomat | 0:b803244146f7 | 158 | Error = Position - SET_POINT; |
bispomat | 0:b803244146f7 | 159 | //bt.printf("\n\rErro: %d", Error); |
bispomat | 0:b803244146f7 | 160 | |
bispomat | 0:b803244146f7 | 161 | if (Error < 20 && Error > -20){ |
bispomat | 0:b803244146f7 | 162 | Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError); |
bispomat | 0:b803244146f7 | 163 | } |
bispomat | 0:b803244146f7 | 164 | else{ |
bispomat | 0:b803244146f7 | 165 | Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError); |
bispomat | 0:b803244146f7 | 166 | } |
bispomat | 0:b803244146f7 | 167 | |
bispomat | 0:b803244146f7 | 168 | //bt.printf("\n\rCorrecao: %.2f | Erro: %d | LastError: %d | KP: %.1f | KD: %.1f", Correcao_Sensor, Error, LastError, (Kp_Sensor * (float)Error), (Kd_Sensor * ((float)Error - (float)LastError))); |
bispomat | 0:b803244146f7 | 169 | //Comando para printar e verificar os dados de saida via bluetooth |
bispomat | 0:b803244146f7 | 170 | |
bispomat | 0:b803244146f7 | 171 | LastError = Error; |
bispomat | 0:b803244146f7 | 172 | |
bispomat | 0:b803244146f7 | 173 | return Correcao_Sensor; |
bispomat | 0:b803244146f7 | 174 | } |
bispomat | 0:b803244146f7 | 175 | |
bispomat | 0:b803244146f7 | 176 | /*================================================= |
bispomat | 0:b803244146f7 | 177 | ---------------- PID DOS MOTORES ------------------ |
bispomat | 0:b803244146f7 | 178 | =================================================*/ |
bispomat | 0:b803244146f7 | 179 | |
bispomat | 0:b803244146f7 | 180 | /* |
bispomat | 0:b803244146f7 | 181 | |
bispomat | 0:b803244146f7 | 182 | void rightEncoderEvent (){ |
bispomat | 0:b803244146f7 | 183 | if (B.read() == 0){ |
bispomat | 0:b803244146f7 | 184 | Direita_Count++; |
bispomat | 0:b803244146f7 | 185 | } |
bispomat | 0:b803244146f7 | 186 | |
bispomat | 0:b803244146f7 | 187 | else{ |
bispomat | 0:b803244146f7 | 188 | Direita_Count--; |
bispomat | 0:b803244146f7 | 189 | } |
bispomat | 0:b803244146f7 | 190 | } |
bispomat | 0:b803244146f7 | 191 | |
bispomat | 0:b803244146f7 | 192 | void leftEncoderEvent(){ |
bispomat | 0:b803244146f7 | 193 | if (C.read() == 0) { Esquerda_Count++; } |
bispomat | 0:b803244146f7 | 194 | else { Esquerda_Count--; } |
bispomat | 0:b803244146f7 | 195 | } |
bispomat | 0:b803244146f7 | 196 | |
bispomat | 0:b803244146f7 | 197 | void Speed () { |
bispomat | 0:b803244146f7 | 198 | |
bispomat | 0:b803244146f7 | 199 | Esquerda_realSpeed = (Esquerda_Count - Esquerda_LastCount); //A realSpeed é o numero de voltas/giros por leitura. Comparar o maximo e minimo do Afro com a leitura do encoder. |
bispomat | 0:b803244146f7 | 200 | Esquerda_LastCount = Esquerda_Count; |
bispomat | 0:b803244146f7 | 201 | |
bispomat | 0:b803244146f7 | 202 | Direita_realSpeed = (Direita_Count - Direita_LastCount); |
bispomat | 0:b803244146f7 | 203 | Direita_LastCount = Direita_Count; |
bispomat | 0:b803244146f7 | 204 | //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed); |
bispomat | 0:b803244146f7 | 205 | |
bispomat | 0:b803244146f7 | 206 | } |
bispomat | 0:b803244146f7 | 207 | |
bispomat | 0:b803244146f7 | 208 | int Converte_Velocidade (int realSpeed){ |
bispomat | 0:b803244146f7 | 209 | int realPWM; |
bispomat | 0:b803244146f7 | 210 | |
bispomat | 0:b803244146f7 | 211 | realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder; |
bispomat | 0:b803244146f7 | 212 | |
bispomat | 0:b803244146f7 | 213 | return realPWM; |
bispomat | 0:b803244146f7 | 214 | } |
bispomat | 0:b803244146f7 | 215 | |
bispomat | 0:b803244146f7 | 216 | int PID_Roda (int Lado_realPWM, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){ |
bispomat | 0:b803244146f7 | 217 | int proporcional, derivativo, Correcao, Velocidade_Corrigida; |
bispomat | 0:b803244146f7 | 218 | |
bispomat | 0:b803244146f7 | 219 | proporcional = Lado_realPWM - Lado_VelocidadeEsperada; |
bispomat | 0:b803244146f7 | 220 | //bt.printf("\n\r%d",proporcional); |
bispomat | 0:b803244146f7 | 221 | derivativo = proporcional - Lado_LastError; |
bispomat | 0:b803244146f7 | 222 | //bt.printf("\n\rDerivativo: %d",derivativo); |
bispomat | 0:b803244146f7 | 223 | |
bispomat | 0:b803244146f7 | 224 | Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda; |
bispomat | 0:b803244146f7 | 225 | //bt.printf("\n\r%d",Correcao); |
bispomat | 0:b803244146f7 | 226 | |
bispomat | 0:b803244146f7 | 227 | Lado_LastError = proporcional; |
bispomat | 0:b803244146f7 | 228 | Integral_Lado_Roda += proporcional; |
bispomat | 0:b803244146f7 | 229 | |
bispomat | 0:b803244146f7 | 230 | if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; } |
bispomat | 0:b803244146f7 | 231 | |
bispomat | 0:b803244146f7 | 232 | if (proporcional == 0){ Lado_LastError = 0; } |
bispomat | 0:b803244146f7 | 233 | else{ |
bispomat | 0:b803244146f7 | 234 | if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; } |
bispomat | 0:b803244146f7 | 235 | else { Integral_Lado_Roda = MIN_INTEGRAL; } |
bispomat | 0:b803244146f7 | 236 | |
bispomat | 0:b803244146f7 | 237 | } |
bispomat | 0:b803244146f7 | 238 | Lado_lastExpectedSpeed = Lado_VelocidadeEsperada; |
bispomat | 0:b803244146f7 | 239 | |
bispomat | 0:b803244146f7 | 240 | Velocidade_Corrigida = (Lado_realPWM - Correcao); |
bispomat | 0:b803244146f7 | 241 | |
bispomat | 0:b803244146f7 | 242 | return Velocidade_Corrigida; |
bispomat | 0:b803244146f7 | 243 | } |
bispomat | 0:b803244146f7 | 244 | |
bispomat | 0:b803244146f7 | 245 | */ |
bispomat | 0:b803244146f7 | 246 | |
bispomat | 0:b803244146f7 | 247 | /*================================================= |
bispomat | 0:b803244146f7 | 248 | --------------- FUNCAO PRINCIPAL ----------------- |
bispomat | 0:b803244146f7 | 249 | =================================================*/ |
bispomat | 0:b803244146f7 | 250 | |
bispomat | 0:b803244146f7 | 251 | int main(void){ |
bispomat | 0:b803244146f7 | 252 | |
bispomat | 0:b803244146f7 | 253 | //A.rise(&rightEncoderEvent); |
bispomat | 0:b803244146f7 | 254 | //END.attach(&Speed, 1.0); |
bispomat | 0:b803244146f7 | 255 | //C.rise(&leftEncoderEvent); |
bispomat | 0:b803244146f7 | 256 | |
bispomat | 0:b803244146f7 | 257 | //END.attach(&Speed, 1.0); |
bispomat | 0:b803244146f7 | 258 | //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA |
bispomat | 0:b803244146f7 | 259 | |
bispomat | 0:b803244146f7 | 260 | Setup_Sensores(); //Faz o setup dos sensores |
bispomat | 0:b803244146f7 | 261 | Setup_Motores(); //Faz o setup dos motores |
bispomat | 0:b803244146f7 | 262 | |
bispomat | 0:b803244146f7 | 263 | //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); } |
bispomat | 0:b803244146f7 | 264 | |
bispomat | 0:b803244146f7 | 265 | wait(2); //Delay para estabilizar |
bispomat | 0:b803244146f7 | 266 | |
bispomat | 0:b803244146f7 | 267 | pwm.write(0.00); //Pulso inicial para cada motor |
bispomat | 0:b803244146f7 | 268 | pwm1.write(0.00); |
bispomat | 0:b803244146f7 | 269 | |
bispomat | 0:b803244146f7 | 270 | END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos |
bispomat | 0:b803244146f7 | 271 | |
bispomat | 0:b803244146f7 | 272 | while(1){ |
bispomat | 0:b803244146f7 | 273 | //Para_Demonio(); |
bispomat | 0:b803244146f7 | 274 | Leitura_Sensores(); //Lê os sensores |
bispomat | 0:b803244146f7 | 275 | |
bispomat | 0:b803244146f7 | 276 | Correction = PID_Sensores(Position); //Calcula a correção |
bispomat | 0:b803244146f7 | 277 | |
bispomat | 0:b803244146f7 | 278 | Esquerda_Esperada = BaseSpeed + (Correction * 0.01); //Aplica a correção |
bispomat | 0:b803244146f7 | 279 | Direita_Esperada = BaseSpeed - (Correction * 0.01); |
bispomat | 0:b803244146f7 | 280 | |
bispomat | 0:b803244146f7 | 281 | //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada); |
bispomat | 0:b803244146f7 | 282 | //Print das velocidades que estão chegando nas rodas após o PID |
bispomat | 0:b803244146f7 | 283 | |
bispomat | 0:b803244146f7 | 284 | //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido |
bispomat | 0:b803244146f7 | 285 | |
bispomat | 0:b803244146f7 | 286 | Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive |
bispomat | 0:b803244146f7 | 287 | |
bispomat | 0:b803244146f7 | 288 | } |
bispomat | 0:b803244146f7 | 289 | } |