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?
User | Revision | Line number | New 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 | } |