Diff: main.cpp

Revision:

5:4955cb4b3646

Parent:

4:f65b0905e4ce

Child:

6:88b4805d33e1

--- a/main.cpp	Wed May 31 10:10:56 2017 +0000
+++ b/main.cpp	Sun Jun 04 15:15:19 2017 +0000
@@ -152,7 +152,7 @@
  */
 
 Serial      Pixy    (PA_0, PA_1, 230400);
-Serial      Pc      (PA_2, PA_3, 460800);
+Serial      Pc      (PA_2, PA_3, 921600);
 
 AnalogIn    CNY1    (PC_4);
 AnalogIn    CNY2    (PA_7);
@@ -398,7 +398,7 @@
 {
 
     int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
-    
+
     Byte        PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
     int         PIXY_objet;
 
@@ -416,6 +416,8 @@
     double      SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
     double      SD1_dist, SD2_dist, LD1_dist, LD2_dist;
 
+    double      MOTG_duty = 0, MOTD_duty = 0;
+
     times.reset();
     times.start();
 
@@ -442,11 +444,9 @@
 
     Pwm_MG.period_us(50);
     Pwm_MD.period_us(50);
-    Pwm_MG = 0;
-    Pwm_MD = 0;
+
     En = 0;
-    SensG = 0;
-    SensD = 0;
+
     Led2 = 0;
 
     Servo.period_ms(20);
@@ -654,26 +654,69 @@
             case 6 :
                 Pc.printf ("\n\n\rMoteur Gauche\n");
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
+
+                En = 1;
+                SensG = 1;
+                MOTG_duty = 0.5;
+                Pwm_MG = 0.5;
+                
                 do {
                     if (FlagTickLed) {
                         Led1 = !Led1;
                         FlagTickLed = 0;
                     }
+
+                    if (BPPressed) {
+                        BPPressed = 0;
+
+                        if (!SensG) {
+                            SensG = 1;
+                        } else {
+                            SensG = 0;
+                        }
+                        Pwm_MG = MOTG_duty;
+                    }
+                    
+                    Pc.printf ("\rPos = %ld Tick", Gauche.GetCounter());
+                    
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
+                En = 0;
+                if (SensG == 0) Pwm_MG = 0;
+                else            Pwm_MG = 1;
                 break;
 
             case 7 :
                 Pc.printf ("\n\n\rMoteur Droit\n");
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
+
+                En = 1;
+                SensD = 1;
+                MOTD_duty = 0.5;
+                Pwm_MD = 0.5;
+
                 do {
                     if (FlagTickLed) {
                         Led1 = !Led1;
                         FlagTickLed = 0;
-                        wait (0.1);
                     }
+                    if (BPPressed) {
+                        BPPressed = 0;
+                        if (SensD) {
+                            SensD = 0;
+                        } else {
+                            SensD = 1;
+                        }
+                        Pwm_MD = MOTD_duty;
+                    }
+                    
+                    Pc.printf ("\rPos = %ld Tick", Droite.GetCounter());
+                    
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
+                En = 0;
+                if (SensD == 0) Pwm_MD = 0;
+                else            Pwm_MD = 1;
                 break;
 
             case 8 :