codigo do encoder do papacuras
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:d0bc73e9c82f
- Child:
- 1:623d2342ba23
diff -r 000000000000 -r d0bc73e9c82f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Jul 12 17:10:26 2020 +0000 @@ -0,0 +1,93 @@ +#include "mbed.h" + +#define debug true; +float perimetroDaRodaPorTick = (22*3.14*2)/(12*10); // perimetro da roda / (precisao do encoder * reducao) +//poderia ser define mas da alguem erro, acredito q seja por causa do OS 6 + +// acertar os pinos do encoder +InterruptIn encoderDireitoInterrupt (p29); +InterruptIn encoderEsquerdoInterrupt (p30); + +DigitalIn encoderDireitoDigital (p28); +DigitalIn encoderEsquerdoDigital(p27); + +Timer TempoParaVelocidadeDireita ; +Timer TempoParaVelocidadeEsquerda; + +int contadorDeGirosDireito = 0; +int contadorDeGirosEsquerdo = 0; +int contadorDoMapa = 0; + +float velocidadeRodaDireita; +float velocidadeRodaEsquerda; +float mapaDaPista[100][2]; // 100 deve mudar + + +void ContaGirosDireito () +{ + if(encoderDireitoDigital == 1) + { + contadorDeGirosDireito +=1 ; // Conta o giro + velocidadeRodaDireita = perimetroDaRodaPorTick/TempoParaVelocidadeDireita.read();//calcula a velocidade + TempoParaVelocidadeDireita.reset(); + + } + else + { + contadorDeGirosDireito -=1;// Conta o giro + velocidadeRodaDireita = -perimetroDaRodaPorTick/TempoParaVelocidadeDireita.read();//calcula a velocidade + TempoParaVelocidadeDireita.reset(); + } + return; +} + +void ContaGirosEsquerdo () +{ + if(encoderEsquerdoDigital == 1) + { + contadorDeGirosEsquerdo += 1 ;// Conta o giro + velocidadeRodaEsquerda = perimetroDaRodaPorTick/TempoParaVelocidadeEsquerda.read();//calcula a velocidade + TempoParaVelocidadeEsquerda.reset(); + } + else + { + contadorDeGirosEsquerdo -= 1;// Conta o giro + velocidadeRodaEsquerda = -perimetroDaRodaPorTick/TempoParaVelocidadeEsquerda.read();//calcula a velocidade + TempoParaVelocidadeEsquerda.reset(); + } + return; +} + +void MapeamentoDaPista() //coloca no mapa da pista o quanto cada roda percorreu +{ + if (contadorDeGirosDireito > 1000 || contadorDeGirosEsquerdo > 1000) + { + // Faz a contagem de giros do motor e guarda a distancia percorrida + mapaDaPista[contadorDoMapa][0] = (contadorDeGirosEsquerdo * perimetroDaRodaPorTick)+mapaDaPista[contadorDoMapa-1][0]; + mapaDaPista[contadorDoMapa][1] = (contadorDeGirosDireito * perimetroDaRodaPorTick)+mapaDaPista[contadorDoMapa-1][1]; + contadorDeGirosDireito = 0; + contadorDeGirosEsquerdo = 0; + contadorDoMapa += 1; + } + return; +} + + +// main() runs in its own thread in the OS +int main() +{ + encoderDireitoInterrupt.rise(&ContaGirosDireito); + encoderEsquerdoInterrupt.rise(&ContaGirosEsquerdo); + TempoParaVelocidadeDireita.start(); + TempoParaVelocidadeEsquerda.start(); + while (true) { + MapeamentoDaPista(); + #ifdef debug + printf ("Giros da roda direita: %i", (contadorDeGirosDireito)); + printf ("Giros da roda esquerda: %i",(contadorDeGirosEsquerdo)); + printf("Velocidade: %f",velocidadeRodaDireita); + #endif + + } +} +