Diff: main.cpp

Revision:

9:9e8b83cf5083

Parent:

8:0977c3794f10

Child:

10:4e1c8b3edee7

diff -r 0977c3794f10 -r 9e8b83cf5083 main.cpp
--- 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);