Diff: main.cpp

Revision:

11:75b6dde00307

Parent:

10:1675ab60cf43

Parent:

9:043f73bf5020

Child:

12:7d61181d7ac7

--- a/main.cpp	Mon Jun 08 12:42:12 2020 +0000
+++ b/main.cpp	Mon Jun 08 12:44:59 2020 +0000
@@ -4,8 +4,8 @@
 DigitalOut alivenessLED(LED1);
 DigitalOut testLed(LED2);
 
-Serial coach(D1, D0);
-/* Serial coach(USBTX, USBRX); */
+/*Serial coach(D1, D0);*/
+Serial coach(USBTX, USBRX);
 Serial myserial_ax12(PC_4,PC_5,115200);                                         // Modifier le baud rate en fonction de votre utilisation des moteurs
 
 const float rapport_vitesse = 0.019383809*0.026315;
@@ -152,7 +152,7 @@
     float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
     float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
     /*moteur a = ID 2 */
-    SetCRSpeed(2,wa);
+    SetCRSpeed(2,0.05);
     /*moteur b = ID 1*/
     SetCRSpeed(1,wb);
     /* moteur c = ID 3*/
@@ -232,7 +232,7 @@
 
 int main()
 {
-    
+
     coach.baud(115200);
     coach.puts("slt\n\r");
     coach.attach(&callback_serial, Serial::RxIrq);
@@ -243,7 +243,8 @@
     //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
     //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};
 
-    //pour regarder les 3 moteurs les uns à la suite des autres// Bonjour Leopled
+    //pour regarder les 3 moteurs les uns à la suite des autres
+
     int etat=1;
     int fm_etat=1;
 
@@ -285,10 +286,17 @@
     ticker_1ms.attach(&fonction_1ms,0.001);
     etat=2;
 
+    coach.printf("fin_initialisation");
+    position_1 = 0;
+    position_2 = 0;
+    position_3 = 0;
+    position_1_old = 0;
+    position_2_old = 0;
+    position_3_old = 0;
+    delta_position_1 = 0;
+    delta_position_2 = 0;
+    delta_position_3 = 0;
     while (true) {
-        reception_com();
-        asser(front, lat, rot);
-
         //on regarde la position des moteurs
         switch(etat) {
             case 2 :
@@ -362,18 +370,46 @@
                 break;
 
             case 5 :
+                if(fm_etat == 1) {
+                    delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old;
+                    if(delta_position_3 > 2048) {
+                        delta_position_3 = delta_position_3 - 4096;
+                    }
 
-                delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old;
-                if(delta_position_3 > 2048) {
-                    delta_position_3 = delta_position_3 - 4096;
+                    if(delta_position_3 < -2048) {
+                        delta_position_3 = delta_position_3 + 4096;
+                    }
+                    send_roues();
                 }
 
-                if(delta_position_3 < -2048) {
-                    delta_position_3 = delta_position_3 + 4096;
+                fm_etat =0;
+
+                if (flag_a_traite == 1) {
+                    etat = 6;
+                    temps_ms = 0;
+                    fm_etat =1;
                 }
-                etat = 2;
-                fm_etat =1;
+                break;
+                
+            case 6 :
+                if(fm_etat ==1){
+                coach.puts("traitement\n");
+                if (flag_msg == 0) {
+                    sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
+                } else {
+                    sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
+                }
+                flag_a_traite = 0;
                 temps_ms = 0;
+                asser(front, lat, rot);
+                fm_etat = 0;
+                }
+                if(temps_ms>= 5) {
+
+                    etat = 2;
+                    fm_etat =1;
+                    temps_ms = 0;
+                }
                 break;
         }
     }