RioBotz
Codigo do seguidor de linha V2 2020.2
Dependencies: mbed
Diff: main.cpp
- Revision:
- 6:5551834026ef
- Parent:
- 0:9efe13b5d868
--- a/main.cpp Sat Sep 19 21:31:41 2020 +0000
+++ b/main.cpp Sat Oct 10 19:08:11 2020 +0000
@@ -5,12 +5,24 @@
#include "motors.h"
#include "PID.h"
+#define TAMANHODOMAPA 50
+#define PERIMETRODARODA 26
+
InterruptIn Botao (p5);
DigitalOut Led (p6);
+InterruptIn OUTA_RIGHT(p26);
+DigitalIn OUTB_RIGHT(p11);
+
+InterruptIn OUTA_LEFT(p21);
+DigitalIn OUTB_LEFT(p7);
+
int botao = 0;
int comando = 1;
-
+int contadorAsaDireita = 0;
+int posicaoNoMapa = 0;
+float distanciaPraCurva;
+
float correcaoPD;
double erroSensores;
@@ -18,9 +30,32 @@
float velocidadeDireita;
float velocidadeRealEsquerda;
float velocidadeRealDireita;
-float velocidadeBase = 0.18;
+float velocidadeAtual = 0.18;
+float novaVelocidade;
+float correcaoEsquerda;
+float correcaoDireita;
+
+bool estadoAsaDireita = false;
+bool estadoAsaEsquerda = false;
+
+float mapaPraCorrer[TAMANHODOMAPA][4];
-Callback<void()> AlteraComando(){
+void leArquivo ()
+{
+ FILE *mapaArquivo = fopen("/local/mapaTeste.txt","r");
+ if(mapaArquivo == NULL){
+ printf("Impossivel criar o arquivo");
+ return;
+ }
+ for(int i = 0;i<TAMANHODOMAPA;i++)
+ {
+ fscanf(mapaArquivo,"%f-%f-%f-%f\n",&mapaPraCorrer[i][0],&mapaPraCorrer[i][1],&mapaPraCorrer[i][2],&mapaPraCorrer[i][3]); // talvez tenha q salvar numa variavel, igualar a um vetor
+ }
+
+ fclose(mapaArquivo);
+}
+
+void AlteraComando(){
botao++;
}
@@ -35,7 +70,11 @@
int main()
{
- Botao.rise(AlteraComando());
+ Botao.rise(&AlteraComando);
+ OUTA_RIGHT.rise(&contaPulso_direito);
+ OUTA_LEFT.rise(&contaPulso_esquerdo);
+ leArquivo();
+
Setup_Motores();
while(botao == 0){ //Espera o aperto do botao para iniciar a calibragem, o led permanecera acesso para indicar esta fase
wait_us(10);
@@ -50,31 +89,92 @@
Led = 0; //Certificando que o led esta apagado depois da calibragem
wait_us(5000); //Espera 5 segundos para poder mudar a posição de calibrar os sensores para a de correr
- //Tocar o buzzer parar avisar q ai começar a correr
- while(1){
+ //Tocar o buzzer parar avisar q ai começar a correr
switch(comando){
//Comando = 1 -> Faz a pista devagar e mapeando
//Comando = 2 -> Faz a pista usando a leitura do mapa
//Comando = 3 -> Faz a usando a leitura do mapa e ultrapassando os limites de velocidade
case 1:
+ while(1){
+ velocidadeAtual = VelocidadeBateria(velocidadeAtual); //Ajuste da velocidade por causa da descarga da bateria
LerSensoresFrontais(); //Leitura dos sensores frontais
erroSensores = CalculaErro(); //Calcula o arco do erro dos sensores
correcaoPD = PID_Sensores(erroSensores); //Calcula a correção do PD angular
- velocidadeEsquerda = velocidadeBase + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
- velocidadeDireita = velocidadeBase - correcaoPD;
- //velocidadeRealDireita = ; //Pega a velocidade do seguidor lida pelos encoderes
- //velocidadeRealEsquerda = ;
- velocidadeEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
- velocidadeDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+ velocidadeEsquerda = velocidadeAtual + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
+ velocidadeDireita = velocidadeAtual - correcaoPD;
+ velocidadeRealDireita = retornaVelDireita(); //velocidade em /m/s
+ velocidadeRealEsquerda = retornaVelEsquerda();
+ velocidadeEsquerda = transformaPWM(velocidadeEsquerda); //Converte velocidade PWM em m/s
+ velocidadeDireita = transformaPWM(velocidadeDireita);
+ correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
+ correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+ velocidadeEsquerda += correcaoEsquerda;
+ velocidadeDireita += correcaoDireita;
ControleMotores(velocidadeEsquerda,velocidadeDireita); //Muda a velocidade das rodas para as velocidades calculadas anteriormente
- //Mapear a pista
+ estadoAsaDireita = LerAsaDireita();
+ estadoAsaEsquerda = LerAsaEsquerda();
+ if(estadoAsaEsquerda || estadoAsaDireita ){ //Checa os sensores laterais
+ mapeamento(estadoAsaEsquerda,estadoAsaDireita); //faz o mapeamento da pista
+ if (estadoAsaDireita && !estadoAsaEsquerda){
+ contadorAsaDireita++; //Faz as contagens das marcacoes a direita
+ if(contadorAsaDireita >=2){//ARRUMAR UM JEITO DE DAR UM TEMPO ENTRE A LEITURA E O SEGUIDOR PARAR PARA ELE PASSAR TODO PELA LINHA DE CHAGADA
+ Aceleracao(0,0);
+ CriaParteQuatro();
+ criaArquivo();
+ ParadaFinal(); //Para o seguidor na segunda marcacao
+ }
+ }
+ }
+ }
break;
- case 2:
+ case 2: //Percorre a pista com o uso do mapa
+ while(1){
+ distanciaPraCurva =retornaPulsoDireito() * PERIMETRODARODA ;
+ if(distanciaPraCurva < mapaPraCorrer[posicaoNoMapa][1] - mapaPraCorrer[posicaoNoMapa][3]){ //Seleciona a velocidade
+ novaVelocidade = VelocidadeBateria (mapaPraCorrer[posicaoNoMapa+1][2]);
+ }
+ else{
+ novaVelocidade = VelocidadeBateria(mapaPraCorrer[posicaoNoMapa][2]);
+ }
+ Aceleracao(novaVelocidade,1); //Acelera ou desacelera o robo
+ LerSensoresFrontais(); //Leitura dos sensores frontais
+ erroSensores = CalculaErro(); //Calcula o arco do erro dos sensores
+ correcaoPD = PID_Sensores(erroSensores); //Calcula a correção do PD angular
+ velocidadeEsquerda = velocidadeAtual + correcaoPD; //Ajusta a inicialmente a velocidade das rodas para manter na linha
+ velocidadeDireita = velocidadeAtual - correcaoPD;
+ velocidadeRealDireita = retornaVelDireita();
+ velocidadeRealEsquerda = retornaVelEsquerda();
+ velocidadeEsquerda = transformaPWM(velocidadeEsquerda); //Converte velocidade PWM em m/s
+ velocidadeDireita = transformaPWM(velocidadeDireita);
+ correcaoEsquerda = PID_rodaEsquerda(velocidadeEsquerda,velocidadeRealEsquerda); //Faz o ajusta final da velocidade das rodas
+ correcaoDireita = PID_rodaDireita(velocidadeDireita,velocidadeRealDireita);
+ velocidadeEsquerda += correcaoEsquerda;
+ velocidadeDireita += correcaoDireita;
+ ControleMotores(velocidadeEsquerda,velocidadeDireita); //Muda a velocidade das rodas para as velocidades calculadas anteriormente
+ estadoAsaDireita = LerAsaDireita();
+ estadoAsaEsquerda = LerAsaEsquerda();
+ if(estadoAsaEsquerda || estadoAsaDireita ){ //Checa os sensores laterais
+ if(!estadoAsaEsquerda || estadoAsaDireita){ //Leu somente o asa direita
+ posicaoNoMapa++; //Atualiza a etapa do mapa da pista que esta sendo lido
+ }
+ if(estadoAsaDireita && !estadoAsaEsquerda){ //Leu somente a asa esquerda
+ contadorAsaDireita++; //Faz as contagens das marcacoes a direita
+ if(contadorAsaDireita >=2){
+ Aceleracao(0,0);
+ ParadaFinal(); //Para o seguidor na segunda marcacao
+ }
+ }
+ }
+ }
+
break;
- case 3:
+ case 3:
+ while(1){
+
+ }
break;
default:
@@ -83,7 +183,7 @@
}
- }
+
}