RioBotz / Mbed 2 deprecated LineFollowerV3_Iron_01-03-2020

Codigo comentado da IronCup 01/03/2020

Dependencies:   mbed

main.cpp@0:b803244146f7, 2019-10-16 (annotated)

Committer:
bispomat
Date:
Wed Oct 16 22:35:17 2019 +0000
Revision:
0:b803244146f7
Child:
1:444af82e35cb
.

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
bispomat 0:b803244146f7 7 PwmOut pwm(p25);
bispomat 0:b803244146f7 8 DigitalOut dir (p26);
bispomat 0:b803244146f7 9 PwmOut pwm1(p24);
bispomat 0:b803244146f7 10 DigitalOut dir1 (p23);
bispomat 0:b803244146f7 11 //DigitalIn button(p18);
bispomat 0:b803244146f7 12 //DigitalOut led(p20);
bispomat 0:b803244146f7 13
bispomat 0:b803244146f7 14 Serial bt(p28, p27);
bispomat 0:b803244146f7 15
bispomat 0:b803244146f7 16 //InterruptIn A (p21);
bispomat 0:b803244146f7 17 //DigitalIn B (p22);
bispomat 0:b803244146f7 18 //InterruptIn C (p29);
bispomat 0:b803244146f7 19 //DigitalIn D (p30);
bispomat 0:b803244146f7 20
bispomat 0:b803244146f7 21 //Ticker END;
bispomat 0:b803244146f7 22 //Ticker END1;
bispomat 0:b803244146f7 23
bispomat 0:b803244146f7 24 DigitalIn s1(p5);
bispomat 0:b803244146f7 25 DigitalIn s2(p6);
bispomat 0:b803244146f7 26 DigitalIn s3(p7);
bispomat 0:b803244146f7 27 DigitalIn s4(p8);
bispomat 0:b803244146f7 28 DigitalIn s5(p9);
bispomat 0:b803244146f7 29 DigitalIn s6(p10);
bispomat 0:b803244146f7 30 DigitalIn s7(p12);
bispomat 0:b803244146f7 31 DigitalIn s8(p13);
bispomat 0:b803244146f7 32
bispomat 0:b803244146f7 33 //#define Kp_Roda 1
bispomat 0:b803244146f7 34 //#define Kd_Roda 1
bispomat 0:b803244146f7 35 //#define Ki_Roda 1
bispomat 0:b803244146f7 36
bispomat 0:b803244146f7 37 //============== VARIAVEIS MAIS IMPORTANTES ================//
bispomat 0:b803244146f7 38 //Separadas do resto para facilitar a alteração
bispomat 0:b803244146f7 39
bispomat 0:b803244146f7 40 float ParametroCorrecao = 5.3;
bispomat 0:b803244146f7 41 float Kp_Sensor = 0.145; //0.14; //0.2
bispomat 0:b803244146f7 42 float Kd_Sensor = 1.64; //1.75; //1.15
bispomat 0:b803244146f7 43 float BaseSpeed = 0.17;
bispomat 0:b803244146f7 44
bispomat 0:b803244146f7 45 //==========================================================//
bispomat 0:b803244146f7 46 //#define VMAX_PWM 100
bispomat 0:b803244146f7 47 //#define VMAX_Encoder 100
bispomat 0:b803244146f7 48
bispomat 0:b803244146f7 49 //#define TESTE_MOTOR
bispomat 0:b803244146f7 50 //#define TESTE_SENSOR
bispomat 0:b803244146f7 51
bispomat 0:b803244146f7 52 Timeout END;
bispomat 0:b803244146f7 53 //========================= VARIAVEIS GLOBAIS =========================
bispomat 0:b803244146f7 54
bispomat 0:b803244146f7 55 int Position;
bispomat 0:b803244146f7 56 int Error=0;
bispomat 0:b803244146f7 57 int Correction;
bispomat 0:b803244146f7 58 int TotalError = 0;
bispomat 0:b803244146f7 59 int LastSensor = 0;
bispomat 0:b803244146f7 60 int SET_POINT = 45;
bispomat 0:b803244146f7 61
bispomat 0:b803244146f7 62 //float Turbo = BaseSpeed;
bispomat 0:b803244146f7 63
bispomat 0:b803244146f7 64 //short LimiteFim;
bispomat 0:b803244146f7 65 //short ContaFim = 0;
bispomat 0:b803244146f7 66 //bool LastRead = false;
bispomat 0:b803244146f7 67 //bool Preto[15];
bispomat 0:b803244146f7 68 //int Threshold = 600;
bispomat 0:b803244146f7 69
bispomat 0:b803244146f7 70 int LastError = 0;
bispomat 0:b803244146f7 71 //int Integral_Esquerda = 0, Integral_Direita = 0;
bispomat 0:b803244146f7 72 int MAX_INTEGRAL = 50;
bispomat 0:b803244146f7 73 int MIN_INTEGRAL = -50;
bispomat 0:b803244146f7 74 float Correcao_Sensor;
bispomat 0:b803244146f7 75
bispomat 0:b803244146f7 76 //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1;
bispomat 0:b803244146f7 77 //int Esquerda_LastError = 0, Direita_LastError = 0;
bispomat 0:b803244146f7 78 float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida;
bispomat 0:b803244146f7 79 float Direita_Esperada, Direita_Real, Direita_Corrigida;
bispomat 0:b803244146f7 80 //int Esquerda_realSpeed, Direita_realSpeed;
bispomat 0:b803244146f7 81 //int Direita_Count = 0, Direita_LastCount = 0;
bispomat 0:b803244146f7 82 //int Esquerda_Count = 0, Esquerda_LastCount = 0;
bispomat 0:b803244146f7 83 //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed;
bispomat 0:b803244146f7 84
bispomat 0:b803244146f7 85 /*=================================================
bispomat 0:b803244146f7 86 -------------- CODIGO DOS SENSORES ----------------
bispomat 0:b803244146f7 87 =================================================*/
bispomat 0:b803244146f7 88
bispomat 0:b803244146f7 89 //------------------ FUNCAO -----------------------
bispomat 0:b803244146f7 90
bispomat 0:b803244146f7 91 void Setup_Sensores (void){ //Define o modo de cada sensor como PullUp
bispomat 0:b803244146f7 92 s1.mode(PullUp);
bispomat 0:b803244146f7 93 s2.mode(PullUp);
bispomat 0:b803244146f7 94 s3.mode(PullUp);
bispomat 0:b803244146f7 95 s4.mode(PullUp);
bispomat 0:b803244146f7 96 s5.mode(PullUp);
bispomat 0:b803244146f7 97 s6.mode(PullUp);
bispomat 0:b803244146f7 98 s7.mode(PullUp);
bispomat 0:b803244146f7 99 s8.mode(PullUp);
bispomat 0:b803244146f7 100 }
bispomat 0:b803244146f7 101
bispomat 0:b803244146f7 102 void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
bispomat 0:b803244146f7 103
bispomat 0:b803244146f7 104 if (s4.read()==0 && s5.read()==0) {Position = 45;} //Se ler os dois do meio, está no SetPoint
bispomat 0:b803244146f7 105 else if (s7.read()==0) {Position = 70;} //Ele só vai ler os sensores das extremidades
bispomat 0:b803244146f7 106 else if (s8.read()==0) {Position = 80;} //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos)
bispomat 0:b803244146f7 107 if (s1.read()==0) {Position = 10;}
bispomat 0:b803244146f7 108 if (s2.read()==0) {Position = 20;}
bispomat 0:b803244146f7 109 if (s3.read()==0) {Position = 30;}
bispomat 0:b803244146f7 110 if (s4.read()==0) {Position = 40;}
bispomat 0:b803244146f7 111 if (s5.read()==0) {Position = 50;}
bispomat 0:b803244146f7 112 if (s6.read()==0) {Position = 60;}
bispomat 0:b803244146f7 113
bispomat 0:b803244146f7 114 //bt.printf("\n\rPosicao: %d", Position); //Print de teste
bispomat 0:b803244146f7 115 }
bispomat 0:b803244146f7 116
bispomat 0:b803244146f7 117 /*=================================================
bispomat 0:b803244146f7 118 --------------- CODIGO DOS MOTORES ----------------
bispomat 0:b803244146f7 119 =================================================*/
bispomat 0:b803244146f7 120
bispomat 0:b803244146f7 121 //------------------ FUNCOES ----------------------
bispomat 0:b803244146f7 122
bispomat 0:b803244146f7 123 void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); }
bispomat 0:b803244146f7 124
bispomat 0:b803244146f7 125 void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda
bispomat 0:b803244146f7 126
bispomat 0:b803244146f7 127 if (VelocidadeEsquerda <= 0){
bispomat 0:b803244146f7 128 VelocidadeEsquerda = abs(VelocidadeEsquerda);
bispomat 0:b803244146f7 129 dir = 0;
bispomat 0:b803244146f7 130 pwm.write(VelocidadeEsquerda);
bispomat 0:b803244146f7 131 }
bispomat 0:b803244146f7 132 else { dir = 1; pwm.write(VelocidadeEsquerda); }
bispomat 0:b803244146f7 133
bispomat 0:b803244146f7 134 if (VelocidadeDireita <= 0){
bispomat 0:b803244146f7 135 VelocidadeDireita = abs(VelocidadeDireita);
bispomat 0:b803244146f7 136 dir1 = 0;
bispomat 0:b803244146f7 137 pwm1.write(VelocidadeDireita);
bispomat 0:b803244146f7 138 }
bispomat 0:b803244146f7 139 else { dir1 = 1; pwm1.write(VelocidadeDireita); }
bispomat 0:b803244146f7 140 }
bispomat 0:b803244146f7 141
bispomat 0:b803244146f7 142 /*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado
bispomat 0:b803244146f7 143
bispomat 0:b803244146f7 144 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 145 if (Position == 0){
bispomat 0:b803244146f7 146 if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; }
bispomat 0:b803244146f7 147 else { Esquerda_Esperada = 80; Direita_Esperada = -50; }
bispomat 0:b803244146f7 148 }
bispomat 0:b803244146f7 149 else { LastSensor = Error; }
bispomat 0:b803244146f7 150 }
bispomat 0:b803244146f7 151 */
bispomat 0:b803244146f7 152 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 153 /*=================================================
bispomat 0:b803244146f7 154 --------------- PID DOS SENSORES -----------------
bispomat 0:b803244146f7 155 =================================================*/
bispomat 0:b803244146f7 156
bispomat 0:b803244146f7 157 float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
bispomat 0:b803244146f7 158 Error = Position - SET_POINT;
bispomat 0:b803244146f7 159 //bt.printf("\n\rErro: %d", Error);
bispomat 0:b803244146f7 160
bispomat 0:b803244146f7 161 if (Error < 20 && Error > -20){
bispomat 0:b803244146f7 162 Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);
bispomat 0:b803244146f7 163 }
bispomat 0:b803244146f7 164 else{
bispomat 0:b803244146f7 165 Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);
bispomat 0:b803244146f7 166 }
bispomat 0:b803244146f7 167
bispomat 0:b803244146f7 168 //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 169 //Comando para printar e verificar os dados de saida via bluetooth
bispomat 0:b803244146f7 170
bispomat 0:b803244146f7 171 LastError = Error;
bispomat 0:b803244146f7 172
bispomat 0:b803244146f7 173 return Correcao_Sensor;
bispomat 0:b803244146f7 174 }
bispomat 0:b803244146f7 175
bispomat 0:b803244146f7 176 /*=================================================
bispomat 0:b803244146f7 177 ---------------- PID DOS MOTORES ------------------
bispomat 0:b803244146f7 178 =================================================*/
bispomat 0:b803244146f7 179
bispomat 0:b803244146f7 180 /*
bispomat 0:b803244146f7 181
bispomat 0:b803244146f7 182 void rightEncoderEvent (){
bispomat 0:b803244146f7 183 if (B.read() == 0){
bispomat 0:b803244146f7 184 Direita_Count++;
bispomat 0:b803244146f7 185 }
bispomat 0:b803244146f7 186
bispomat 0:b803244146f7 187 else{
bispomat 0:b803244146f7 188 Direita_Count--;
bispomat 0:b803244146f7 189 }
bispomat 0:b803244146f7 190 }
bispomat 0:b803244146f7 191
bispomat 0:b803244146f7 192 void leftEncoderEvent(){
bispomat 0:b803244146f7 193 if (C.read() == 0) { Esquerda_Count++; }
bispomat 0:b803244146f7 194 else { Esquerda_Count--; }
bispomat 0:b803244146f7 195 }
bispomat 0:b803244146f7 196
bispomat 0:b803244146f7 197 void Speed () {
bispomat 0:b803244146f7 198
bispomat 0:b803244146f7 199 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 200 Esquerda_LastCount = Esquerda_Count;
bispomat 0:b803244146f7 201
bispomat 0:b803244146f7 202 Direita_realSpeed = (Direita_Count - Direita_LastCount);
bispomat 0:b803244146f7 203 Direita_LastCount = Direita_Count;
bispomat 0:b803244146f7 204 //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed);
bispomat 0:b803244146f7 205
bispomat 0:b803244146f7 206 }
bispomat 0:b803244146f7 207
bispomat 0:b803244146f7 208 int Converte_Velocidade (int realSpeed){
bispomat 0:b803244146f7 209 int realPWM;
bispomat 0:b803244146f7 210
bispomat 0:b803244146f7 211 realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder;
bispomat 0:b803244146f7 212
bispomat 0:b803244146f7 213 return realPWM;
bispomat 0:b803244146f7 214 }
bispomat 0:b803244146f7 215
bispomat 0:b803244146f7 216 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 217 int proporcional, derivativo, Correcao, Velocidade_Corrigida;
bispomat 0:b803244146f7 218
bispomat 0:b803244146f7 219 proporcional = Lado_realPWM - Lado_VelocidadeEsperada;
bispomat 0:b803244146f7 220 //bt.printf("\n\r%d",proporcional);
bispomat 0:b803244146f7 221 derivativo = proporcional - Lado_LastError;
bispomat 0:b803244146f7 222 //bt.printf("\n\rDerivativo: %d",derivativo);
bispomat 0:b803244146f7 223
bispomat 0:b803244146f7 224 Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda;
bispomat 0:b803244146f7 225 //bt.printf("\n\r%d",Correcao);
bispomat 0:b803244146f7 226
bispomat 0:b803244146f7 227 Lado_LastError = proporcional;
bispomat 0:b803244146f7 228 Integral_Lado_Roda += proporcional;
bispomat 0:b803244146f7 229
bispomat 0:b803244146f7 230 if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; }
bispomat 0:b803244146f7 231
bispomat 0:b803244146f7 232 if (proporcional == 0){ Lado_LastError = 0; }
bispomat 0:b803244146f7 233 else{
bispomat 0:b803244146f7 234 if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; }
bispomat 0:b803244146f7 235 else { Integral_Lado_Roda = MIN_INTEGRAL; }
bispomat 0:b803244146f7 236
bispomat 0:b803244146f7 237 }
bispomat 0:b803244146f7 238 Lado_lastExpectedSpeed = Lado_VelocidadeEsperada;
bispomat 0:b803244146f7 239
bispomat 0:b803244146f7 240 Velocidade_Corrigida = (Lado_realPWM - Correcao);
bispomat 0:b803244146f7 241
bispomat 0:b803244146f7 242 return Velocidade_Corrigida;
bispomat 0:b803244146f7 243 }
bispomat 0:b803244146f7 244
bispomat 0:b803244146f7 245 */
bispomat 0:b803244146f7 246
bispomat 0:b803244146f7 247 /*=================================================
bispomat 0:b803244146f7 248 --------------- FUNCAO PRINCIPAL -----------------
bispomat 0:b803244146f7 249 =================================================*/
bispomat 0:b803244146f7 250
bispomat 0:b803244146f7 251 int main(void){
bispomat 0:b803244146f7 252
bispomat 0:b803244146f7 253 //A.rise(&rightEncoderEvent);
bispomat 0:b803244146f7 254 //END.attach(&Speed, 1.0);
bispomat 0:b803244146f7 255 //C.rise(&leftEncoderEvent);
bispomat 0:b803244146f7 256
bispomat 0:b803244146f7 257 //END.attach(&Speed, 1.0);
bispomat 0:b803244146f7 258 //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA
bispomat 0:b803244146f7 259
bispomat 0:b803244146f7 260 Setup_Sensores(); //Faz o setup dos sensores
bispomat 0:b803244146f7 261 Setup_Motores(); //Faz o setup dos motores
bispomat 0:b803244146f7 262
bispomat 0:b803244146f7 263 //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); }
bispomat 0:b803244146f7 264
bispomat 0:b803244146f7 265 wait(2); //Delay para estabilizar
bispomat 0:b803244146f7 266
bispomat 0:b803244146f7 267 pwm.write(0.00); //Pulso inicial para cada motor
bispomat 0:b803244146f7 268 pwm1.write(0.00);
bispomat 0:b803244146f7 269
bispomat 0:b803244146f7 270 END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos
bispomat 0:b803244146f7 271
bispomat 0:b803244146f7 272 while(1){
bispomat 0:b803244146f7 273 //Para_Demonio();
bispomat 0:b803244146f7 274 Leitura_Sensores(); //Lê os sensores
bispomat 0:b803244146f7 275
bispomat 0:b803244146f7 276 Correction = PID_Sensores(Position); //Calcula a correção
bispomat 0:b803244146f7 277
bispomat 0:b803244146f7 278 Esquerda_Esperada = BaseSpeed + (Correction * 0.01); //Aplica a correção
bispomat 0:b803244146f7 279 Direita_Esperada = BaseSpeed - (Correction * 0.01);
bispomat 0:b803244146f7 280
bispomat 0:b803244146f7 281 //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);
bispomat 0:b803244146f7 282 //Print das velocidades que estão chegando nas rodas após o PID
bispomat 0:b803244146f7 283
bispomat 0:b803244146f7 284 //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido
bispomat 0:b803244146f7 285
bispomat 0:b803244146f7 286 Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive
bispomat 0:b803244146f7 287
bispomat 0:b803244146f7 288 }
bispomat 0:b803244146f7 289 }