Revision 9:168226ff8f76, committed 2017-06-09

Comitter:

DOREL

Date:

Fri Jun 09 19:38:56 2017 +0000

Parent:

8:ad8b64ca548d

Commit message:

Moteurs

Changed in this revision

--- a/function_library.h	Fri Jun 09 17:06:05 2017 +0000
+++ b/function_library.h	Fri Jun 09 19:38:56 2017 +0000
@@ -272,4 +272,21 @@
         *LD2_dist = 59.175/(LD2_val*3.3-0.0275);
         if (*LD2_dist > 150) *LD2_dist = 150;
     }
+}
+
+void motor_command(int mg_command, int md_command, int* mg_pwm, int* md_pwm, int* mg_sens, int* md_sens) {
+    if(mg_command > 0) {
+        *mg_sens = 1;
+        *mg_pwm = mg_command;
+    } else {
+        *mg_sens = -1;
+        *mg_pwm = -mg_command;
+    }
+    if(md_command > 0) {
+        *mg_sens = 1;
+        *md_pwm = md_command;
+    } else {
+        *md_sens = -1;
+        *md_pwm = md_command;
+    }
 }
\ No newline at end of file

--- a/main.cpp	Fri Jun 09 17:06:05 2017 +0000
+++ b/main.cpp	Fri Jun 09 19:38:56 2017 +0000
@@ -12,7 +12,6 @@
 
 int main()
 {
-
     int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
 
     Byte        PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
@@ -31,15 +30,19 @@
     double      CNY1_val, CNY2_val, CNY3_val, Vbat_val;
     double      SD2_dist, LD2_dist;
 
-    int         MOTG_evol = 1, MOTD_evol = 1;
-    double      MOTG_duty = 0.5, MOTD_duty = 0.5;
-
     //Variables Automate
     int etat_actuel = 0, etat_futur = 0, etat_precedent = 0; //Need etat_precedent ?
     
     //Variables test capteurs
     int ball_found = 0, ball_forward = 0, ball_captured = 0, face_ennemy_side = 0, face_our_side = 0;
     int wall = 0, white_line_crossed = 0, return_done = 0, rotation_done; //Rename white_line_crossed
+    
+    //Variables moteurs
+    
+    int md_pwm = 0;
+    int mg_pwm = 0;
+    int md_sens = 0;
+    int mg_sens = 0;
 
     //Variables
     
@@ -97,12 +100,48 @@
     Servo.period_ms (20);
     Servo.pulsewidth_us(200);
     
-    while(1) {
-
+    //Moteurs
+    En = 1;    
+    
+    while(1) {    
+        int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;
+    
+        Byte        PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
+        int         PIXY_objet;
+    
+        int         SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
+        T_SERVODIR  SERVO_dir = S_monte;
+    
+        char        MENU_choix = 0;
+    
+        char        BOUSSOLE_status[1] = {0};
+        char        I2C_registerValue[4];
+        double      BOUSSOLE_periode;
+    
+        double      CAP_I2C, CAP_PWM;
+        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;     
+        
+        
         //capteurs_read(); //Lecture tous les capteurs
         CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
-        Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
+        
         //capteurs_test(); //Teste les valeurs des capteurs et change des variables booleenne 
+        face_ennemy_side = (((int)CAP_PWM > (orientation_camp_adversaire - 10))&((int)CAP_PWM < (orientation_camp_adversaire + 10))); 
+        
+        //Actions moteurs
+        Pwm_MD = md_pwm;
+        Pwm_MG = mg_pwm;
+        SensD = md_sens;
+        SensG = mg_sens;
+        
+        //Test
+        
+        motor_command(100, 100, *mg_pwm, *md_pwm, *mg_sens, *md_sens);
+        
+        //Action cage
+        
+        //Automate
         etat_actuel = etat_futur;
         
         switch(etat_actuel) {