Diff: main.cpp

Revision:

2:223d6363315b

Parent:

1:c5e9c436f8d0

Child:

3:c2c22b4f984b

--- a/main.cpp	Fri Apr 24 07:58:54 2020 +0000
+++ b/main.cpp	Fri May 15 09:51:02 2020 +0000
@@ -6,7 +6,8 @@
 DigitalOut alivenessLED(LED1);
 DigitalOut testLed(LED2);
 
-Serial coach(D1, D0);
+/* Serial coach(D1, D0, 115200); */
+Serial coach(USBTX, USBRX);
 
 const float rapport_vitesse = 0.019383809*0.026315;
 const float root3 = 1.7321;
@@ -17,33 +18,12 @@
 int i = 0;
 char last_read;
 int indice_a_modifie = 1;
-char message[50];
+char message_0[50] = {};
+char message_1[50] = {};
 char message_vide = '0';
 float front, lat, rot, T_front, T_lat = 0;
-
-/*typedef struct {
-    float front;
-    float lat;
-    float rot ;
-} dep;
-typedef struct {
-    float front;
-    float lat;
-} tire;*/
-/*
-void blinkCallback(void)
-{
-    alivenessLED = !alivenessLED;
-}*/
-
-void setup()
-{
-    //eventQueue.call_every(500, blinkCallback);
-    coach.baud(115200);
-    myax12a.SetMode(1);
-    myax12b.SetMode(1);
-    myax12c.SetMode(1);
-}
+int flag_msg = 0;
+int flag_a_traite = 0;
 
 void asser(float Vx, float Vy, float gammaz)
 {
@@ -60,96 +40,74 @@
     coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat );
 }
 
-void mise_a_zero_msg()
+void mise_a_zero_msg_0()
 {
     for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
-        message[i] = message_vide;
+        message_0[i] = message_vide;
     }
-    indice_a_modifie = 0;
+}
+
+
+void mise_a_zero_msg_1()
+{
+    for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
+        message_1[i] = message_vide;
+    }
 }
 
-void reception_com() 
+void callback_serial()
 {
-    if (coach.readable()) {
-        last_read = coach.getc();
-        /*
-        if (last_read == 'A') {
-            wait(0.05);
-            indice_a_modifie = 1;
-            last_read = coach.getc();
-            while ((last_read != ';')) {
-                message[indice_a_modifie] = last_read;
-                indice_a_modifie ++;
-                coach.printf("|%i|%s|", indice_a_modifie, message);
-                last_read = coach.getc();
-            }
-            message[indice_a_modifie] = last_read;
-            sscanf(message,"%f,%f,%f,%f,%f;", &front, &lat, &rot, &front, &lat );
-            mise_a_zero_msg();
-        }*/
-        switch(last_read){
-            case 'Z':
-            front = 0;
-            lat = 0;
-            rot = 0;
-            T_front = 0;
-            T_lat = 0;
-            break;
-            
-            case 'a':
-            front ++;
-            break;
-            
-            case 'b':
-            front --;
-            break;
-            
-            case 'A':
-            front = front + 10;
-            break;
-            
-            case 'B':
-            front = front - 10;
-            break;
+    last_read = coach.getc();
+    if (last_read == 'S') {
+        send_stat();
+    } else if (last_read == ';') {
+        coach.puts("EOL");
+        coach.puts(message_0);
+        coach.puts(message_1);
+        if (flag_msg == 0) {
+            message_0[indice_a_modifie] = last_read;
+            mise_a_zero_msg_1();
+            flag_msg = 1;
+        } else {
+            message_1[indice_a_modifie] = last_read;
+            mise_a_zero_msg_0();
+            flag_msg = 0;
+        }
+        flag_a_traite = 1;
+        indice_a_modifie = 0;
+    } else {
+        if (flag_msg == 0) {
+            message_0[indice_a_modifie] = last_read;
+        } else {
+            message_1[indice_a_modifie] = last_read;
+        }
+        indice_a_modifie++;
+    }
+}
 
-            case 'c':
-            lat ++;
-            break;
-            
-            case 'd':
-            lat --;
-            break;
-            
-            case 'C':
-            lat = lat + 10;
-            break;
-            
-            case 'D':
-            lat = lat - 10;
-            break;
-            
-            case 'e':
-            rot ++;
-            break;
-            
-            case 'f':
-            rot --;
-            break;
-            
-            case 'E':
-            rot = rot + 10;
-            break;
-            
-            case 'F':
-            rot = rot - 10;
-            break;
-                    
-            case 'S' :
-            send_stat();
+void reception_com()
+{
+    if (flag_a_traite == 1) {
+        coach.puts("traitement");
+        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;
     }
 }
 
+void setup()
+{
+    coach.baud(115200);
+    myax12a.SetMode(1);
+    myax12b.SetMode(1);
+    myax12c.SetMode(1);
+    coach.puts("stl\n\r");
+    coach.attach(&callback_serial, Serial::RxIrq);
+}
+
 int main()
 {
     setup();
@@ -158,4 +116,4 @@
         asser(front, lat, rot);
     }
 
-}
\ No newline at end of file
+}