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