Diff: main.cpp

Revision:

8:922178e5d286

Parent:

6:b32a24905cd6

Child:

9:043f73bf5020

--- a/main.cpp	Fri Jun 05 08:47:26 2020 +0000
+++ b/main.cpp	Mon Jun 08 12:28:22 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,6 +232,10 @@
 
 int main()
 {
+    coach.baud(115200);
+    coach.puts("stl\n\r");
+    coach.attach(&callback_serial, Serial::RxIrq);
+
     //myax12.SetMode(1); //passage en Continuous rotation
 
     //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
@@ -239,6 +243,7 @@
     //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
+
     int etat=1;
     int fm_etat=1;
 
@@ -281,14 +286,17 @@
     ticker_1ms.attach(&fonction_1ms,0.001);
     etat=2;
 
-    coach.baud(115200);
-    coach.puts("stl\n\r");
-    coach.attach(&callback_serial, Serial::RxIrq);
-
+    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;
         }
     }