codigo do encoder do papacuras
Dependencies: mbed
main.cpp@0:d0bc73e9c82f, 2020-07-12 (annotated)
- Committer:
- rperoba
- Date:
- Sun Jul 12 17:10:26 2020 +0000
- Revision:
- 0:d0bc73e9c82f
- Child:
- 1:623d2342ba23
Codigo do encoder do Papacuras
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rperoba | 0:d0bc73e9c82f | 1 | #include "mbed.h" |
rperoba | 0:d0bc73e9c82f | 2 | |
rperoba | 0:d0bc73e9c82f | 3 | #define debug true; |
rperoba | 0:d0bc73e9c82f | 4 | float perimetroDaRodaPorTick = (22*3.14*2)/(12*10); // perimetro da roda / (precisao do encoder * reducao) |
rperoba | 0:d0bc73e9c82f | 5 | //poderia ser define mas da alguem erro, acredito q seja por causa do OS 6 |
rperoba | 0:d0bc73e9c82f | 6 | |
rperoba | 0:d0bc73e9c82f | 7 | // acertar os pinos do encoder |
rperoba | 0:d0bc73e9c82f | 8 | InterruptIn encoderDireitoInterrupt (p29); |
rperoba | 0:d0bc73e9c82f | 9 | InterruptIn encoderEsquerdoInterrupt (p30); |
rperoba | 0:d0bc73e9c82f | 10 | |
rperoba | 0:d0bc73e9c82f | 11 | DigitalIn encoderDireitoDigital (p28); |
rperoba | 0:d0bc73e9c82f | 12 | DigitalIn encoderEsquerdoDigital(p27); |
rperoba | 0:d0bc73e9c82f | 13 | |
rperoba | 0:d0bc73e9c82f | 14 | Timer TempoParaVelocidadeDireita ; |
rperoba | 0:d0bc73e9c82f | 15 | Timer TempoParaVelocidadeEsquerda; |
rperoba | 0:d0bc73e9c82f | 16 | |
rperoba | 0:d0bc73e9c82f | 17 | int contadorDeGirosDireito = 0; |
rperoba | 0:d0bc73e9c82f | 18 | int contadorDeGirosEsquerdo = 0; |
rperoba | 0:d0bc73e9c82f | 19 | int contadorDoMapa = 0; |
rperoba | 0:d0bc73e9c82f | 20 | |
rperoba | 0:d0bc73e9c82f | 21 | float velocidadeRodaDireita; |
rperoba | 0:d0bc73e9c82f | 22 | float velocidadeRodaEsquerda; |
rperoba | 0:d0bc73e9c82f | 23 | float mapaDaPista[100][2]; // 100 deve mudar |
rperoba | 0:d0bc73e9c82f | 24 | |
rperoba | 0:d0bc73e9c82f | 25 | |
rperoba | 0:d0bc73e9c82f | 26 | void ContaGirosDireito () |
rperoba | 0:d0bc73e9c82f | 27 | { |
rperoba | 0:d0bc73e9c82f | 28 | if(encoderDireitoDigital == 1) |
rperoba | 0:d0bc73e9c82f | 29 | { |
rperoba | 0:d0bc73e9c82f | 30 | contadorDeGirosDireito +=1 ; // Conta o giro |
rperoba | 0:d0bc73e9c82f | 31 | velocidadeRodaDireita = perimetroDaRodaPorTick/TempoParaVelocidadeDireita.read();//calcula a velocidade |
rperoba | 0:d0bc73e9c82f | 32 | TempoParaVelocidadeDireita.reset(); |
rperoba | 0:d0bc73e9c82f | 33 | |
rperoba | 0:d0bc73e9c82f | 34 | } |
rperoba | 0:d0bc73e9c82f | 35 | else |
rperoba | 0:d0bc73e9c82f | 36 | { |
rperoba | 0:d0bc73e9c82f | 37 | contadorDeGirosDireito -=1;// Conta o giro |
rperoba | 0:d0bc73e9c82f | 38 | velocidadeRodaDireita = -perimetroDaRodaPorTick/TempoParaVelocidadeDireita.read();//calcula a velocidade |
rperoba | 0:d0bc73e9c82f | 39 | TempoParaVelocidadeDireita.reset(); |
rperoba | 0:d0bc73e9c82f | 40 | } |
rperoba | 0:d0bc73e9c82f | 41 | return; |
rperoba | 0:d0bc73e9c82f | 42 | } |
rperoba | 0:d0bc73e9c82f | 43 | |
rperoba | 0:d0bc73e9c82f | 44 | void ContaGirosEsquerdo () |
rperoba | 0:d0bc73e9c82f | 45 | { |
rperoba | 0:d0bc73e9c82f | 46 | if(encoderEsquerdoDigital == 1) |
rperoba | 0:d0bc73e9c82f | 47 | { |
rperoba | 0:d0bc73e9c82f | 48 | contadorDeGirosEsquerdo += 1 ;// Conta o giro |
rperoba | 0:d0bc73e9c82f | 49 | velocidadeRodaEsquerda = perimetroDaRodaPorTick/TempoParaVelocidadeEsquerda.read();//calcula a velocidade |
rperoba | 0:d0bc73e9c82f | 50 | TempoParaVelocidadeEsquerda.reset(); |
rperoba | 0:d0bc73e9c82f | 51 | } |
rperoba | 0:d0bc73e9c82f | 52 | else |
rperoba | 0:d0bc73e9c82f | 53 | { |
rperoba | 0:d0bc73e9c82f | 54 | contadorDeGirosEsquerdo -= 1;// Conta o giro |
rperoba | 0:d0bc73e9c82f | 55 | velocidadeRodaEsquerda = -perimetroDaRodaPorTick/TempoParaVelocidadeEsquerda.read();//calcula a velocidade |
rperoba | 0:d0bc73e9c82f | 56 | TempoParaVelocidadeEsquerda.reset(); |
rperoba | 0:d0bc73e9c82f | 57 | } |
rperoba | 0:d0bc73e9c82f | 58 | return; |
rperoba | 0:d0bc73e9c82f | 59 | } |
rperoba | 0:d0bc73e9c82f | 60 | |
rperoba | 0:d0bc73e9c82f | 61 | void MapeamentoDaPista() //coloca no mapa da pista o quanto cada roda percorreu |
rperoba | 0:d0bc73e9c82f | 62 | { |
rperoba | 0:d0bc73e9c82f | 63 | if (contadorDeGirosDireito > 1000 || contadorDeGirosEsquerdo > 1000) |
rperoba | 0:d0bc73e9c82f | 64 | { |
rperoba | 0:d0bc73e9c82f | 65 | // Faz a contagem de giros do motor e guarda a distancia percorrida |
rperoba | 0:d0bc73e9c82f | 66 | mapaDaPista[contadorDoMapa][0] = (contadorDeGirosEsquerdo * perimetroDaRodaPorTick)+mapaDaPista[contadorDoMapa-1][0]; |
rperoba | 0:d0bc73e9c82f | 67 | mapaDaPista[contadorDoMapa][1] = (contadorDeGirosDireito * perimetroDaRodaPorTick)+mapaDaPista[contadorDoMapa-1][1]; |
rperoba | 0:d0bc73e9c82f | 68 | contadorDeGirosDireito = 0; |
rperoba | 0:d0bc73e9c82f | 69 | contadorDeGirosEsquerdo = 0; |
rperoba | 0:d0bc73e9c82f | 70 | contadorDoMapa += 1; |
rperoba | 0:d0bc73e9c82f | 71 | } |
rperoba | 0:d0bc73e9c82f | 72 | return; |
rperoba | 0:d0bc73e9c82f | 73 | } |
rperoba | 0:d0bc73e9c82f | 74 | |
rperoba | 0:d0bc73e9c82f | 75 | |
rperoba | 0:d0bc73e9c82f | 76 | // main() runs in its own thread in the OS |
rperoba | 0:d0bc73e9c82f | 77 | int main() |
rperoba | 0:d0bc73e9c82f | 78 | { |
rperoba | 0:d0bc73e9c82f | 79 | encoderDireitoInterrupt.rise(&ContaGirosDireito); |
rperoba | 0:d0bc73e9c82f | 80 | encoderEsquerdoInterrupt.rise(&ContaGirosEsquerdo); |
rperoba | 0:d0bc73e9c82f | 81 | TempoParaVelocidadeDireita.start(); |
rperoba | 0:d0bc73e9c82f | 82 | TempoParaVelocidadeEsquerda.start(); |
rperoba | 0:d0bc73e9c82f | 83 | while (true) { |
rperoba | 0:d0bc73e9c82f | 84 | MapeamentoDaPista(); |
rperoba | 0:d0bc73e9c82f | 85 | #ifdef debug |
rperoba | 0:d0bc73e9c82f | 86 | printf ("Giros da roda direita: %i", (contadorDeGirosDireito)); |
rperoba | 0:d0bc73e9c82f | 87 | printf ("Giros da roda esquerda: %i",(contadorDeGirosEsquerdo)); |
rperoba | 0:d0bc73e9c82f | 88 | printf("Velocidade: %f",velocidadeRodaDireita); |
rperoba | 0:d0bc73e9c82f | 89 | #endif |
rperoba | 0:d0bc73e9c82f | 90 | |
rperoba | 0:d0bc73e9c82f | 91 | } |
rperoba | 0:d0bc73e9c82f | 92 | } |
rperoba | 0:d0bc73e9c82f | 93 |