Diff: main.cpp

Revision:

0:c3b54b05bf77

diff -r 000000000000 -r c3b54b05bf77 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 06 12:48:02 2018 +0000
@@ -0,0 +1,253 @@
+#include "mbed.h"
+#include "Pixy.h"
+#include "PID.h"
+
+Serial          pc              (PA_2, PA_3, 921600);
+
+PID             motor           (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+
+InterruptIn     boussole        (PC_4);
+
+AnalogIn        CNY70ligneD     (PC_3);
+AnalogIn        CNY70ligneG     (PC_2);
+AnalogIn        CNY70exterior   (PA_7);
+
+AnalogIn        GP2sensorL      (PA_4);
+AnalogIn        GP2sensorS      (PB_0);
+
+DigitalOut      trig1           (PB_15);
+InterruptIn     echo1           (PA_6);
+DigitalOut      trig2           (PB_14);
+InterruptIn     echo2           (PC_7);
+DigitalOut      trig3           (PB_13);
+InterruptIn     echo3           (PB_2);
+
+//PwmOut          ballon          (PB_10);
+//PwmOut          verrou          (PA_15);
+
+PIXY            pixy            (PA_0, PA_1, 230400);
+
+DigitalIn       bp              (PC_13);
+
+DigitalOut      led1            (PA_5);
+DigitalOut      led2            (PD_2);
+DigitalOut      disquette       (PA_12);
+
+Timer           temps;
+Ticker          tick;
+
+bool            CMPS03flag, VMA306Flag1, VMA306Flag2, VMA306Flag3;
+long            CMPS03startTime, VMA306startTime1, VMA306startTime2, VMA306startTime3;
+double          CMPS03bearing, VMA306dist1, VMA306dist2, VMA306dist3;
+double          _Kp, _Ki, _Kd;
+double          _SpeedG, _SpeedD;
+double          _measDistG, _measDistD;
+int             _WheelStuckG, _WheelStuckD;
+double          _X, _Y, _THETA;
+
+void VMA306rise1(void)
+{
+    VMA306startTime1 = temps.read_us();
+}
+
+void VMA306fall1(void)
+{
+    VMA306dist1 = (double)(temps.read_us() - VMA306startTime1)/58.0;
+    VMA306Flag1 = 1;
+}
+
+// Clear and start the timer at the begining of the echo pulse
+void VMA306rise2(void)
+{
+    VMA306startTime2 = temps.read_us();
+}
+
+// Stop and read the timer at the end of the pulse
+void VMA306fall2(void)
+{
+    VMA306dist2 = (double)(temps.read_us() - VMA306startTime2)/58.0;
+    VMA306Flag2 = 1;
+}
+
+// Clear and start the timer at the begining of the echo pulse
+void VMA306rise3(void)
+{
+    VMA306startTime3 = temps.read_us();
+}
+
+// Stop and read the timer at the end of the pulse
+void VMA306fall3(void)
+{
+    VMA306dist3 = (double)(temps.read_us() - VMA306startTime3)/58.0;
+    VMA306Flag3 = 1;
+}
+
+void CMPS03rise (void)
+{
+    CMPS03startTime = temps.read_us();
+}
+
+void CMPS03fall (void)
+{
+    CMPS03bearing = ((double)(temps.read_us() - CMPS03startTime - 1000)/100.0);
+}
+
+double getGP2Distance (double sensorValue, double slope)
+{
+    return  slope/(sensorValue * 3.3);
+}
+
+bool whatIsUnderMyCNY (double sensorValue)
+{
+    return (sensorValue > 0.5) ? 0 : 1;
+}
+
+void baisser (PwmOut *ptr)
+{
+    ptr->pulsewidth_ms(2);
+}
+
+void lever (PwmOut *ptr)
+{
+    ptr->pulsewidth_ms(1);
+}
+
+
+main ()
+{
+    int                 nbCC, nbNM, objet = 0;
+    T_pixyCCBloc        CCBuf[20];
+    T_pixyNMBloc        NMBuf[20];
+    double              topDepart, capAuDepart;
+    double              x, y, theta;
+    double              speedG = 1000, speedD = 1000;
+    PwmOut              *ptr;
+
+    boussole.rise       (&CMPS03rise);
+    boussole.fall       (&CMPS03fall);
+
+    echo1.rise          (&VMA306rise1);
+    echo1.fall          (&VMA306fall1);
+    echo2.rise          (&VMA306rise2);
+    echo2.fall          (&VMA306fall2);
+    echo3.rise          (&VMA306rise3);
+    echo3.fall          (&VMA306fall3);
+
+    temps.start();
+
+/*    ptr = new PwmOut(PB_10);
+    ptr->period_ms(20);
+    baisser(ptr);
+    wait (2);
+    delete ptr;
+    
+    ptr = new PwmOut(PA_15);
+    ptr->period_ms(20);
+    lever(ptr);
+    wait (2);
+    delete ptr;*/
+
+    pc.printf ("\n\rHelloWorld\n");
+    led1 = 1;
+    led2 = 0;
+    disquette = 0;
+    motor.resetPosition();
+    wait(3);
+    
+    pc.printf("\rcheck Pixy\n");
+    if (pixy.checkPixy()) led2=1;
+    else {
+        pc.printf("\rLa pixy ne parle pas\n");
+        while(1);
+    }
+    pc.printf("\rApprochez une balle\n");
+    do {
+        pixy.detectedObject(&nbNM, &nbCC);
+    } while(nbNM!=1);
+    pc.printf("\rOK !\n");
+    led1 = 0;
+
+    pc.printf("\rApprochez une balise\n");
+    do {
+        pixy.detectedObject(&nbNM, &nbCC);
+    } while(nbCC!=1);
+    pc.printf("\rOK !\n");
+    led2 = 0;
+
+    pc.printf("\n\rcheck Moteur\n");
+    while(bp.read());
+    led1 = 1;
+
+    //Avance
+    motor.setSpeed (80,80);
+    do {
+        motor.getPosition (&x,&y, &theta);
+    } while (x<50);
+    motor.setSpeed (0,0);
+    wait_ms(10);
+
+    //Recule
+    motor.setSpeed (-80,-80);
+    do {
+        motor.getPosition (&x,&y, &theta);
+    } while (x>5);
+    motor.setSpeed (0,0);
+    wait_ms(10);
+
+    //Tourne à droite
+    motor.setSpeed (80,-80);
+    do {
+        motor.getPosition (&x,&y, &theta);
+    } while (theta > -PI/2.0);
+
+    motor.setSpeed (0,0);
+    pc.printf("\rMoteurs OK\n");
+    while (bp.read());
+    
+    pc.printf("\rScan de la zone\n");
+
+    motor.setSpeed (-50,50);
+    do {
+        pixy.detectedObject(&nbNM, &nbCC);
+    } while (nbNM != 1);
+
+    pc.printf("\rBalle trouvee\n");
+    pc.printf("\rAlignement\n");
+
+    motor.setSpeed (-30,30);
+    do {
+        pixy.detectedObject(&nbNM, &nbCC);
+        if (nbNM==1) {
+            NMBuf[objet] = pixy.getNMBloc ();
+            nbNM--;
+        }
+    } while (NMBuf[objet].x != 160);
+
+    pc.printf("\rBalle alignee\n");
+    pc.printf("\n\rREADY TO GO\n");
+
+    do {
+        led1 = !led1;
+        wait(0.1);
+    } while (bp);
+
+    topDepart = temps.read();
+    capAuDepart = CMPS03bearing;
+
+    while (1) {
+        do {
+            motor.setSpeed (speedG, speedD);
+            pixy.detectedObject(&nbNM, &nbCC);
+            if (nbNM == 1) {
+                NMBuf[0] = pixy.getNMBloc ();
+                nbNM--;
+            }
+            speedG = 1000 - 10*(double)(NMBuf[0].x - 160);
+            speedD = 1000 - 10*(double)(NMBuf[0].x + 160);
+        } while (NMBuf[0].y < 170);
+        
+        
+
+
+    }
+}
\ No newline at end of file