Jonas DOREL
Asservissement
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of
TestBoard
by 
IUT DE CACHAN GE1
Diff: main.cpp
- Revision:
- 8:0977c3794f10
- Parent:
- 6:88b4805d33e1
- Child:
- 9:9e8b83cf5083
diff -r 075ba099309b -r 0977c3794f10 main.cpp
--- a/main.cpp Thu Jun 08 13:04:22 2017 +0000
+++ b/main.cpp Sat Jun 10 05:39:15 2017 +0000
@@ -456,6 +456,9 @@
int MOTG_evol = 1, MOTD_evol = 1;
double MOTG_duty = 0.5, MOTD_duty = 0.5;
+
+ int i = 0, val_H_balle = 0, val_W_balle=0, val_max=0;
+ float val_carre_balle=0, val_x_balle=0, measure_angle_pixy=0;
times.reset();
times.start();
@@ -510,382 +513,17 @@
Servo.pulsewidth_us(200);
while(1) {
-
- do {
- Led1 = 0;
- Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n");
- Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n");
- Pc.printf ("\r2- Boussole et I2C\n");
- Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
- Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
- Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n");
- Pc.printf ("\r6- Moteur Gauche\n");
- Pc.printf ("\r7- Moteur Droit\n");
- Pc.printf ("\r8- Servomoteur\n");
- Pc.printf ("\r9- PIXY (CMUCAM5)\n");
- MENU_choix = Pc.getc ();
- } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));
-
- switch (MENU_choix-'0') {
-
- 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());
- MENU_choix = Pc.getc();
- break;
-
- case 2 :
- Pc.printf ("\n\n\rBoussole\n");
- Pc.printf ("\rAppuyez sur Entree pour quitter\n");
-
- Pc.printf ("\n\rVerif du bus I2C :");
- I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false);
- if (I2C_check==0) {
- Pc.printf (" OK\n");
- Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
- Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
- Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]);
- } else {
- Pc.printf (" FAIL\n");
- }
-
- BOUSSOLE_periode = PWMB.period();
- Pc.printf ("\rVerif de la PWM :");
- if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) {
- Pc.printf (" FAIL\n\n");
- } else {
- Pc.printf (" OK\n\n");
- BOUSSOLE_check = 0;
- }
-
- 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);
- }
- }
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- 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);
- }
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- break;
-
- case 4 :
- 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);
- }
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- break;
-
- case 5 :
- Pc.printf ("\n\n\rVbat\n");
- Pc.printf ("\rAppuyez sur Entree pour quitter\n");
- do {
- if (FlagTickLed) {
- Led1 = !Led1;
- FlagTickLed = 0;
-
- Vbat_val = Vbat;
- Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3);
- }
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- break;
-
- case 6 :
- Pc.printf ("\n\n\rMoteur Gauche\n");
- Pc.printf ("\rAppuyez sur Entree pour quitter\n");
-
- En = 1;
- SensG = 1;
- Led2 = 1;
- MOTG_duty = 0.5;
- MOTG_evol = 1;
- Pwm_MG = 0.5;
-
- do {
- if (FlagTickLed) {
- Led1 = !Led1;
- FlagTickLed = 0;
- }
- if (BPPressed) {
- BPPressed = 0;
- SensG = !SensG;
- Led2 = !Led2;
- }
-
- if (((Tick%1000)==0) && FlagTick) {
- FlagTick = 0;
- if (MOTG_evol) {
- if (MOTG_duty < 0.9) MOTG_duty += 0.1;
- else {
- MOTG_evol = 0;
- MOTG_duty = 0.9;
- }
- } else {
- if (MOTG_duty > 0.1) MOTG_duty -= 0.1;
- else {
- MOTG_evol = 1;
- MOTG_duty = 0.1;
- }
- }
- }
-
- Pwm_MG = MOTG_duty;
- Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod);
-
-
- } 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;
- Led2 = 1;
- MOTD_duty = 0.5;
- MOTD_evol = 1;
- Pwm_MD = 0.5;
-
- do {
- if (FlagTickLed) {
- Led1 = !Led1;
- FlagTickLed = 0;
- }
- if (BPPressed) {
- BPPressed = 0;
- SensD = !SensD;
- Led2 = !Led2;
- }
-
- if (((Tick%1000)==0) && FlagTick) {
- FlagTick = 0;
- if (MOTD_evol) {
- if (MOTD_duty < 0.9) MOTD_duty += 0.1;
- else {
- MOTD_evol = 0;
- MOTD_duty = 0.9;
- }
- } else {
- if (MOTD_duty > 0.1) MOTD_duty -= 0.1;
- else {
- MOTD_evol = 1;
- MOTD_duty = 0.1;
- }
- }
- }
- Pwm_MD = MOTD_duty;
- Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod);
-
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
-
- En = 0;
- if (SensD == 0) Pwm_MD = 0;
- else Pwm_MD = 1;
-
- break;
-
- case 8 :
- Pc.printf ("\n\n\rServo Moteur\n");
- 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);
- }
-
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- break;
-
- case 9 :
- Pc.printf ("\n\n\rPixy\n");
- Pc.printf ("\rAppuyez sur Entree pour quitter\n");
-
- if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n");
- else Pc.printf ("\n\rPIXY don't talk\n");
-
- do {
- if (FlagPixy) {
-
- if (FlagPixyOverflow) {
- Pc.printf ("\rSome Data were lost\n");
- FlagPixyOverflow = 0;
- }
- for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) {
- Pc.printf ("\rNMobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_NMObjet, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.signature, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.x, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.y, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.width, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.height);
-
- if (PIXY_rNMObjet<19) PIXY_rNMObjet++;
- else PIXY_rNMObjet = 0;
- }
- Pixy_NMObjet = 0;
-
- for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) {
- Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_CCObjet, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.signature, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.x, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.y, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.width, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.height);
-
- if (PIXY_rCCObjet<19) PIXY_rCCObjet++;
- else PIXY_rCCObjet = 0;
- }
- Pixy_CCObjet = 0;
- Pc.printf("\n\r");
- FlagPixy = 0;
- }
-
- if (FlagTickLed) {
- Led1 = !Led1;
- FlagTickLed = 0;
- }
- } while(!Pc.readable());
- MENU_choix = Pc.getc();
- break;
-
- }
+ i=0;
+ do {
+ //test carre +/-5%
+ val_carre_balle = (float)Pixy_NMFIFO[i].NMbloc.height/(float)Pixy_NMFIFO[i].NMbloc.width;
+ if ((0.85<val_carre_balle) && (val_carre_balle<1.15)) val_x_balle =Pixy_NMFIFO[i].NMbloc.x+Pixy_NMFIFO[i].NMbloc.width/2;
+ i++;
+ } while((val_x_balle==0) && (i>19));
+ measure_angle_pixy=(val_x_balle-160)*(37.5/160);
+ Pc.printf("%lf %lf\r\n", val_x_balle, measure_angle_pixy);
+ wait_us(50000);
+ erreur_asservissement = 2 * (-mesure_angle_pixy);
+ motor_command(commande_moteur+erreur_asservissement, commande_moteur-erreur_asservissement, &mg_pwm, &md_pwm, &mg_sens, &md_sens);
}
}
\ No newline at end of file