Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
Revision 15:b2952e78faa2, committed 2020-03-25
- Comitter:
- lucalm
- Date:
- Wed Mar 25 21:38:47 2020 +0000
- Parent:
- 14:6acac859ceb0
- Commit message:
- Codigo comentado Ironcup 01/03/2020
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Mar 10 19:39:15 2020 +0000 +++ b/main.cpp Wed Mar 25 21:38:47 2020 +0000 @@ -6,15 +6,15 @@ #define threshold 250 // parametro que diferencia a leitura do branco pro preto // 250 na ultima tomada de tempo -PwmOut pwm(p25); // Potencias da roda e direções -DigitalOut dir (p26); -PwmOut pwm1(p24); -DigitalOut dir1 (p23); +rodaEsquerdaOut rodaEsquerda(p25); // Potencias da roda e direções +DigitalOut direcaoEsquerda (p26); +rodaEsquerdaOut rodaDireita(p24); +DigitalOut direcaoDireita (p23); //DigitalIn button(p18); //DigitalOut led(p20); -Serial bt(p28, p27); -Serial pc(USBTX, USBRX); // serial pra debugar pelo pc +Serial bt(p28, p27);// Serial para conexao de bluetooth +Serial pc(USBTX, USBRX); // Serial para conexao pelo pc //InterruptIn A (p21); //DigitalIn B (p22); @@ -26,14 +26,14 @@ DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha DigitalOut multiplexador2(p6); -DigitalOut multiplexador3(p8); // PRECISA AJUSTAR OS PINOS +DigitalOut multiplexador3(p8); 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 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 +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. 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 @@ -45,30 +45,30 @@ //============== VARIAVEIS MAIS IMPORTANTES ================// //Separadas do resto para facilitar a alteração -float ParametroCorrecao = 1.15; //1.1 // 4.63 --1 // 2 ----> 1.15----1.02 //1.02 //1.15 -float Kp_Sensor = 0.675; //0.14; //0.2 //0.4 // 0.2 --0.7 // 0.35--->0.55--0.7--0.64--0.92 //0.65 //0.675 -float Kd_Sensor = 24.55; //1.75; //1.15 //0.6 // 2.7 --15 // 4.3 --->7.1--8.4--8.8--12.1 //24.45 //24.55 -float BaseSpeed = 0.18; //0.12 // 0.15 --0.15 // 0.18--->0.18 //0.18 //0.18 -//float Ki_Sensor = -0.0000005;//testando //8.02V na bateria //7.93Volts +float ParametroCorrecao = 1.15; //1.02 //1.15 +float Kp_Sensor = 0.675; //0.65 //0.675 +float Kd_Sensor = 24.55; //24.45 //24.55 +float BaseSpeed = 0.18; //0.18 //0.18 + //8.02V na bateria //7.93Volts +//float Ki_Sensor = -0.0000005;//testando KI // //==========================================================// -//#define VMAX_PWM 100 +//#define VMAX_rodaEsquerda 100 //#define VMAX_Encoder 100 //#define TESTE_MOTOR //#define TESTE_SENSOR -Timeout END; +Timeout END;//Variavel END que e utilizada para interromper o programa do papacuras //========================= VARIAVEIS GLOBAIS ========================= -int Position; +int Position; //De acordo com a leitura dos sensores, sera determinada uma posicao int Error=0; -int Correction; +int Correction; //Variavel que armazena a correcao do PID int TotalError = 0; -int LastSensor = 0; -int SET_POINT = 45; +int SET_POINT = 45; //Centro dos sensores, nosso setpoint //int Integral=0; -int lastSensor = 4; +int lastSensor = 4; // Ultima sensor lido //float Turbo = BaseSpeed; //short LimiteFim; @@ -77,11 +77,11 @@ //bool Preto[15]; //int Threshold = 600; -int LastError = 0; +int LastError = 0;//Ultimo erro gerado pela leitura dos sensores, utilizado pelo KD. //int Integral_Esquerda = 0, Integral_Direita = 0; -int MAX_INTEGRAL = 25; -int MIN_INTEGRAL = -25; -float Correcao_Sensor; +//int MAX_INTEGRAL = 25; +//int MIN_INTEGRAL = -25; +float Correcao_Sensor;//Variavel que armazena o resultado do PID //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1; //int Esquerda_LastError = 0, Direita_LastError = 0; @@ -92,7 +92,7 @@ //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 +void Final_Stop(){ while(1) { direcaoEsquerda = 0; direcaoDireita = 0; rodaEsquerda.write(0); rodaDireita.write(0); } } //Função de parada do LF depois do percurso /*================================================= -------------- CODIGO DOS SENSORES ---------------- @@ -104,7 +104,7 @@ multiplexadorInOut.mode(PullNone); } -int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 +int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 "0 a 7 sao sensores frontais" // função para ler os motores,retorna true se for branco e false se for preto multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor @@ -122,14 +122,14 @@ 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 + while(multiplexadorInOut == 1){// esperando a leitura do sensor. //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(); + tempoDoSensor.stop(); //pc.printf("To no if ->"); - float tempoDeLeitura = tempoDoSensor.read_us(); + float tempoDeLeitura = tempoDoSensor.read_us();//Tempo de leitura sera igual a leitura o tempo de leitura dos sensores //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug tempoDoSensor.reset(); multiplexadorInOut.output(); @@ -139,7 +139,7 @@ } tempoDoSensor.stop(); - float tempoDeLeitura = tempoDoSensor.read_us(); + float tempoDeLeitura = tempoDoSensor.read_us();//Tempo de leitura sera igual a leitura o tempo de leitura dos sensores //pc.printf ("Tempo de leitura :%f \n\r ",tempoDeLeitura); // debug tempoDoSensor.reset(); multiplexadorInOut.output(); @@ -153,32 +153,15 @@ //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 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;} - if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;} - //else if (leitura[1]== 1 && leitura[6] == 1) {Position = 45;} - //else if (leitura[2]== 1 && leitura[3] == 1) {Position = 45;lastSensor = 3;} - //else if (leitura[2]== 1 && leitura[6] == 1) {Position = 45;} - //else if (leitura[3]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;} + if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;}// lastSensor=3 pois esta no meio, medida tomada para cruzamentos. else if (leitura[1]== 1 && leitura[5] == 1) {Position = 45;lastSensor = 3;} - else if (leitura[1]==1) {Position = 20;lastSensor = 3;} - //else if (leitura[6]==1) {Position = 70;} - else if (leitura[0]==1) {Position = 10;lastSensor = 4;} - else if (leitura[7]==1) {Position = 80;lastSensor = 4;} - else if (leitura[2]==1) {Position = 30;lastSensor = 3;} - else if (leitura[5]==1) {Position = 60;lastSensor = 4;} - // if (lastSensor==3){Position = 50;lastSensor = 4;} + else if (leitura[1]==1) {Position = 20;lastSensor = 1;} + else if (leitura[0]==1) {Position = 10;lastSensor = 0;} + else if (leitura[7]==1) {Position = 80;lastSensor = 7;} + else if (leitura[2]==1) {Position = 30;lastSensor = 2;} + else if (leitura[5]==1) {Position = 60;lastSensor = 5;} else if (leitura[3]==1) {Position = 40;lastSensor = 3;} - else if (leitura[6]==1) {Position = 70;lastSensor = 4;} - /* - 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 if (leitura[6]==1) {Position = 70;lastSensor = 6;} //else {Final_Stop();} //pc.printf("\n\rPosicao: %d", Position); //Print de teste } @@ -189,33 +172,33 @@ //------------------ FUNCOES ---------------------- -void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); } +void Setup_Motores(void){ rodaEsquerda.period(0.01); rodaDireita.period(0.01); } //Funcao de setup dos motores 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); + if (VelocidadeEsquerda <= 0){// + VelocidadeEsquerda = abs(VelocidadeEsquerda);//Utiliza o modulo da velocidade, pois esta nao pode ser negativa + direcaoEsquerda = 0;//Muda a direcao da roda. + rodaEsquerda.write(VelocidadeEsquerda); } - else { dir = 1; pwm.write(VelocidadeEsquerda); } + else { direcaoEsquerda = 1; rodaEsquerda.write(VelocidadeEsquerda); }//Mantem a direcao da roda. if (VelocidadeDireita <= 0){ VelocidadeDireita = abs(VelocidadeDireita); - dir1 = 0; - pwm1.write(VelocidadeDireita); + direcaoDireita = 0; + rodaDireita.write(VelocidadeDireita); } - else { dir1 = 1; pwm1.write(VelocidadeDireita); } + else { direcaoDireita = 1; rodaDireita.write(VelocidadeDireita); } } -/*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado +/*void Para_Demonio (void){ direcaoEsquerda = 0; direcaoDireita = 0; rodaEsquerda.write(0); rodaDireita.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; } + if (lastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; } else { Esquerda_Esperada = 80; Direita_Esperada = -50; } } - else { LastSensor = Error; } + else { lastSensor = Error; } } */ @@ -224,7 +207,7 @@ =================================================*/ float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção - Error = Position - SET_POINT; + Error = Position - SET_POINT;//Achando o erro do PID atraves da leitura dos sensores. // Integral=Integral + Error; // acumulador de erro da integral //bt.printf("\n\rErro: %d", Error); /* @@ -233,10 +216,10 @@ else if (Integral<MIN_INTEGRAL){ Integral=MIN_INTEGRAL;} */ - if (Error < 25 && Error > -25){ + if (Error < 25 && Error > -25){//Sem o parametro de correcao Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);// + Integral*Ki_Sensor ; } - else{ + else{//Com o parametro de correcao Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);// + Integral*Ki_Sensor; } @@ -281,17 +264,17 @@ } int Converte_Velocidade (int realSpeed){ - int realPWM; + int realrodaEsquerda; - realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder; + realrodaEsquerda = (VMAX_rodaEsquerda*realSpeed)/VMAX_Encoder; - return realPWM; + return realrodaEsquerda; } -int PID_Roda (int Lado_realPWM, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){ +int PID_Roda (int Lado_realrodaEsquerda, 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; + proporcional = Lado_realrodaEsquerda - Lado_VelocidadeEsperada; //bt.printf("\n\r%d",proporcional); derivativo = proporcional - Lado_LastError; //bt.printf("\n\rDerivativo: %d",derivativo); @@ -312,7 +295,7 @@ } Lado_lastExpectedSpeed = Lado_VelocidadeEsperada; - Velocidade_Corrigida = (Lado_realPWM - Correcao); + Velocidade_Corrigida = (Lado_realrodaEsquerda - Correcao); return Velocidade_Corrigida; } @@ -339,10 +322,10 @@ wait(2); //Delay para estabilizar - pwm.write(0.00); //Pulso inicial para cada motor - pwm1.write(0.00); + rodaEsquerda.write(0.00); //Pulso inicial para cada motor + rodaDireita.write(0.00); - END.attach(&Final_Stop, 24.5); //Faz o LF parar depois de 23 segundos + END.attach(&Final_Stop, 24.5); //Chama a funcao Final_Stop em x segundos while(1){ //Para_Demonio();