RioBotz / Mbed 2 deprecated LineFollowerV3_Iron_01-03-2020

Codigo comentado da IronCup 01/03/2020

Dependencies:   mbed

main.cpp@4:247c0fc124db, 2020-02-25 (annotated)

Committer:
rperoba
Date:
Tue Feb 25 21:20:05 2020 +0000
Revision:
4:247c0fc124db
Parent:
3:3b0c36115348
Child:
5:41a59c155649
Acertamos o pino do turbo que estava errado;

Who changed what in which revision?

UserRevisionLine numberNew 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
rperoba 3:3b0c36115348 7 #define threshold 120 // parametro que diferencia a leitura do branco pro preto
bispomat 0:b803244146f7 8 PwmOut pwm(p25);
bispomat 0:b803244146f7 9 DigitalOut dir (p26);
bispomat 0:b803244146f7 10 PwmOut pwm1(p24);
bispomat 0:b803244146f7 11 DigitalOut dir1 (p23);
bispomat 0:b803244146f7 12 //DigitalIn button(p18);
bispomat 0:b803244146f7 13 //DigitalOut led(p20);
bispomat 0:b803244146f7 14
bispomat 0:b803244146f7 15 Serial bt(p28, p27);
bispomat 1:444af82e35cb 16 Serial pc(USBTX, USBRX);
bispomat 0:b803244146f7 17
bispomat 0:b803244146f7 18 //InterruptIn A (p21);
bispomat 0:b803244146f7 19 //DigitalIn B (p22);
bispomat 0:b803244146f7 20 //InterruptIn C (p29);
bispomat 0:b803244146f7 21 //DigitalIn D (p30);
bispomat 0:b803244146f7 22
bispomat 0:b803244146f7 23 //Ticker END;
bispomat 0:b803244146f7 24 //Ticker END1;
rperoba 4:247c0fc124db 25 DigitalIn sensorTurbo(p10);
bispomat 1:444af82e35cb 26 DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha
bispomat 1:444af82e35cb 27 DigitalOut multiplexador2(p6);
bispomat 1:444af82e35cb 28 DigitalOut multiplexador3(p8); // PRECISA AJUSTAR OS PINOS
bispomat 1:444af82e35cb 29 DigitalOut multiplexador4(p7);
bispomat 1:444af82e35cb 30 //DigitalOut multiplexador5(p9);// Enable do multiplexador
bispomat 1:444af82e35cb 31 DigitalInOut multiplexadorInOut(p12); // In/Out do multiplexador
bispomat 1:444af82e35cb 32
bispomat 1:444af82e35cb 33 int tabelaVerdade1[16] = {0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1}; // Arrays para selecionar o sensor de linha na função sensorCheck
bispomat 1:444af82e35cb 34 int tabelaVerdade2[16] = {0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1};
bispomat 1:444af82e35cb 35 int tabelaVerdade3[16] = {0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1};
bispomat 1:444af82e35cb 36 int tabelaVerdade4[16] = {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1};
bispomat 1:444af82e35cb 37
bispomat 1:444af82e35cb 38 Timer tempoDoSensor; // timer para leitura do sensor de linha
bispomat 0:b803244146f7 39
bispomat 0:b803244146f7 40 //#define Kp_Roda 1
bispomat 0:b803244146f7 41 //#define Kd_Roda 1
bispomat 0:b803244146f7 42 //#define Ki_Roda 1
bispomat 0:b803244146f7 43
bispomat 0:b803244146f7 44 //============== VARIAVEIS MAIS IMPORTANTES ================//
bispomat 0:b803244146f7 45 //Separadas do resto para facilitar a alteração
bispomat 0:b803244146f7 46
rperoba 3:3b0c36115348 47 float ParametroCorrecao = 2.43; //2
rperoba 3:3b0c36115348 48 float Kp_Sensor = 0.24; //0.14; //0.2 //0.16
rperoba 3:3b0c36115348 49 float Kd_Sensor = 0.7; //1.75; //1.15 //0.3
rperoba 3:3b0c36115348 50 float BaseSpeed = 0.15; //0.12
bispomat 0:b803244146f7 51
bispomat 0:b803244146f7 52 //==========================================================//
bispomat 0:b803244146f7 53 //#define VMAX_PWM 100
bispomat 0:b803244146f7 54 //#define VMAX_Encoder 100
bispomat 0:b803244146f7 55
bispomat 0:b803244146f7 56 //#define TESTE_MOTOR
bispomat 0:b803244146f7 57 //#define TESTE_SENSOR
bispomat 0:b803244146f7 58
bispomat 0:b803244146f7 59 Timeout END;
bispomat 0:b803244146f7 60 //========================= VARIAVEIS GLOBAIS =========================
bispomat 0:b803244146f7 61
bispomat 0:b803244146f7 62 int Position;
bispomat 0:b803244146f7 63 int Error=0;
bispomat 0:b803244146f7 64 int Correction;
bispomat 0:b803244146f7 65 int TotalError = 0;
bispomat 0:b803244146f7 66 int LastSensor = 0;
bispomat 0:b803244146f7 67 int SET_POINT = 45;
bispomat 0:b803244146f7 68
rperoba 3:3b0c36115348 69 float turbo = 0.05;
bispomat 0:b803244146f7 70
bispomat 0:b803244146f7 71 //short LimiteFim;
bispomat 0:b803244146f7 72 //short ContaFim = 0;
bispomat 0:b803244146f7 73 //bool LastRead = false;
bispomat 0:b803244146f7 74 //bool Preto[15];
bispomat 0:b803244146f7 75 //int Threshold = 600;
bispomat 0:b803244146f7 76
bispomat 0:b803244146f7 77 int LastError = 0;
bispomat 0:b803244146f7 78 //int Integral_Esquerda = 0, Integral_Direita = 0;
bispomat 0:b803244146f7 79 int MAX_INTEGRAL = 50;
bispomat 0:b803244146f7 80 int MIN_INTEGRAL = -50;
bispomat 0:b803244146f7 81 float Correcao_Sensor;
bispomat 0:b803244146f7 82
bispomat 0:b803244146f7 83 //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1;
bispomat 0:b803244146f7 84 //int Esquerda_LastError = 0, Direita_LastError = 0;
bispomat 0:b803244146f7 85 float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida;
bispomat 0:b803244146f7 86 float Direita_Esperada, Direita_Real, Direita_Corrigida;
bispomat 0:b803244146f7 87 //int Esquerda_realSpeed, Direita_realSpeed;
bispomat 0:b803244146f7 88 //int Direita_Count = 0, Direita_LastCount = 0;
bispomat 0:b803244146f7 89 //int Esquerda_Count = 0, Esquerda_LastCount = 0;
bispomat 0:b803244146f7 90 //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed;
bispomat 0:b803244146f7 91
bispomat 0:b803244146f7 92 /*=================================================
bispomat 0:b803244146f7 93 -------------- CODIGO DOS SENSORES ----------------
bispomat 0:b803244146f7 94 =================================================*/
bispomat 0:b803244146f7 95
bispomat 0:b803244146f7 96 //------------------ FUNCAO -----------------------
bispomat 0:b803244146f7 97
bispomat 1:444af82e35cb 98 void Setup_Sensores (void){ //Define o modo de cada sensor
bispomat 1:444af82e35cb 99 multiplexadorInOut.mode(PullNone);
rperoba 3:3b0c36115348 100 sensorTurbo.mode(PullUp);
bispomat 0:b803244146f7 101 }
bispomat 0:b803244146f7 102
bispomat 1:444af82e35cb 103 bool sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15
bispomat 1:444af82e35cb 104 // função para ler os motores,retorna true se for branco e false se for preto
bispomat 1:444af82e35cb 105
bispomat 1:444af82e35cb 106 multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor
bispomat 1:444af82e35cb 107 multiplexador2 = tabelaVerdade2[sensor];
bispomat 1:444af82e35cb 108 multiplexador3 = tabelaVerdade3[sensor];
bispomat 1:444af82e35cb 109 multiplexador4 = tabelaVerdade4[sensor];
bispomat 1:444af82e35cb 110 //pc.printf("Status do Sensor entrando no check: %d ", multiplexadorInOut.read());
bispomat 1:444af82e35cb 111 multiplexadorInOut.output(); // colocando o InOut como Out
bispomat 1:444af82e35cb 112 multiplexadorInOut = 1; // levando ele a high por pelo menos 10 microssegundos
bispomat 1:444af82e35cb 113
bispomat 1:444af82e35cb 114 tempoDoSensor.start(); // iniciando o contador
bispomat 1:444af82e35cb 115
rperoba 2:28f4f676e446 116 wait_us(10); // tempo para o output ir para high
bispomat 1:444af82e35cb 117
bispomat 1:444af82e35cb 118 multiplexadorInOut.input(); // colocando o InOut como In
bispomat 1:444af82e35cb 119 //pc.printf("Status do Sensor supostamente em HIGH: %d ", multiplexadorInOut.read());
bispomat 1:444af82e35cb 120
bispomat 1:444af82e35cb 121 while(multiplexadorInOut == 1){// enrolando enquanto o sensor esta lendo
bispomat 1:444af82e35cb 122 //pc.printf("To no loop ->");
bispomat 1:444af82e35cb 123 //pc.printf("Status do Sensor: %d ", multiplexadorInOut.read());
bispomat 1:444af82e35cb 124
bispomat 1:444af82e35cb 125 if (tempoDoSensor.read_us() >= threshold){// termina o while se o tempo for maior que o threshold,agilizando o processo de leitura
bispomat 1:444af82e35cb 126 tempoDoSensor.stop();
bispomat 1:444af82e35cb 127 //pc.printf("To no if ->");
bispomat 1:444af82e35cb 128 float tempoDeLeitura = tempoDoSensor.read_us();
bispomat 1:444af82e35cb 129 //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
bispomat 1:444af82e35cb 130 tempoDoSensor.reset();
bispomat 1:444af82e35cb 131 multiplexadorInOut.output();
bispomat 1:444af82e35cb 132 multiplexadorInOut = 0;
bispomat 1:444af82e35cb 133 return false; // retorna quando for preto
bispomat 1:444af82e35cb 134 }
bispomat 1:444af82e35cb 135
bispomat 1:444af82e35cb 136 }
bispomat 1:444af82e35cb 137 tempoDoSensor.stop();
bispomat 1:444af82e35cb 138 float tempoDeLeitura = tempoDoSensor.read_us();
bispomat 1:444af82e35cb 139 //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug
bispomat 1:444af82e35cb 140 tempoDoSensor.reset();
bispomat 1:444af82e35cb 141 multiplexadorInOut.output();
bispomat 1:444af82e35cb 142 multiplexadorInOut = 0;
bispomat 1:444af82e35cb 143 return true;// retorna quando for branco
bispomat 1:444af82e35cb 144 }
bispomat 1:444af82e35cb 145
bispomat 0:b803244146f7 146 void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
bispomat 0:b803244146f7 147
bispomat 1:444af82e35cb 148 if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint
rperoba 2:28f4f676e446 149 else if (sensorCheck(3)==1) {Position = 40;}
rperoba 2:28f4f676e446 150 else if (sensorCheck(4)==1) {Position = 50;}
rperoba 2:28f4f676e446 151 else if (sensorCheck(2)==1) {Position = 30;}
rperoba 2:28f4f676e446 152 else if (sensorCheck(5)==1) {Position = 60;}
rperoba 2:28f4f676e446 153 else if (sensorCheck(1)==1) {Position = 20;}
rperoba 2:28f4f676e446 154 else if (sensorCheck(6)==1) {Position = 70;}
rperoba 2:28f4f676e446 155 else if (sensorCheck(0)==1) {Position = 10;}
rperoba 2:28f4f676e446 156 else if (sensorCheck(7) == 1) {Position = 80;}
rperoba 2:28f4f676e446 157
bispomat 1:444af82e35cb 158 //pc.printf("\n\rPosicao: %d", Position); //Print de teste
bispomat 0:b803244146f7 159 }
bispomat 0:b803244146f7 160
bispomat 0:b803244146f7 161 /*=================================================
bispomat 0:b803244146f7 162 --------------- CODIGO DOS MOTORES ----------------
bispomat 0:b803244146f7 163 =================================================*/
bispomat 0:b803244146f7 164
bispomat 0:b803244146f7 165 //------------------ FUNCOES ----------------------
bispomat 0:b803244146f7 166
bispomat 0:b803244146f7 167 void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); }
bispomat 0:b803244146f7 168
bispomat 0:b803244146f7 169 void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda
bispomat 0:b803244146f7 170
bispomat 0:b803244146f7 171 if (VelocidadeEsquerda <= 0){
bispomat 0:b803244146f7 172 VelocidadeEsquerda = abs(VelocidadeEsquerda);
bispomat 0:b803244146f7 173 dir = 0;
bispomat 0:b803244146f7 174 pwm.write(VelocidadeEsquerda);
bispomat 0:b803244146f7 175 }
bispomat 0:b803244146f7 176 else { dir = 1; pwm.write(VelocidadeEsquerda); }
bispomat 0:b803244146f7 177
bispomat 0:b803244146f7 178 if (VelocidadeDireita <= 0){
bispomat 0:b803244146f7 179 VelocidadeDireita = abs(VelocidadeDireita);
bispomat 0:b803244146f7 180 dir1 = 0;
bispomat 0:b803244146f7 181 pwm1.write(VelocidadeDireita);
bispomat 0:b803244146f7 182 }
bispomat 0:b803244146f7 183 else { dir1 = 1; pwm1.write(VelocidadeDireita); }
bispomat 0:b803244146f7 184 }
bispomat 0:b803244146f7 185
bispomat 0:b803244146f7 186 /*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado
bispomat 0:b803244146f7 187
bispomat 0:b803244146f7 188 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 189 if (Position == 0){
bispomat 0:b803244146f7 190 if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; }
bispomat 0:b803244146f7 191 else { Esquerda_Esperada = 80; Direita_Esperada = -50; }
bispomat 0:b803244146f7 192 }
bispomat 0:b803244146f7 193 else { LastSensor = Error; }
bispomat 0:b803244146f7 194 }
bispomat 0:b803244146f7 195 */
bispomat 0:b803244146f7 196 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 197 /*=================================================
bispomat 0:b803244146f7 198 --------------- PID DOS SENSORES -----------------
bispomat 0:b803244146f7 199 =================================================*/
bispomat 0:b803244146f7 200
bispomat 0:b803244146f7 201 float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
rperoba 3:3b0c36115348 202 Error = Position - SET_POINT;
bispomat 0:b803244146f7 203 //bt.printf("\n\rErro: %d", Error);
bispomat 0:b803244146f7 204
bispomat 0:b803244146f7 205 if (Error < 20 && Error > -20){
bispomat 0:b803244146f7 206 Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);
bispomat 0:b803244146f7 207 }
bispomat 0:b803244146f7 208 else{
bispomat 0:b803244146f7 209 Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);
bispomat 0:b803244146f7 210 }
bispomat 0:b803244146f7 211
bispomat 0:b803244146f7 212 //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 213 //Comando para printar e verificar os dados de saida via bluetooth
bispomat 0:b803244146f7 214
bispomat 0:b803244146f7 215 LastError = Error;
bispomat 0:b803244146f7 216
bispomat 0:b803244146f7 217 return Correcao_Sensor;
bispomat 0:b803244146f7 218 }
bispomat 0:b803244146f7 219
bispomat 0:b803244146f7 220 /*=================================================
bispomat 0:b803244146f7 221 ---------------- PID DOS MOTORES ------------------
bispomat 0:b803244146f7 222 =================================================*/
bispomat 0:b803244146f7 223
bispomat 0:b803244146f7 224 /*
bispomat 0:b803244146f7 225
bispomat 0:b803244146f7 226 void rightEncoderEvent (){
bispomat 0:b803244146f7 227 if (B.read() == 0){
bispomat 0:b803244146f7 228 Direita_Count++;
bispomat 0:b803244146f7 229 }
bispomat 0:b803244146f7 230
bispomat 0:b803244146f7 231 else{
bispomat 0:b803244146f7 232 Direita_Count--;
bispomat 0:b803244146f7 233 }
bispomat 0:b803244146f7 234 }
bispomat 0:b803244146f7 235
bispomat 0:b803244146f7 236 void leftEncoderEvent(){
bispomat 0:b803244146f7 237 if (C.read() == 0) { Esquerda_Count++; }
bispomat 0:b803244146f7 238 else { Esquerda_Count--; }
bispomat 0:b803244146f7 239 }
bispomat 0:b803244146f7 240
bispomat 0:b803244146f7 241 void Speed () {
bispomat 0:b803244146f7 242
bispomat 0:b803244146f7 243 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 244 Esquerda_LastCount = Esquerda_Count;
bispomat 0:b803244146f7 245
bispomat 0:b803244146f7 246 Direita_realSpeed = (Direita_Count - Direita_LastCount);
bispomat 0:b803244146f7 247 Direita_LastCount = Direita_Count;
bispomat 0:b803244146f7 248 //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed);
bispomat 0:b803244146f7 249
bispomat 0:b803244146f7 250 }
bispomat 0:b803244146f7 251
bispomat 0:b803244146f7 252 int Converte_Velocidade (int realSpeed){
bispomat 0:b803244146f7 253 int realPWM;
bispomat 0:b803244146f7 254
bispomat 0:b803244146f7 255 realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder;
bispomat 0:b803244146f7 256
bispomat 0:b803244146f7 257 return realPWM;
bispomat 0:b803244146f7 258 }
bispomat 0:b803244146f7 259
bispomat 0:b803244146f7 260 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 261 int proporcional, derivativo, Correcao, Velocidade_Corrigida;
bispomat 0:b803244146f7 262
bispomat 0:b803244146f7 263 proporcional = Lado_realPWM - Lado_VelocidadeEsperada;
bispomat 0:b803244146f7 264 //bt.printf("\n\r%d",proporcional);
bispomat 0:b803244146f7 265 derivativo = proporcional - Lado_LastError;
bispomat 0:b803244146f7 266 //bt.printf("\n\rDerivativo: %d",derivativo);
bispomat 0:b803244146f7 267
bispomat 0:b803244146f7 268 Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda;
bispomat 0:b803244146f7 269 //bt.printf("\n\r%d",Correcao);
bispomat 0:b803244146f7 270
bispomat 0:b803244146f7 271 Lado_LastError = proporcional;
bispomat 0:b803244146f7 272 Integral_Lado_Roda += proporcional;
bispomat 0:b803244146f7 273
bispomat 0:b803244146f7 274 if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; }
bispomat 0:b803244146f7 275
bispomat 0:b803244146f7 276 if (proporcional == 0){ Lado_LastError = 0; }
bispomat 0:b803244146f7 277 else{
bispomat 0:b803244146f7 278 if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; }
bispomat 0:b803244146f7 279 else { Integral_Lado_Roda = MIN_INTEGRAL; }
bispomat 0:b803244146f7 280
bispomat 0:b803244146f7 281 }
bispomat 0:b803244146f7 282 Lado_lastExpectedSpeed = Lado_VelocidadeEsperada;
bispomat 0:b803244146f7 283
bispomat 0:b803244146f7 284 Velocidade_Corrigida = (Lado_realPWM - Correcao);
bispomat 0:b803244146f7 285
bispomat 0:b803244146f7 286 return Velocidade_Corrigida;
bispomat 0:b803244146f7 287 }
bispomat 0:b803244146f7 288
bispomat 0:b803244146f7 289 */
bispomat 0:b803244146f7 290
bispomat 0:b803244146f7 291 /*=================================================
bispomat 0:b803244146f7 292 --------------- FUNCAO PRINCIPAL -----------------
bispomat 0:b803244146f7 293 =================================================*/
bispomat 0:b803244146f7 294
bispomat 0:b803244146f7 295 int main(void){
bispomat 0:b803244146f7 296
bispomat 0:b803244146f7 297 //A.rise(&rightEncoderEvent);
bispomat 0:b803244146f7 298 //END.attach(&Speed, 1.0);
bispomat 0:b803244146f7 299 //C.rise(&leftEncoderEvent);
bispomat 0:b803244146f7 300
bispomat 0:b803244146f7 301 //END.attach(&Speed, 1.0);
bispomat 0:b803244146f7 302 //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA
bispomat 0:b803244146f7 303
bispomat 0:b803244146f7 304 Setup_Sensores(); //Faz o setup dos sensores
bispomat 0:b803244146f7 305 Setup_Motores(); //Faz o setup dos motores
bispomat 0:b803244146f7 306
bispomat 0:b803244146f7 307 //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); }
bispomat 0:b803244146f7 308
bispomat 0:b803244146f7 309 wait(2); //Delay para estabilizar
bispomat 0:b803244146f7 310
bispomat 0:b803244146f7 311 pwm.write(0.00); //Pulso inicial para cada motor
bispomat 0:b803244146f7 312 pwm1.write(0.00);
bispomat 0:b803244146f7 313
bispomat 1:444af82e35cb 314 //END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos
bispomat 0:b803244146f7 315
bispomat 0:b803244146f7 316 while(1){
bispomat 0:b803244146f7 317 //Para_Demonio();
bispomat 0:b803244146f7 318 Leitura_Sensores(); //Lê os sensores
bispomat 0:b803244146f7 319
bispomat 0:b803244146f7 320 Correction = PID_Sensores(Position); //Calcula a correção
bispomat 0:b803244146f7 321
rperoba 2:28f4f676e446 322 Esquerda_Esperada = -1*(BaseSpeed + (Correction * 0.01)); //Aplica a correção
bispomat 0:b803244146f7 323 Direita_Esperada = BaseSpeed - (Correction * 0.01);
bispomat 0:b803244146f7 324
rperoba 3:3b0c36115348 325 if (sensorTurbo.read() == 0 && sensorCheck(3)){
rperoba 3:3b0c36115348 326 Esquerda_Esperada += turbo;
rperoba 3:3b0c36115348 327 Direita_Esperada += turbo;
rperoba 3:3b0c36115348 328 }
rperoba 3:3b0c36115348 329
bispomat 0:b803244146f7 330 //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);
bispomat 0:b803244146f7 331 //Print das velocidades que estão chegando nas rodas após o PID
bispomat 0:b803244146f7 332
bispomat 0:b803244146f7 333 //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido
bispomat 0:b803244146f7 334
bispomat 0:b803244146f7 335 Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive
bispomat 0:b803244146f7 336
bispomat 0:b803244146f7 337 }
bispomat 0:b803244146f7 338 }