![](/media/cache/group/Logo_RioBotz.png.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |