![](/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:
- 10:4e1c8b3edee7
- Parent:
- 9:9e8b83cf5083
diff -r 9e8b83cf5083 -r 4e1c8b3edee7 main.cpp --- a/main.cpp Sat Jun 10 05:49:03 2017 +0000 +++ b/main.cpp Sat Jun 10 12:23:42 2017 +0000 @@ -434,18 +434,18 @@ 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); + *mg_sens = 1; + *mg_pwm = 1-((float)(mg_command%101)/100); } else { - *mg_sens = 1; - *mg_pwm = ((-mg_command%101)/100); + *mg_sens = 0; + *mg_pwm = ((float)(-mg_command%101)/100); } if(md_command >= 0) { - *mg_sens = 0; - *md_pwm = ((md_command%101)/100); + *md_sens = 0; + *md_pwm = ((float)(md_command%101)/100); } else { *md_sens = 1; - *md_pwm = ((-md_command%101)/100); + *md_pwm = 1-((float)(-md_command%101)/100); } } @@ -475,10 +475,10 @@ int md_sens = 1; int mg_sens = 1; - int i = 0, val_H_balle = 0, val_W_balle=0, val_max=0; + int i = 0; float val_carre_balle=0, val_x_balle=0; - float erreur_asservissement, commande_moteur = 10, mesure_angle_pixy; + float erreur_asservissement, commande_moteur = 30, mesure_angle_pixy; times.reset(); times.start(); @@ -529,13 +529,32 @@ // Led Led2 = 0; + En = 1; + Servo.period_ms (20); - Servo.pulsewidth_us(200); + Servo.pulsewidth_us(200); + + Pc.printf("test1\n\r"); + + motor_command(60, -60, &mg_pwm, &md_pwm, &mg_sens, &md_sens); + Pwm_MD = md_pwm; + Pwm_MG = mg_pwm; + SensD = md_sens; + SensG = mg_sens; + wait_ms(3000); + + Pc.printf("Pwm_MG = %lf, Pwm_MD = %lf, SensG = %d, SensD = %d\r\n", mg_pwm, md_pwm, mg_sens, md_sens); + Pwm_MD = 0; + Pwm_MG = 0; + SensD = 0; + SensG = 0; + while(1) { + /* val_x_balle = 0; i = 0; - while ((val_x_balle==0) && (i>19)) { + 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; @@ -546,5 +565,11 @@ 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); + mesure_angle_pixy; + Pwm_MD = md_pwm; + Pwm_MG = mg_pwm; + SensD = md_sens; + SensG = mg_sens; + */ } } \ No newline at end of file