RioBotz / Mbed 2 deprecated LineFollowerV3_Iron_01-03-2020

Codigo comentado da IronCup 01/03/2020

Dependencies:   mbed

main.cpp@15:b2952e78faa2, 2020-03-25 (annotated)

Committer:
lucalm
Date:
Wed Mar 25 21:38:47 2020 +0000
Revision:
15:b2952e78faa2
Parent:
14:6acac859ceb0 
Codigo comentado Ironcup 01/03/2020

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