Revision 10:4e1c8b3edee7, committed 2017-06-10

Comitter:

DOREL

Date:

Sat Jun 10 12:23:42 2017 +0000

Parent:

9:9e8b83cf5083

Commit message:

Motors work;

Changed in this revision

--- 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