Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
main.cpp
- Committer:
- rperoba
- Date:
- 2020-02-27
- Revision:
- 10:d546eec37792
- Parent:
- 9:fccecc817082
- Child:
- 11:4f3dcdd4d8ce
File content as of revision 10:d546eec37792:
//========================= BIBLIOTECAS =========================// #include "mbed.h" //========================= DEFINICOES =========================// #define threshold 160 // parametro que diferencia a leitura do branco pro preto PwmOut pwm(p25); DigitalOut dir (p26); PwmOut pwm1(p24); DigitalOut dir1 (p23); //DigitalIn button(p18); //DigitalOut led(p20); Serial bt(p28, p27); Serial pc(USBTX, USBRX); //InterruptIn A (p21); //DigitalIn B (p22); //InterruptIn C (p29); //DigitalIn D (p30); //Ticker END; //Ticker END1; DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha DigitalOut multiplexador2(p6); DigitalOut multiplexador3(p8); // PRECISA AJUSTAR OS PINOS DigitalOut multiplexador4(p7); //DigitalOut multiplexador5(p9);// Enable do multiplexador DigitalInOut multiplexadorInOut(p12); // In/Out do multiplexador 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 int tabelaVerdade2[16] = {0,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1}; int tabelaVerdade3[16] = {0,0,0,0,1,1,1,1,0,0,0,0,1,1,1,1}; int tabelaVerdade4[16] = {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1}; Timer tempoDoSensor; // timer para leitura do sensor de linha //#define Kp_Roda 1 //#define Kd_Roda 1 //#define Ki_Roda 1 //============== VARIAVEIS MAIS IMPORTANTES ================// //Separadas do resto para facilitar a alteração float ParametroCorrecao = 1.2; //1.1 // 4.63 // 2 ----> 1.2 float Kp_Sensor = 0.55; //0.14; //0.2 //0.4 // 0.2 // 0.35--->0.55 float Kd_Sensor = 7.2; //1.75; //1.15 //0.6 // 2.7 // 4.3 --->7.2 float BaseSpeed = 0.18; //0.12 // 0.15 // 0.18--->0.18 //==========================================================// //#define VMAX_PWM 100 //#define VMAX_Encoder 100 //#define TESTE_MOTOR //#define TESTE_SENSOR Timeout END; //========================= VARIAVEIS GLOBAIS ========================= int Position; int Error=0; int Correction; int TotalError = 0; int LastSensor = 0; int SET_POINT = 45; //float Turbo = BaseSpeed; //short LimiteFim; //short ContaFim = 0; //bool LastRead = false; //bool Preto[15]; //int Threshold = 600; int LastError = 0; //int Integral_Esquerda = 0, Integral_Direita = 0; int MAX_INTEGRAL = 50; int MIN_INTEGRAL = -50; float Correcao_Sensor; //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1; //int Esquerda_LastError = 0, Direita_LastError = 0; float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida; float Direita_Esperada, Direita_Real, Direita_Corrigida; //int Esquerda_realSpeed, Direita_realSpeed; //int Direita_Count = 0, Direita_LastCount = 0; //int Esquerda_Count = 0, Esquerda_LastCount = 0; //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed; void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso /*================================================= -------------- CODIGO DOS SENSORES ---------------- =================================================*/ //------------------ FUNCAO ----------------------- void Setup_Sensores (void){ //Define o modo de cada sensor multiplexadorInOut.mode(PullNone); } int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 // função para ler os motores,retorna true se for branco e false se for preto multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor multiplexador2 = tabelaVerdade2[sensor]; multiplexador3 = tabelaVerdade3[sensor]; multiplexador4 = tabelaVerdade4[sensor]; //pc.printf("Status do Sensor entrando no check: %d ", multiplexadorInOut.read()); multiplexadorInOut.output(); // colocando o InOut como Out multiplexadorInOut = 1; // levando ele a high por pelo menos 10 microssegundos tempoDoSensor.start(); // iniciando o contador wait_us(10); // tempo para o output ir para high multiplexadorInOut.input(); // colocando o InOut como In //pc.printf("Status do Sensor supostamente em HIGH: %d ", multiplexadorInOut.read()); while(multiplexadorInOut == 1){// enrolando enquanto o sensor esta lendo //pc.printf("To no loop ->"); //pc.printf("Status do Sensor: %d ", multiplexadorInOut.read()); if (tempoDoSensor.read_us() >= threshold){// termina o while se o tempo for maior que o threshold,agilizando o processo de leitura tempoDoSensor.stop(); //pc.printf("To no if ->"); float tempoDeLeitura = tempoDoSensor.read_us(); //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug tempoDoSensor.reset(); multiplexadorInOut.output(); multiplexadorInOut = 0; return 0; // retorna quando for preto } } tempoDoSensor.stop(); float tempoDeLeitura = tempoDoSensor.read_us(); //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug tempoDoSensor.reset(); multiplexadorInOut.output(); multiplexadorInOut = 0; return 1;// retorna quando for branco } void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição int leitura[8]={0,0,0,0,1,0,0,0}; // leitura representa os sensores lidos,onde 1 é branco e 0 é preto for (int i=0;i<8;i++){leitura[i] = sensorCheck(i);} // leitura dos sensores //analizando a leitura e setando a posição if (leitura[3]== 1 && leitura[4] == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint else if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;} else if (leitura[1]== 1 && leitura[6] == 1) {Position = 45;} else if (leitura[3]==1) {Position = 40;} else if (leitura[2]==1) {Position = 30;} else if (leitura[5]==1) {Position = 60;} else if (leitura[1]==1) {Position = 20;} else if (leitura[6]==1) {Position = 70;} else if (leitura[0]==1) {Position = 10;} else if (leitura[7]==1) {Position = 80;} else if (leitura[4]==1) {Position = 50;} //else {Final_Stop();} //pc.printf("\n\rPosicao: %d", Position); //Print de teste } /*================================================= --------------- CODIGO DOS MOTORES ---------------- =================================================*/ //------------------ FUNCOES ---------------------- void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); } void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda if (VelocidadeEsquerda <= 0){ VelocidadeEsquerda = abs(VelocidadeEsquerda); dir = 0; pwm.write(VelocidadeEsquerda); } else { dir = 1; pwm.write(VelocidadeEsquerda); } if (VelocidadeDireita <= 0){ VelocidadeDireita = abs(VelocidadeDireita); dir1 = 0; pwm1.write(VelocidadeDireita); } else { dir1 = 1; pwm1.write(VelocidadeDireita); } } /*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado 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) if (Position == 0){ if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; } else { Esquerda_Esperada = 80; Direita_Esperada = -50; } } else { LastSensor = Error; } } */ /*================================================= --------------- PID DOS SENSORES ----------------- =================================================*/ float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção Error = Position - SET_POINT; //bt.printf("\n\rErro: %d", Error); if (Error < 25 && Error > -25){ Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError); } else{ Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError); } //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))); //Comando para printar e verificar os dados de saida via bluetooth LastError = Error; return Correcao_Sensor; } /*================================================= ---------------- PID DOS MOTORES ------------------ =================================================*/ /* void rightEncoderEvent (){ if (B.read() == 0){ Direita_Count++; } else{ Direita_Count--; } } void leftEncoderEvent(){ if (C.read() == 0) { Esquerda_Count++; } else { Esquerda_Count--; } } void Speed () { 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. Esquerda_LastCount = Esquerda_Count; Direita_realSpeed = (Direita_Count - Direita_LastCount); Direita_LastCount = Direita_Count; //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed); } int Converte_Velocidade (int realSpeed){ int realPWM; realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder; return realPWM; } int PID_Roda (int Lado_realPWM, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){ int proporcional, derivativo, Correcao, Velocidade_Corrigida; proporcional = Lado_realPWM - Lado_VelocidadeEsperada; //bt.printf("\n\r%d",proporcional); derivativo = proporcional - Lado_LastError; //bt.printf("\n\rDerivativo: %d",derivativo); Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda; //bt.printf("\n\r%d",Correcao); Lado_LastError = proporcional; Integral_Lado_Roda += proporcional; if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; } if (proporcional == 0){ Lado_LastError = 0; } else{ if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; } else { Integral_Lado_Roda = MIN_INTEGRAL; } } Lado_lastExpectedSpeed = Lado_VelocidadeEsperada; Velocidade_Corrigida = (Lado_realPWM - Correcao); return Velocidade_Corrigida; } */ /*================================================= --------------- FUNCAO PRINCIPAL ----------------- =================================================*/ int main(void){ //A.rise(&rightEncoderEvent); //END.attach(&Speed, 1.0); //C.rise(&leftEncoderEvent); //END.attach(&Speed, 1.0); //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA Setup_Sensores(); //Faz o setup dos sensores Setup_Motores(); //Faz o setup dos motores //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); } wait(2); //Delay para estabilizar pwm.write(0.00); //Pulso inicial para cada motor pwm1.write(0.00); //END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos while(1){ //Para_Demonio(); Leitura_Sensores(); //Lê os sensores Correction = PID_Sensores(Position); //Calcula a correção Esquerda_Esperada = -1*(BaseSpeed + (Correction * 0.01)); //Aplica a correção Direita_Esperada = BaseSpeed - (Correction * 0.01); //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada); //Print das velocidades que estão chegando nas rodas após o PID //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive } }