Diff: main.cpp

Revision:

3:0221daeeaa86

Parent:

2:e1372e7b671f

Child:

4:5038b4cd1088

--- a/main.cpp	Wed May 23 15:11:02 2018 +0000
+++ b/main.cpp	Thu May 31 17:47:43 2018 +0000
@@ -2,49 +2,98 @@
 #include "CMPS03.h"
 #include "CNY70.h"
 #include "GP2A.h"
-#include "PID.h"
-#include "Pixy.h"
 #include "RC_Servo.h"
 #include "VMA306.h"
+#include "Pixy.h"
 
-main ()
-{
-Serial      pc          (USBTX, USBRX, 921600);
-
-CMPS03      boussole    (PC_4, PB_9, PB_8);
+Serial      pc          (PA_2, PA_3, 921600);
 
-CNY70       ligneG      (PC_3);
-CNY70       ligneD      (PC_2);
-CNY70       mediane     (PA_7);
+CMPS03      boussole    (PC_4);
 
-GP2A        ld1         (PC_0, 20, 150, 60);
-GP2A        sd1         (PB_0, 7, 80, 20.88);
-GP2A        ld2         (PC_1, 20, 150, 60);
-GP2A        sd2         (PA_4, 7, 80, 20.88);
+CNY70       ligneD      (PC_3);
+CNY70       ligneG      (PC_2);
+CNY70       exterior    (PA_7);
 
-PID         motG        (TIM4, PA_9, PC_9, PC_8);
-PID         motD        (TIM3, PA_8, PC_6, PC_5);
-
-PIXY        pixy        (PA_1, PA_0);
+GP2A        ld1         (PA_4, 20, 150, 55);
+GP2A        sd1         (PB_0, 5, 80, 10);
 
-VMA306      us_G        (PB_15, PA_6);
-VMA306      us_B        (PB_14, PC_7);
-VMA306      us_D        (PB_13, PB_2);
+RC_Servo    ballon      (PB_10, 0);
+RC_Servo    verrou      (PA_15, 0);
 
-AnalogIn    vbat        (PB_1);
+VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
-DigitalIn   jack        (PC_13);
-
-InterruptIn Vg          (PA_11);
-InterruptIn Vd          (PA_10);
+PIXY        Pixy        (PA_0, PA_1, 230400);
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
 DigitalOut  disquette   (PA_12);
 
-RC_Servo    ballon      (PB_10, 0);
-RC_Servo    verrou      (PA_15, 0);
+main ()
+{
+    int                 sens = 1, nbiter = 0, nbCC, nbNM;
+    double              pos = 0.5;
+    T_pixyCCBloc        CCBuf;
+    T_pixyNMBloc        NMBuf;
+
+    pc.printf ("\n\rHelloWorld\n");
+    led1 = 1;
+    led2 = 0;
+    disquette = 0;
+
+    wait (5);
+
+    while (1) {
+        pc.printf ("\rCap = %5.2lf\n", boussole.getBearing());
+
+        if (exterior.whatAmIOn())   pc.printf("\rwhite");
+        else                        pc.printf("\rblue ");
+        if (ligneG.whatAmIOn())     pc.printf("\r\twhite");
+        else                        pc.printf("\r\tblue ");
+        if (ligneD.whatAmIOn())     pc.printf("\r\t\twhite\n");
+        else                        pc.printf("\r\t\tblue\n");
+
+        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 ("HelloWorld");
+        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());
+        if (UltraSon.isUSDReady())     pc.printf ("\r\t\t\t\t usD = %5.2lf", UltraSon.readUSD());
+        pc.printf ("\n");
 
+        if (Pixy.checkNewImage()) {
+            Pixy.detectedObject (&nbNM, &nbCC);
+            pc.printf ("\rnbCC = %2d - nbNM = %2d\n", nbCC, nbNM);
+            while (nbCC > 0) {
+                CCBuf = Pixy.getCCBloc ();
+                nbCC--;
+                pc.printf ("\rCC %5d : x=%5d, y=%5d - w=%5d, h=%5d, a=%5d\n", CCBuf.signature, CCBuf.x, CCBuf.y, CCBuf.width, CCBuf.height, (short)CCBuf.angle);
+            }
+            while (nbNM > 0) {
+                NMBuf = Pixy.getNMBloc ();
+                nbNM--;
+                pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
+            }
+        }
+
+        pc.printf ("\n");
+        led1 = !led1;
+        led2 = !led2;
+        wait (0.2);
+    }
 }
\ No newline at end of file