RioBotz / Mbed 2 deprecated LineFollowerV3_Iron_01-03-2020

Codigo comentado da IronCup 01/03/2020

Dependencies:   mbed

main.cpp@10:d546eec37792, 2020-02-27 (annotated)

Committer:
rperoba
Date:
Thu Feb 27 00:01:42 2020 +0000
Revision:
10:d546eec37792
Parent:
9:fccecc817082
Child:
11:4f3dcdd4d8ce
codigo pre iron;

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