![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TP_presa
Dependencies: SRF08 Servo mbed
Diff: main.cpp
- Revision:
- 7:7d3c6326cbc3
- Parent:
- 6:d61052f4ab1e
- Child:
- 8:ea8db9aacdfb
diff -r d61052f4ab1e -r 7d3c6326cbc3 main.cpp --- a/main.cpp Mon Jun 26 06:36:03 2017 +0000 +++ b/main.cpp Mon Jun 26 09:58:11 2017 +0000 @@ -1,55 +1,132 @@ #include "mbed.h" #include "CAR.h" #include "SRF08.h" +#include "Servo.h" +DigitalOut S2 (p11); +AnalogIn opt (p15); Serial bth(p13,p14); -SRF08 capteur_US(p28,p27,0x0E); +//Serial sbt(p9, p10); // tx, rx +SRF08 srf08(p28, p27, 0xE0); +//Servo myservo(p26); + +float aMes[]= {0,0,0}; +PwmOut servo(p26); +int iState = 1; +bool bState =0; + +char Adress = 128; +char action; CAR robot(p9,p10); //CAR robot; -void act(char action) { - switch(action) { - case 'a': - bth.printf("avancer\n\r"); - robot.avancer(70); - break; - case 's': - bth.printf("stop\n\r"); - robot.arreter(); - break; - case 'u': - bth.printf("arret\n\r"); - robot.arreter(); +void act() +{ + action=bth.getc(); + switch(action) { + case 'a': + bth.printf("avancer\n\r"); + robot.avancer(70); + break; + case 's': + bth.printf("stop\n\r"); + robot.arreter(); + break; + case 'u': + bth.printf("arret\n\r"); + robot.arreter(); + break; + case 'r': + bth.printf("reculer\n\r"); + robot.reculer(70); + break; + case 'd': + bth.printf("droite\n\r"); + robot.tourner_droite(70); + break; + case 'g': + bth.printf("gauche\n\r"); + robot.tourner_gauche(70); + break; + case 'h': + bth.printf("demi tour horaire\n\r"); + robot.demi_tour_droite(100); + break; + case'l': + bth.printf("demi tour left\n\r"); + robot.demi_tour_gauche(100); + break; + case not 'o': + bState = 0; + case 'o': + bState = !bState; + if (not bState) { + bth.printf("stop\n\r"); + robot.arreter(); + } + break; + } + action='0'; +} + + +//////////////////////////////////////////// + +int main() +{ + S2=0; + int visu; + bth.baud(57600); + bth.attach(&act); + + servo.period(0.02); + while(1) { + bState = 1; + if (bState) { + switch (iState) { + + case 0: + + if (srf08.read() <60 and srf08.read() >0 ) { + iState = 1; + bth.printf("stop\n\r"); + robot.arreter(); + } else { + servo.write(0.08); + bth.printf("avancer\n\r"); + robot.avancer(50); + } break; - case 'r': - bth.printf("reculer\n\r"); - robot.reculer(70); - break; - case 'd': - bth.printf("droite\n\r"); - robot.tourner_droite(70); - break; - case 'g': - bth.printf("gauche\n\r"); - robot.tourner_gauche(70); - break; - case 'h': - bth.printf("demi tour horaire\n\r"); - robot.demi_tour_droite(100); - break; - case'l': - bth.printf("demi tour left\n\r"); - robot.demi_tour_gauche(100); + + case 1 : + servo.write(0.05); + wait (0.2); + aMes[1] = srf08.read(); + + servo.write(0.1); + wait (0.2); + aMes[2]= srf08.read(); + + servo.write(0.08); + + if (aMes[1] <100 and aMes[1] >0) { + action = 'g'; + bth.printf("gauche\n\r"); + robot.tourner_gauche(70); + } else if (aMes[2] <100 and aMes[2] >0) { + bth.printf("droite\n\r"); + robot.tourner_droite(70); + } else { + bth.printf("demi tour horaire\n\r"); + robot.demi_tour_droite(100); + } + iState = 0; break; } - action='0'; } -int main(void) -{ - //S2=0; - // CAR(p9,p10); - - while(1) { - + visu=opt.read_u16(); + visu=422006/(visu-3475); + bth.printf("Measured range : %.2f cm\n",srf08.read()); } -} +} +