Revision 8:527f5db9b3dc, committed 2017-06-09

Comitter:

illan

Date:

Fri Jun 09 02:09:09 2017 +0000

Parent:

7:075ba099309b

Commit message:

version test boussole et pixy

Changed in this revision

diff -r 075ba099309b -r 527f5db9b3dc TestBoard.lib
--- a/TestBoard.lib	Thu Jun 08 13:04:22 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/IUT-DE-CACHAN-GE1/code/TestBoard/#4955cb4b3646

diff -r 075ba099309b -r 527f5db9b3dc main.cpp
--- a/main.cpp	Thu Jun 08 13:04:22 2017 +0000
+++ b/main.cpp	Fri Jun 09 02:09:09 2017 +0000
@@ -233,7 +233,9 @@
 int                 FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
 int                 FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
 int                 Pixy_check = -1;
-
+char        BOUSSOLE_status[1] = {0};
+char        I2C_registerValue[4];
+    
 //  Dialogue avec la Pixy
 T_pixyCCData        Pixy_CCFIFO[20];
 T_pixyNMData        Pixy_NMFIFO[20];
@@ -242,10 +244,24 @@
 // Gestion des capteurs Ultrason
 long                Echo1Start, Echo2Start, Echo3Start;
 double              DistUS1, DistUS2, DistUS3;
+int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
+int         SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
+T_SERVODIR  SERVO_dir = S_monte;
+double      MOTG_duty = 0.5, MOTD_duty = 0.5;
+double      CAP_I2C, CAP_PWM;
 
 /** Liste des interruptions
  *
  */
+ 
+void capteur_ultrason(void);
+void boussole(void);
+void capteur_infrarouge(void);
+void CNY(void);
+void servomoteur(void);
+void avancer(void);
+void tour_droit(void);
+void tour_gauche(void);
 
 void tickTime()
 {
@@ -436,26 +452,25 @@
 int main()
 {
 
-    int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
+    int i = 0, val_H_balle = 0, val_W_balle=0, max_val_W_balle = 0, indice_balle = 0, val_carre_balle=0, val_x_balle=0;
 
     Byte        PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
     int         PIXY_objet;
 
-    int         SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
-    T_SERVODIR  SERVO_dir = S_monte;
+
 
     char        MENU_choix = 0;
 
-    char        BOUSSOLE_status[1] = {0};
-    char        I2C_registerValue[4];
-    double      BOUSSOLE_periode;
 
-    double      CAP_I2C, CAP_PWM;
-    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      BOUSSOLE_periode;
+    
+    double      Vbat_val;
+
+
+
 
     int         MOTG_evol = 1, MOTD_evol = 1;
-    double      MOTG_duty = 0.5, MOTD_duty = 0.5;
+    
 
     times.reset();
     times.start();
@@ -531,50 +546,11 @@
             case 1 :
                 Pc.printf ("\n\n\rTest des captreurs Ultrason\n");
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
-                do {
-
-                    if (FlagTickLed) {
-                        Led1 = !Led1;
-                        FlagTickLed = 0;
-                    }
-
-                    // Gestion des US
-                    if (((Tick%150)==0) && FlagTick) {
-                        Trig1 = 1;
-                        wait_us(20);
-                        Trig1 = 0;
-                        FlagTick = 0;
-                    }
-
-                    if (((Tick%150)==50) && FlagTick) {
-                        Trig2 = 1;
-                        wait_us(20);
-                        Trig2 = 0;
-                        FlagTick = 0;
-                    }
-
-                    if (((Tick%150)==100) && FlagTick) {
-                        Trig3 = 1;
-                        wait_us(20);
-                        Trig3 = 0;
-                        FlagTick = 0;
-                    }
-
-                    if (FlagUS1==1) {
-                        Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
-                        FlagUS1 = 0;
-                    }
-
-                    if (FlagUS2==1) {
-                        Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
-                        FlagUS2 = 0;
-                    }
-
-                    if (FlagUS3==1) {
-                        Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
-                        FlagUS3 = 0;
-                    }
-                } while(!Pc.readable());
+                do 
+                {
+                   capteur_ultrason();
+                }
+                while(!Pc.readable());
                 MENU_choix = Pc.getc();
                 break;
 
@@ -603,75 +579,30 @@
                 }
 
                 do {
-                    if (FlagTickLed) {
-                        Led1 = !Led1;
-                        FlagTickLed = 0;
-                    }
-
-                    if (((Tick%150)==0) && FlagTick) {
-                        FlagTick = 0;
-                        if (BOUSSOLE_check==0) {
-                            CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
-                            Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
-                        }
-                        if (I2C_check==0) {
-                            Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
-                            Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
-                            CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
-                            Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
-                        }
-                    }
+                 boussole();
+                 if(CAP_PWM<160)
+                 {
+                     tour_droit();
+                 }
+                 else if(CAP_PWM>200)
+                 {
+                     tour_gauche();
+                 }
+                 else
+                 {
+                     avancer();
+                 }
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
+                En=0;
                 break;
 
             case 3 :
                 Pc.printf ("\n\n\rGP2xx\n");
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                 do {
-                    if (FlagTickLed) {
-                        Led1 = !Led1;
-                        FlagTickLed = 0;
-
-                        SD1_val = SD1;
-                        SD2_val = SD2;
-                        LD1_val = LD1;
-                        LD2_val = LD2;
-
-                        if (SD1_val < 0.06) {
-                            SD1_val = 0;
-                            SD1_dist = 40;
-                        } else {
-                            SD1_dist = 11.611/(SD1_val*3.3-0.0237);
-                            if (SD1_dist > 40) SD1_dist = 40;
-                        }
-
-                        if (SD2_val < 0.06) {
-                            SD2_val = 0;
-                            SD2_dist = 40;
-                        } else {
-                            SD2_dist = 11.611/(SD2_val*3.3-0.0237);
-                            if (SD2_dist > 40) SD2_dist = 40;
-                        }
-
-                        if (LD1_val < 0.1) {
-                            LD1_val = 0;
-                            LD1_dist = 150;
-                        } else {
-                            LD1_dist = 59.175/(LD1_val*3.3-0.0275);
-                            if (LD1_dist > 150) LD1_dist = 150;
-                        }
-
-                        if (LD2_val < 0.1) {
-                            LD2_val = 0;
-                            LD2_dist = 150;
-                        } else {
-                            LD2_dist = 59.175/(LD2_val*3.3-0.0275);
-                            if (LD2_dist > 150) LD2_dist = 150;
-                        }
-
-                        Pc.printf ("\r SD1 = %3.1lf cm - SD2 = %3.1lf cm - LD1 = %4.1lf cm - LD2 = %4.1lf cm", SD1_dist, SD2_dist, LD1_dist, LD2_dist);
-                    }
+                    capteur_infrarouge();
+                    
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
                 break;
@@ -680,16 +611,7 @@
                 Pc.printf ("\n\n\rCNY70\n");
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                 do {
-                    if (FlagTickLed) {
-                        Led1 = !Led1;
-                        FlagTickLed = 0;
-
-                        CNY1_val = CNY1;
-                        CNY2_val = CNY2;
-                        CNY3_val = CNY3;
-
-                        Pc.printf ("\r CNY1 = %3.2lf V\t CNY2 = %3.2lf V\t CNY3 = %3.2lf V", CNY1_val*3.3, CNY2_val*3.3, CNY3_val*3.3);
-                    }
+                    CNY();
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
                 break;
@@ -816,33 +738,12 @@
                 Pc.printf ("\rAppuyez sur Entree pour quitter\n");
 
                 do {
-                    if (FlagTickLed) {
-                        Led1 = !Led1;
-                        FlagTickLed = 0;
-                    }
-
-                    if (((Tick%250)==0) && FlagTick) {
-                        FlagTick = 0;
-                        if (SERVO_dir == S_monte) {
-                            if (SERVO_pulseWidth < (SERVO_max - 100))   SERVO_pulseWidth +=100;
-                            else {
-                                SERVO_dir = S_descente;
-                                SERVO_pulseWidth = SERVO_max;
-                            }
-                        } else {
-                            if (SERVO_pulseWidth > (SERVO_min + 100))   SERVO_pulseWidth -=100;
-                            else {
-                                SERVO_dir = S_monte;
-                                SERVO_pulseWidth = SERVO_min;
-                            }
-                        }
-
-                        Servo.pulsewidth_us (SERVO_pulseWidth);
-                        Pc.printf ("\rPulse = %d",SERVO_pulseWidth);
-                    }
-
+                    servomoteur();
                 } while(!Pc.readable());
                 MENU_choix = Pc.getc();
+                
+                En = 0;
+                
                 break;
 
             case 9 :
@@ -873,9 +774,38 @@
                             if (PIXY_rCCObjet<19)   PIXY_rCCObjet++;
                             else                    PIXY_rCCObjet = 0;
                         }
+                        val_W_balle = 0;
+                        val_H_balle = 0;
+                        val_carre_balle = 0;
+                        max_val_W_balle = 0;
+                        indice_balle = 0;
+                        for(i=0;i<14;i++){
+                            val_W_balle=Pixy_NMFIFO[i].NMbloc.width;
+                            val_H_balle=Pixy_NMFIFO[i].NMbloc.height;
+                            
+                            val_carre_balle = val_H_balle/val_W_balle;
+                            
+                            val_x_balle =Pixy_NMFIFO[i].NMbloc.x;
+                            
+                            
+                                if(val_W_balle>max_val_W_balle){
+                                max_val_W_balle=val_W_balle;
+                                indice_balle=i;
+                                if(val_x_balle>130 && val_x_balle<170){
+                                    avancer();
+                                    }
+                                else{
+                                    En = 0;
+                                    }
+                                
+                                }
+                        }
+Pc.printf("\rNMobj sig = %hd : X=%5hd,Y=%5hd (W=%5hd, H=%5hd)\n",Pixy_NMFIFO[indice_balle].NMbloc.signature,Pixy_NMFIFO[indice_balle].NMbloc.x,Pixy_NMFIFO[indice_balle].NMbloc.y,Pixy_NMFIFO[indice_balle].NMbloc.width,Pixy_NMFIFO[indice_balle].NMbloc.height);
+
                         Pixy_CCObjet = 0;
                         Pc.printf("\n\r");
                         FlagPixy = 0;
+                        wait_ms(500);
                     }
 
                     if (FlagTickLed) {
@@ -888,4 +818,214 @@
 
         }
     }
-}
\ No newline at end of file
+}
+
+void capteur_ultrason(void)
+{
+                    if (FlagTickLed) {
+                    Led1 = !Led1;
+                    FlagTickLed = 0;
+                    }
+
+                    // Gestion des US
+                    if (((Tick%150)==0) && FlagTick) {
+                        Trig1 = 1;
+                        wait_us(20);
+                        Trig1 = 0;
+                        FlagTick = 0;
+                    }
+
+                    if (((Tick%150)==50) && FlagTick) {
+                        Trig2 = 1;
+                        wait_us(20);
+                        Trig2 = 0;
+                        FlagTick = 0;
+                    }
+
+                    if (((Tick%150)==100) && FlagTick) {
+                        Trig3 = 1;
+                        wait_us(20);
+                        Trig3 = 0;
+                        FlagTick = 0;
+                    }
+
+                    if (FlagUS1==1) {
+                        Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
+                        FlagUS1 = 0;
+                    }
+
+                    if (FlagUS2==1) {
+                        Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
+                        FlagUS2 = 0;
+                    }
+
+                    if (FlagUS3==1) {
+                        Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
+                        FlagUS3 = 0;
+                    }
+}
+
+void boussole(void)
+{
+     
+                   
+
+                    if (FlagTickLed) {
+                        Led1 = !Led1;
+                        FlagTickLed = 0;
+                    }
+
+                    if (((Tick%150)==0) && FlagTick) {
+                        FlagTick = 0;
+                        if (BOUSSOLE_check==0) {
+                            CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
+                            Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
+                        }
+                        if (I2C_check==0) {
+                            Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
+                            Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
+                            CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
+                            Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
+                        }
+                    }
+}
+
+void capteur_infrarouge(void)
+{
+    double      SD1_val, SD2_val, LD1_val, LD2_val; 
+    double      SD1_dist, SD2_dist, LD1_dist, LD2_dist;
+    
+                        if (FlagTickLed) {
+                        Led1 = !Led1;
+                        FlagTickLed = 0;
+
+                        SD1_val = SD1;
+                        SD2_val = SD2;
+                        LD1_val = LD1;
+                        LD2_val = LD2;
+
+                        if (SD1_val < 0.06) {
+                            SD1_val = 0;
+                            SD1_dist = 40;
+                        } else {
+                            SD1_dist = 11.611/(SD1_val*3.3-0.0237);
+                            if (SD1_dist > 40) SD1_dist = 40;
+                        }
+
+                        if (SD2_val < 0.06) {
+                            SD2_val = 0;
+                            SD2_dist = 40;
+                        } else {
+                            SD2_dist = 11.611/(SD2_val*3.3-0.0237);
+                            if (SD2_dist > 40) SD2_dist = 40;
+                        }
+
+                        if (LD1_val < 0.1) {
+                            LD1_val = 0;
+                            LD1_dist = 150;
+                        } else {
+                            LD1_dist = 59.175/(LD1_val*3.3-0.0275);
+                            if (LD1_dist > 150) LD1_dist = 150;
+                        }
+
+                        if (LD2_val < 0.1) {
+                            LD2_val = 0;
+                            LD2_dist = 150;
+                        } else {
+                            LD2_dist = 59.175/(LD2_val*3.3-0.0275);
+                            if (LD2_dist > 150) LD2_dist = 150;
+                        }
+
+                        Pc.printf ("\r SD1 = %3.1lf cm - SD2 = %3.1lf cm - LD1 = %4.1lf cm - LD2 = %4.1lf cm", SD1_dist, SD2_dist, LD1_dist, LD2_dist);
+                    }
+}
+
+void CNY(void)
+{
+    double CNY1_val, CNY2_val, CNY3_val;
+    
+                        if (FlagTickLed) {
+                        Led1 = !Led1;
+                        FlagTickLed = 0;
+
+                        CNY1_val = CNY1;
+                        CNY2_val = CNY2;
+                        CNY3_val = CNY3;
+
+                        Pc.printf ("\r CNY1 = %3.2lf V\t CNY2 = %3.2lf V\t CNY3 = %3.2lf V", CNY1_val*3.3, CNY2_val*3.3, CNY3_val*3.3);
+                    }
+}
+
+
+void servomoteur(void)
+{
+
+
+                    if (FlagTickLed) {
+                        Led1 = !Led1;
+                        FlagTickLed = 0;
+                    }
+
+                    if (((Tick%250)==0) && FlagTick) {
+                        FlagTick = 0;
+                        if (SERVO_dir == S_monte) {
+                            if (SERVO_pulseWidth < (SERVO_max - 100))   SERVO_pulseWidth +=100;
+                            else {
+                                SERVO_dir = S_descente;
+                                SERVO_pulseWidth = SERVO_max;
+                            }
+                        } else {
+                            if (SERVO_pulseWidth > (SERVO_min + 100))   SERVO_pulseWidth -=100;
+                            else {
+                                SERVO_dir = S_monte;
+                                SERVO_pulseWidth = SERVO_min;
+                            }
+                        }
+
+                        Servo.pulsewidth_us (SERVO_pulseWidth);
+                        Pc.printf ("\rPulse = %d",SERVO_pulseWidth);
+                    }
+}
+
+void avancer(void)
+{
+                En = 1;
+                SensD = 1;
+                Led2 = 1;
+                MOTD_duty = 0.48;
+                Pwm_MD = 0.5;  
+                SensG = 0;
+                MOTG_duty = 0.5;
+                Pwm_MG = 0.5;
+                
+                Pwm_MG = MOTG_duty;
+                Pwm_MD = MOTD_duty;
+}
+void tour_droit(void)
+{
+                En = 1;
+                SensD = 0;
+                Led2 = 1;
+                MOTD_duty = 0.5;
+                Pwm_MD = 0.5;  
+                SensG = 0;
+                MOTG_duty = 0.5;
+                Pwm_MG = 0.5;
+                
+                Pwm_MG = MOTG_duty;
+                Pwm_MD = MOTD_duty;
+}
+void tour_gauche(void)
+{
+                En = 1;
+                SensD = 1;
+                Led2 = 1;
+                MOTD_duty = 0.5;
+                Pwm_MD = 0.5;  
+                SensG = 1;
+                MOTG_duty = 0.5;
+                Pwm_MG = 0.5;
+                
+                Pwm_MG = MOTG_duty;
+                Pwm_MD = MOTD_duty;
+}