Diff: main.cpp

Revision:

7:7d3c6326cbc3

Parent:

6:d61052f4ab1e

Child:

8:ea8db9aacdfb

--- 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());
     }
-}    
+}
+