![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Asservissement
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of TestBoard by
Diff: main.cpp
- Revision:
- 9:9e8b83cf5083
- Parent:
- 8:0977c3794f10
- Child:
- 10:4e1c8b3edee7
--- a/main.cpp Sat Jun 10 05:39:15 2017 +0000 +++ b/main.cpp Sat Jun 10 05:49:03 2017 +0000 @@ -432,6 +432,22 @@ } } +void motor_command(int mg_command, int md_command, float* mg_pwm, float* md_pwm, int* mg_sens, int* md_sens) { + if(mg_command >= 0) { + *mg_sens = 0; + *mg_pwm = ((mg_command%101)/100); + } else { + *mg_sens = 1; + *mg_pwm = ((-mg_command%101)/100); + } + if(md_command >= 0) { + *mg_sens = 0; + *md_pwm = ((md_command%101)/100); + } else { + *md_sens = 1; + *md_pwm = ((-md_command%101)/100); + } +} int main() { @@ -454,11 +470,15 @@ 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; - int MOTG_evol = 1, MOTD_evol = 1; - double MOTG_duty = 0.5, MOTD_duty = 0.5; + float md_pwm = 0; + float mg_pwm = 0; + int md_sens = 1; + int mg_sens = 1; 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; + float val_carre_balle=0, val_x_balle=0; + + float erreur_asservissement, commande_moteur = 10, mesure_angle_pixy; times.reset(); times.start(); @@ -513,15 +533,16 @@ Servo.pulsewidth_us(200); while(1) { - i=0; - do { + val_x_balle = 0; + i = 0; + while ((val_x_balle==0) && (i>19)) { //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); + } + mesure_angle_pixy=(val_x_balle-160)*(37.5/160); + Pc.printf("%lf %lf\r\n", val_x_balle, mesure_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);