Tomato Ketchanonimous
Test moteur
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of
TestBoardv2_boussole_pixi
by 
ilan Sandoz
Diff: main.cpp
- Revision:
- 8:527f5db9b3dc
- Parent:
- 6:88b4805d33e1
- Child:
- 9:53dd6df76cf8
--- 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;
+}