Codigo comentado da IronCup 01/03/2020

Dependencies:   mbed

Committer:
rperoba
Date:
Tue Feb 25 21:25:23 2020 +0000
Revision:
5:41a59c155649
Parent:
4:247c0fc124db
com lastrear

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