RioBotz
Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
Diff: main.cpp
- Revision:
- 15:b2952e78faa2
- Parent:
- 14:6acac859ceb0
--- 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();