TP_presa
Dependencies: SRF08 Servo mbed
main.cpp@11:8853764285f9, 2017-07-03 (annotated)
- Committer:
- Remi95
- Date:
- Mon Jul 03 09:42:02 2017 +0000
- Revision:
- 11:8853764285f9
- Parent:
- 10:cf2719d4b93f
finished
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Remi95 | 0:99cb324a2329 | 1 | #include "mbed.h" |
Remi95 | 6:d61052f4ab1e | 2 | #include "CAR.h" |
Remi95 | 5:8c2101d3ecef | 3 | #include "SRF08.h" |
Remi95 | 7:7d3c6326cbc3 | 4 | #include "Servo.h" |
Remi95 | 0:99cb324a2329 | 5 | |
Remi95 | 10:cf2719d4b93f | 6 | #define BAT 7.5 |
Remi95 | 10:cf2719d4b93f | 7 | |
Remi95 | 7:7d3c6326cbc3 | 8 | DigitalOut S2 (p11); |
Remi95 | 7:7d3c6326cbc3 | 9 | AnalogIn opt (p15); |
Remi95 | 10:cf2719d4b93f | 10 | //AnalogIn batterie (p20); |
Remi95 | 6:d61052f4ab1e | 11 | Serial bth(p13,p14); |
Remi95 | 7:7d3c6326cbc3 | 12 | //Serial sbt(p9, p10); // tx, rx |
Remi95 | 7:7d3c6326cbc3 | 13 | SRF08 srf08(p28, p27, 0xE0); |
Remi95 | 7:7d3c6326cbc3 | 14 | //Servo myservo(p26); |
Remi95 | 7:7d3c6326cbc3 | 15 | |
Remi95 | 7:7d3c6326cbc3 | 16 | float aMes[]= {0,0,0}; |
Remi95 | 7:7d3c6326cbc3 | 17 | PwmOut servo(p26); |
Remi95 | 7:7d3c6326cbc3 | 18 | int iState = 1; |
Remi95 | 7:7d3c6326cbc3 | 19 | bool bState =0; |
Remi95 | 7:7d3c6326cbc3 | 20 | |
Remi95 | 7:7d3c6326cbc3 | 21 | char Adress = 128; |
Remi95 | 7:7d3c6326cbc3 | 22 | char action; |
Remi95 | 6:d61052f4ab1e | 23 | CAR robot(p9,p10); |
Remi95 | 6:d61052f4ab1e | 24 | //CAR robot; |
Remi95 | 4:aafb0016ed84 | 25 | |
Remi95 | 7:7d3c6326cbc3 | 26 | void act() |
Remi95 | 7:7d3c6326cbc3 | 27 | { |
Remi95 | 10:cf2719d4b93f | 28 | //bth.printf("Batterie : %.2f\n\r", batterie.read()*36.30); |
Remi95 | 7:7d3c6326cbc3 | 29 | action=bth.getc(); |
Remi95 | 7:7d3c6326cbc3 | 30 | switch(action) { |
Remi95 | 7:7d3c6326cbc3 | 31 | case 'a': |
Remi95 | 7:7d3c6326cbc3 | 32 | bth.printf("avancer\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 33 | robot.avancer(70); |
Remi95 | 7:7d3c6326cbc3 | 34 | break; |
Remi95 | 7:7d3c6326cbc3 | 35 | case 's': |
Remi95 | 7:7d3c6326cbc3 | 36 | bth.printf("stop\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 37 | robot.arreter(); |
Remi95 | 7:7d3c6326cbc3 | 38 | break; |
Remi95 | 7:7d3c6326cbc3 | 39 | case 'u': |
Remi95 | 7:7d3c6326cbc3 | 40 | bth.printf("arret\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 41 | robot.arreter(); |
Remi95 | 7:7d3c6326cbc3 | 42 | break; |
Remi95 | 7:7d3c6326cbc3 | 43 | case 'r': |
Remi95 | 7:7d3c6326cbc3 | 44 | bth.printf("reculer\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 45 | robot.reculer(70); |
Remi95 | 7:7d3c6326cbc3 | 46 | break; |
Remi95 | 7:7d3c6326cbc3 | 47 | case 'd': |
Remi95 | 7:7d3c6326cbc3 | 48 | bth.printf("droite\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 49 | robot.tourner_droite(70); |
Remi95 | 7:7d3c6326cbc3 | 50 | break; |
Remi95 | 7:7d3c6326cbc3 | 51 | case 'g': |
Remi95 | 7:7d3c6326cbc3 | 52 | bth.printf("gauche\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 53 | robot.tourner_gauche(70); |
Remi95 | 7:7d3c6326cbc3 | 54 | break; |
Remi95 | 7:7d3c6326cbc3 | 55 | case 'h': |
Remi95 | 7:7d3c6326cbc3 | 56 | bth.printf("demi tour horaire\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 57 | robot.demi_tour_droite(100); |
Remi95 | 7:7d3c6326cbc3 | 58 | break; |
Remi95 | 7:7d3c6326cbc3 | 59 | case'l': |
Remi95 | 7:7d3c6326cbc3 | 60 | bth.printf("demi tour left\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 61 | robot.demi_tour_gauche(100); |
Remi95 | 7:7d3c6326cbc3 | 62 | break; |
Remi95 | 7:7d3c6326cbc3 | 63 | case 'o': |
Remi95 | 7:7d3c6326cbc3 | 64 | bState = !bState; |
Remi95 | 9:53a6a4e0db9f | 65 | //iState = 0; |
Remi95 | 7:7d3c6326cbc3 | 66 | break; |
Remi95 | 7:7d3c6326cbc3 | 67 | } |
Remi95 | 7:7d3c6326cbc3 | 68 | action='0'; |
Remi95 | 7:7d3c6326cbc3 | 69 | } |
Remi95 | 7:7d3c6326cbc3 | 70 | |
Remi95 | 7:7d3c6326cbc3 | 71 | |
Remi95 | 7:7d3c6326cbc3 | 72 | //////////////////////////////////////////// |
Remi95 | 7:7d3c6326cbc3 | 73 | |
Remi95 | 7:7d3c6326cbc3 | 74 | int main() |
Remi95 | 7:7d3c6326cbc3 | 75 | { |
Remi95 | 7:7d3c6326cbc3 | 76 | S2=0; |
Remi95 | 7:7d3c6326cbc3 | 77 | int visu; |
Remi95 | 7:7d3c6326cbc3 | 78 | bth.baud(57600); |
Remi95 | 7:7d3c6326cbc3 | 79 | bth.attach(&act); |
Remi95 | 8:ea8db9aacdfb | 80 | bState = 1; |
Remi95 | 7:7d3c6326cbc3 | 81 | servo.period(0.02); |
Remi95 | 7:7d3c6326cbc3 | 82 | while(1) { |
Remi95 | 7:7d3c6326cbc3 | 83 | if (bState) { |
Remi95 | 7:7d3c6326cbc3 | 84 | switch (iState) { |
Remi95 | 7:7d3c6326cbc3 | 85 | |
Remi95 | 7:7d3c6326cbc3 | 86 | case 0: |
Remi95 | 7:7d3c6326cbc3 | 87 | |
Remi95 | 8:ea8db9aacdfb | 88 | if (srf08.read() <40 and srf08.read() >0 ) { |
Remi95 | 7:7d3c6326cbc3 | 89 | iState = 1; |
Remi95 | 7:7d3c6326cbc3 | 90 | bth.printf("stop\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 91 | robot.arreter(); |
Remi95 | 7:7d3c6326cbc3 | 92 | } else { |
Remi95 | 7:7d3c6326cbc3 | 93 | servo.write(0.08); |
Remi95 | 7:7d3c6326cbc3 | 94 | bth.printf("avancer\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 95 | robot.avancer(50); |
Remi95 | 7:7d3c6326cbc3 | 96 | } |
Remi95 | 6:d61052f4ab1e | 97 | break; |
Remi95 | 7:7d3c6326cbc3 | 98 | |
Remi95 | 7:7d3c6326cbc3 | 99 | case 1 : |
Remi95 | 7:7d3c6326cbc3 | 100 | servo.write(0.05); |
Remi95 | 8:ea8db9aacdfb | 101 | wait (0.4); |
Remi95 | 7:7d3c6326cbc3 | 102 | aMes[1] = srf08.read(); |
Remi95 | 7:7d3c6326cbc3 | 103 | |
Remi95 | 8:ea8db9aacdfb | 104 | servo.write(0.095); |
Remi95 | 8:ea8db9aacdfb | 105 | wait (0.4); |
Remi95 | 7:7d3c6326cbc3 | 106 | aMes[2]= srf08.read(); |
Remi95 | 7:7d3c6326cbc3 | 107 | |
Remi95 | 7:7d3c6326cbc3 | 108 | servo.write(0.08); |
Remi95 | 8:ea8db9aacdfb | 109 | |
Remi95 | 8:ea8db9aacdfb | 110 | bth.printf("capt 1 : %.2f capt 2 : %.2f",aMes[1], aMes[2]); |
Remi95 | 8:ea8db9aacdfb | 111 | //if (aMes[1] <80 and aMes[1] >0) |
Remi95 | 8:ea8db9aacdfb | 112 | if (aMes[2]>40) { |
Remi95 | 7:7d3c6326cbc3 | 113 | action = 'g'; |
Remi95 | 7:7d3c6326cbc3 | 114 | bth.printf("gauche\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 115 | robot.tourner_gauche(70); |
Remi95 | 8:ea8db9aacdfb | 116 | //} else if (aMes[2] <80 and aMes[2] >0) { |
Remi95 | 8:ea8db9aacdfb | 117 | } else if (aMes[1]>40) { |
Remi95 | 7:7d3c6326cbc3 | 118 | bth.printf("droite\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 119 | robot.tourner_droite(70); |
Remi95 | 7:7d3c6326cbc3 | 120 | } else { |
Remi95 | 7:7d3c6326cbc3 | 121 | bth.printf("demi tour horaire\n\r"); |
Remi95 | 7:7d3c6326cbc3 | 122 | robot.demi_tour_droite(100); |
Remi95 | 7:7d3c6326cbc3 | 123 | } |
Remi95 | 7:7d3c6326cbc3 | 124 | iState = 0; |
Remi95 | 6:d61052f4ab1e | 125 | break; |
Remi95 | 6:d61052f4ab1e | 126 | } |
Remi95 | 6:d61052f4ab1e | 127 | } |
Remi95 | 7:7d3c6326cbc3 | 128 | visu=opt.read_u16(); |
Remi95 | 7:7d3c6326cbc3 | 129 | visu=422006/(visu-3475); |
Remi95 | 10:cf2719d4b93f | 130 | //bth.printf("Measured range : %.2f cm\n",srf08.read()); |
Remi95 | 6:d61052f4ab1e | 131 | } |
Remi95 | 7:7d3c6326cbc3 | 132 | } |
Remi95 | 7:7d3c6326cbc3 | 133 |