Diff: main.cpp

Revision:

8:7b0fa8a914c0

Parent:

7:6d58adc26b78

Child:

9:234439133426

--- a/main.cpp	Tue Jun 05 13:55:04 2018 +0000
+++ b/main.cpp	Thu Jun 06 12:34:37 2019 +0000
@@ -1,18 +1,17 @@
 #include "mbed.h"
 #include "CMPS03.h"
 #include "CNY70.h"
-#include "GP2A.h"
-#include "RC_Servo.h"
 #include "VMA306.h"
 #include "Pixy.h"
 #include "PID.h"
 
+
 #define PI  3.1415926535898
 
 #define NSpeed  100
 
 Serial      pc          (PA_2, PA_3, 921600);
-PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+PID         motor       (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);
 
 CMPS03      boussole    (PC_4);
 
@@ -20,13 +19,8 @@
 CNY70       ligneG      (PC_2);
 CNY70       exterior    (PA_7);
 
-GP2A        ld1         (PA_4, 20, 150, 55);
-GP2A        sd1         (PB_0, 5, 80, 10);
 
-RC_Servo    ballon      (PB_10, 0);
-RC_Servo    verrou      (PA_15, 0);
-
-VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
+VMA306      UltraSon    (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6);
 
 PIXY        Pixy        (PA_0, PA_1, 230400);
 
@@ -34,25 +28,37 @@
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
-DigitalOut  disquette   (PA_12);
 
 main ()
 {
-    int                 etatmvt = 0;
+    led1 = 0;
+    led2 = 0;
+    pc.printf ("\n\rHelloWorld\n");
+    wait(0.5);
+    led1 = 1;
+    led2 = 0;
+    
+    int                 etatmvt = 00;
     int                 sens = 1, nbiter = 0, nbCC, nbNM;
     double              pos = 0.5;
     T_pixyCCBloc        CCBuf;
     T_pixyNMBloc        NMBuf;
     double              x, y, theta, vG, vD;
 
-    pc.printf ("\n\rHelloWorld\n");
+    pc.printf ("\n\rinit\n");
     led1 = 1;
     led2 = 0;
-    disquette = 0;
     
-    //motor.resetPosition();
+    motor.resetPosition();
 
-    wait (5);
+    pc.printf ("\n\r3\n");
+    wait (1);
+    pc.printf ("\n\r2\n");
+    wait (1);
+    pc.printf ("\n\r1\n");
+    wait (1);
+    
+    motor.setSpeed (0,0);
 
     while (1) {
         pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());
@@ -66,22 +72,6 @@
 
         pc.printf ("\r%4.3lf\t %4.3lf\t %4.3lf\n", exterior.getVoltage(), ligneG.getVoltage(), ligneD.getVoltage());
 
-        pc.printf("\rGP2 longue distance = %5.2f\n",ld1.getDistance ());
-        pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ());
-
-        pc.printf("\rpos = %2.1lf\n",pos);
-        ballon.write (pos);
-        verrou.write (pos);
-        nbiter++;
-        if (nbiter%5==0) {
-            if (sens) {
-                pos += 0.1;
-                if (pos>0.9) sens = 0;
-            } else {
-                pos-=0.1;
-                if (pos<0.1) sens = 1;
-            }
-        }
 
         if (UltraSon.isUSGReady())     pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
         if (UltraSon.isUSBReady())     pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
@@ -108,7 +98,7 @@
         motor.getSpeed (&vG, &vD);
 
         pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD);
-
+/*
         switch (etatmvt) {
             case 0 :
                 // On avance de 50cm (coordonnés X = 500, Y = 0)
@@ -167,10 +157,10 @@
                 }
                 break;
         }
-
+*/
         pc.printf ("\n\n");
         led1 = !led1;
         led2 = !led2;
-        wait (0.2);
+        wait (1);
     }
 }
\ No newline at end of file