boussole et pixi

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of TestBoard by IUT DE CACHAN GE1

Files at this revision

API Documentation at this revision

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

TestBoard.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
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;
+}