![](/media/cache/profiles/603ec80e671c74a8641d332712c458e8.jpg.50x50_q85.jpg)
Robô móvel com FREDOM KL25Z
Dependencies: Servo mbed HC-SR04
main.cpp@0:182a12372dd4, 2015-01-27 (annotated)
- Committer:
- Nestordp
- Date:
- Tue Jan 27 21:57:22 2015 +0000
- Revision:
- 0:182a12372dd4
- Child:
- 1:027b210e631d
Rob? m?vel.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Nestordp | 0:182a12372dd4 | 1 | #include "mbed.h" |
Nestordp | 0:182a12372dd4 | 2 | #include "Servo.h" |
Nestordp | 0:182a12372dd4 | 3 | |
Nestordp | 0:182a12372dd4 | 4 | #define velocidade(a, b) MDPWM = a; MEPWM = b |
Nestordp | 0:182a12372dd4 | 5 | #define frente() MDdirect = 1; MEdirect = 1 |
Nestordp | 0:182a12372dd4 | 6 | #define re() MDdirect = 0; MEdirect = 0 |
Nestordp | 0:182a12372dd4 | 7 | #define esquerda() MDdirect = 1; MEdirect = 0 |
Nestordp | 0:182a12372dd4 | 8 | #define direita() MDdirect = 0; MEdirect = 1 |
Nestordp | 0:182a12372dd4 | 9 | |
Nestordp | 0:182a12372dd4 | 10 | //Sonar Frente |
Nestordp | 0:182a12372dd4 | 11 | //DigitalOut trig(PTB2,0); //Configuração do pino de Trigger |
Nestordp | 0:182a12372dd4 | 12 | //InterruptIn echo(D12); //Configuração da interrupção por pino de Echo |
Nestordp | 0:182a12372dd4 | 13 | //Sonar Esquerdo |
Nestordp | 0:182a12372dd4 | 14 | //DigitalOut trig(A0,0); //Configuração do pino de Trigger |
Nestordp | 0:182a12372dd4 | 15 | //InterruptIn echo(D11); //Configuração da interrupção por pino de Echo |
Nestordp | 0:182a12372dd4 | 16 | |
Nestordp | 0:182a12372dd4 | 17 | |
Nestordp | 0:182a12372dd4 | 18 | |
Nestordp | 0:182a12372dd4 | 19 | DigitalOut bip(D2); // |
Nestordp | 0:182a12372dd4 | 20 | Servo myservo(D3); // |
Nestordp | 0:182a12372dd4 | 21 | DigitalOut MEdirect(D4); //Motor 2 Direction control |
Nestordp | 0:182a12372dd4 | 22 | PwmOut MEPWM(D5); //Motor 2 PWM control |
Nestordp | 0:182a12372dd4 | 23 | PwmOut MDPWM(D6); //Motor 1 PWM control |
Nestordp | 0:182a12372dd4 | 24 | DigitalOut MDdirect(D7); //Motor 1 Direction control |
Nestordp | 0:182a12372dd4 | 25 | //Sonar Frente |
Nestordp | 0:182a12372dd4 | 26 | DigitalOut trig(PTB2,0); //Configuração do pino de Trigger |
Nestordp | 0:182a12372dd4 | 27 | InterruptIn echo(D12); //Configuração da interrupção por pino de Echo |
Nestordp | 0:182a12372dd4 | 28 | DigitalOut Led(LED1); |
Nestordp | 0:182a12372dd4 | 29 | |
Nestordp | 0:182a12372dd4 | 30 | Serial pc(USBTX,USBRX); //Configuração da comunicação serial para enviar o valor do sensor |
Nestordp | 0:182a12372dd4 | 31 | Timer tempo; //Para usar rotinas de tempo |
Nestordp | 0:182a12372dd4 | 32 | |
Nestordp | 0:182a12372dd4 | 33 | |
Nestordp | 0:182a12372dd4 | 34 | |
Nestordp | 0:182a12372dd4 | 35 | void iniP(void); |
Nestordp | 0:182a12372dd4 | 36 | void finP(void); |
Nestordp | 0:182a12372dd4 | 37 | void lerSonar(void); |
Nestordp | 0:182a12372dd4 | 38 | |
Nestordp | 0:182a12372dd4 | 39 | float tdist=0, distcm=0, distin=0, dist0=0; |
Nestordp | 0:182a12372dd4 | 40 | float dist_esq = 0, dist_dir = 0, dist_fre = 0; |
Nestordp | 0:182a12372dd4 | 41 | |
Nestordp | 0:182a12372dd4 | 42 | int main() { |
Nestordp | 0:182a12372dd4 | 43 | printf("INICIA\n"); |
Nestordp | 0:182a12372dd4 | 44 | myservo.calibrate(0.0013, 45.0); |
Nestordp | 0:182a12372dd4 | 45 | wait_ms(3000); |
Nestordp | 0:182a12372dd4 | 46 | //myservo.position(-33.0); todo pra direita |
Nestordp | 0:182a12372dd4 | 47 | //myservo.position(30.8); todo pra esquerda |
Nestordp | 0:182a12372dd4 | 48 | myservo.position(-3.0); |
Nestordp | 0:182a12372dd4 | 49 | Led = 1; |
Nestordp | 0:182a12372dd4 | 50 | printf("COMECA O PROGRAMA\n"); |
Nestordp | 0:182a12372dd4 | 51 | velocidade(0.38, 0.38); |
Nestordp | 0:182a12372dd4 | 52 | frente(); |
Nestordp | 0:182a12372dd4 | 53 | |
Nestordp | 0:182a12372dd4 | 54 | while(1){ |
Nestordp | 0:182a12372dd4 | 55 | lerSonar(); |
Nestordp | 0:182a12372dd4 | 56 | printf("Distancia detectada pelo sensor %.2f cm \n",distcm); |
Nestordp | 0:182a12372dd4 | 57 | dist_fre = distcm; |
Nestordp | 0:182a12372dd4 | 58 | |
Nestordp | 0:182a12372dd4 | 59 | if((dist_fre <= 30) && (dist_fre > 3)){ |
Nestordp | 0:182a12372dd4 | 60 | bip = 1; |
Nestordp | 0:182a12372dd4 | 61 | printf("PARA\n"); |
Nestordp | 0:182a12372dd4 | 62 | re(); |
Nestordp | 0:182a12372dd4 | 63 | wait_ms(100); |
Nestordp | 0:182a12372dd4 | 64 | velocidade(0.0, 0.0); |
Nestordp | 0:182a12372dd4 | 65 | bip = 0; |
Nestordp | 0:182a12372dd4 | 66 | |
Nestordp | 0:182a12372dd4 | 67 | myservo.position(-33.0); |
Nestordp | 0:182a12372dd4 | 68 | wait_ms(1000); |
Nestordp | 0:182a12372dd4 | 69 | lerSonar(); |
Nestordp | 0:182a12372dd4 | 70 | printf("Distancia direita %.2f cm \n",distcm); |
Nestordp | 0:182a12372dd4 | 71 | dist_dir = distcm; |
Nestordp | 0:182a12372dd4 | 72 | |
Nestordp | 0:182a12372dd4 | 73 | myservo.position(30.8); |
Nestordp | 0:182a12372dd4 | 74 | wait_ms(1000); |
Nestordp | 0:182a12372dd4 | 75 | lerSonar(); |
Nestordp | 0:182a12372dd4 | 76 | printf("Distancia esquerda %.2f cm \n",distcm); |
Nestordp | 0:182a12372dd4 | 77 | dist_esq = distcm; |
Nestordp | 0:182a12372dd4 | 78 | |
Nestordp | 0:182a12372dd4 | 79 | if(dist_dir > dist_esq){ |
Nestordp | 0:182a12372dd4 | 80 | printf("VIRA ESQUERDA \n"); |
Nestordp | 0:182a12372dd4 | 81 | direita(); |
Nestordp | 0:182a12372dd4 | 82 | velocidade(0.38, 0.38); |
Nestordp | 0:182a12372dd4 | 83 | wait_ms(250); |
Nestordp | 0:182a12372dd4 | 84 | velocidade(0.0, 0.0); |
Nestordp | 0:182a12372dd4 | 85 | myservo.position(-3.0); |
Nestordp | 0:182a12372dd4 | 86 | wait_ms(500); |
Nestordp | 0:182a12372dd4 | 87 | velocidade(0.38, 0.38); |
Nestordp | 0:182a12372dd4 | 88 | frente(); |
Nestordp | 0:182a12372dd4 | 89 | } |
Nestordp | 0:182a12372dd4 | 90 | else if(dist_dir < dist_esq){ |
Nestordp | 0:182a12372dd4 | 91 | printf("VIRA ESQUERDA \n"); |
Nestordp | 0:182a12372dd4 | 92 | esquerda(); |
Nestordp | 0:182a12372dd4 | 93 | velocidade(0.38, 0.38); |
Nestordp | 0:182a12372dd4 | 94 | wait_ms(250); |
Nestordp | 0:182a12372dd4 | 95 | velocidade(0.0, 0.0); |
Nestordp | 0:182a12372dd4 | 96 | myservo.position(-3.0); |
Nestordp | 0:182a12372dd4 | 97 | wait_ms(500); |
Nestordp | 0:182a12372dd4 | 98 | velocidade(0.38, 0.38); |
Nestordp | 0:182a12372dd4 | 99 | frente(); |
Nestordp | 0:182a12372dd4 | 100 | } |
Nestordp | 0:182a12372dd4 | 101 | //myservo.position(-3.0); |
Nestordp | 0:182a12372dd4 | 102 | //wait_ms(100); |
Nestordp | 0:182a12372dd4 | 103 | } |
Nestordp | 0:182a12372dd4 | 104 | } |
Nestordp | 0:182a12372dd4 | 105 | |
Nestordp | 0:182a12372dd4 | 106 | |
Nestordp | 0:182a12372dd4 | 107 | |
Nestordp | 0:182a12372dd4 | 108 | /*while(1){ |
Nestordp | 0:182a12372dd4 | 109 | M1direct = 1; |
Nestordp | 0:182a12372dd4 | 110 | M2direct = 1; |
Nestordp | 0:182a12372dd4 | 111 | bip = 0; |
Nestordp | 0:182a12372dd4 | 112 | |
Nestordp | 0:182a12372dd4 | 113 | for(float p=0; p<=1.0; p += 0.1){ |
Nestordp | 0:182a12372dd4 | 114 | myservo = p; |
Nestordp | 0:182a12372dd4 | 115 | trig=1; //Inicio do trigger |
Nestordp | 0:182a12372dd4 | 116 | wait_us(10); // 10us de pulso |
Nestordp | 0:182a12372dd4 | 117 | trig=0; //Fim do trigger |
Nestordp | 0:182a12372dd4 | 118 | echo.rise(&iniP); //leitura do inicio do pulso de retorno |
Nestordp | 0:182a12372dd4 | 119 | echo.fall(&finP); //Leitura do final do pulso de retorno |
Nestordp | 0:182a12372dd4 | 120 | printf("Distancia detectada pelo sensor %.2f cm \n",distcm); |
Nestordp | 0:182a12372dd4 | 121 | wait(0.05); |
Nestordp | 0:182a12372dd4 | 122 | } |
Nestordp | 0:182a12372dd4 | 123 | wait(0.13); |
Nestordp | 0:182a12372dd4 | 124 | |
Nestordp | 0:182a12372dd4 | 125 | M1direct = 1; |
Nestordp | 0:182a12372dd4 | 126 | M2direct = 1; |
Nestordp | 0:182a12372dd4 | 127 | bip = 0; |
Nestordp | 0:182a12372dd4 | 128 | |
Nestordp | 0:182a12372dd4 | 129 | for(float p=1.0; p>=0.0; p -= 0.1) { |
Nestordp | 0:182a12372dd4 | 130 | myservo = p; |
Nestordp | 0:182a12372dd4 | 131 | trig=1; //Inicio do trigger |
Nestordp | 0:182a12372dd4 | 132 | wait_us(10); // 10us de pulso |
Nestordp | 0:182a12372dd4 | 133 | trig=0; //Fim do trigger |
Nestordp | 0:182a12372dd4 | 134 | echo.rise(&iniP); //leitura do inicio do pulso de retorno |
Nestordp | 0:182a12372dd4 | 135 | echo.fall(&finP); //Leitura do final do pulso de retorno |
Nestordp | 0:182a12372dd4 | 136 | printf("Distancia detectada pelo sensor %.2f cm \n",distcm); |
Nestordp | 0:182a12372dd4 | 137 | wait(0.05); |
Nestordp | 0:182a12372dd4 | 138 | } |
Nestordp | 0:182a12372dd4 | 139 | wait(0.13); |
Nestordp | 0:182a12372dd4 | 140 | }*/ |
Nestordp | 0:182a12372dd4 | 141 | } |
Nestordp | 0:182a12372dd4 | 142 | |
Nestordp | 0:182a12372dd4 | 143 | //Rotina para receber o pulso inicial do pino Echo |
Nestordp | 0:182a12372dd4 | 144 | void iniP(void) |
Nestordp | 0:182a12372dd4 | 145 | { |
Nestordp | 0:182a12372dd4 | 146 | tempo.start(); //Rotina para iniciar o contador |
Nestordp | 0:182a12372dd4 | 147 | return; |
Nestordp | 0:182a12372dd4 | 148 | } |
Nestordp | 0:182a12372dd4 | 149 | //Rotina para pegar o tempo final do pulso |
Nestordp | 0:182a12372dd4 | 150 | void finP(void) |
Nestordp | 0:182a12372dd4 | 151 | { |
Nestordp | 0:182a12372dd4 | 152 | tdist = tempo.read_us(); //Leitura do tempo transcorrido |
Nestordp | 0:182a12372dd4 | 153 | distcm = tdist/58; //Cálculo da distância detectada em "cm" |
Nestordp | 0:182a12372dd4 | 154 | //distin = tdist/148; //Cálculo da distância detectada em "in" |
Nestordp | 0:182a12372dd4 | 155 | |
Nestordp | 0:182a12372dd4 | 156 | tempo.stop(); //Paro o temporizador |
Nestordp | 0:182a12372dd4 | 157 | tempo.reset(); //Reset para o próximo ciclo |
Nestordp | 0:182a12372dd4 | 158 | return; |
Nestordp | 0:182a12372dd4 | 159 | } |
Nestordp | 0:182a12372dd4 | 160 | |
Nestordp | 0:182a12372dd4 | 161 | void lerSonar(void) |
Nestordp | 0:182a12372dd4 | 162 | { |
Nestordp | 0:182a12372dd4 | 163 | trig=1; //Inicio do trigger |
Nestordp | 0:182a12372dd4 | 164 | wait_us(10); //10us de pulso |
Nestordp | 0:182a12372dd4 | 165 | trig=0; //Fim do trigger |
Nestordp | 0:182a12372dd4 | 166 | echo.rise(&iniP); //leitura do inicio do pulso de retorno |
Nestordp | 0:182a12372dd4 | 167 | echo.fall(&finP); //Leitura do final do pulso de retorno |
Nestordp | 0:182a12372dd4 | 168 | wait_ms(100); |
Nestordp | 0:182a12372dd4 | 169 | return; |
Nestordp | 0:182a12372dd4 | 170 | } |