RioBotz / MotorsLF

Header dos motores do seguidor de linha

main.cpp@0:8c5a0e6a171b, 2020-09-17 (annotated)

Committer:
lucalm
Date:
Thu Sep 17 15:08:31 2020 +0000
Revision:
0:8c5a0e6a171b
Child:
2:a68b0082faac
Header dos motores

Who changed what in which revision?

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