Diff: main.cpp

Revision:

1:c5e9c436f8d0

Parent:

0:8762773cddf9

Child:

2:223d6363315b

--- a/main.cpp	Fri Mar 27 07:46:40 2020 +0000
+++ b/main.cpp	Fri Apr 24 07:58:54 2020 +0000
@@ -16,9 +16,9 @@
 
 int i = 0;
 char last_read;
-int indice_a_modifie = 0;
+int indice_a_modifie = 1;
 char message[50];
-char message_vide = 0;
+char message_vide = '0';
 float front, lat, rot, T_front, T_lat = 0;
 
 /*typedef struct {
@@ -36,58 +36,126 @@
     alivenessLED = !alivenessLED;
 }*/
 
-void setup(){
+void setup()
+{
     //eventQueue.call_every(500, blinkCallback);
     coach.baud(115200);
     myax12a.SetMode(1);
     myax12b.SetMode(1);
     myax12c.SetMode(1);
-    }
-    
-void asser(float Vx, float Vy, float gammaz){
+}
+
+void asser(float Vx, float Vy, float gammaz)
+{
     float wa = rapport_vitesse*(Vx + 100 * gammaz);
     float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
     float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
     myax12a.SetCRSpeed(wa);
     myax12b.SetCRSpeed(wb);
     myax12c.SetCRSpeed(wc);
-    }
+}
 
-void send_stat(){
-    coach.printf("%f,%f,%f,%f,%f \n", front, lat, rot, front, lat );
-    }
+void send_stat()
+{
+    coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat );
+}
 
-void mise_a_zero_msg(){
-    for(i=0;i<50;i++){//On réinitialise la chaîne de caractères
-        message[i] = message_vide;}
+void mise_a_zero_msg()
+{
+    for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
+        message[i] = message_vide;
+    }
     indice_a_modifie = 0;
-    }
-    
-void reception_com(){
+}
+
+void reception_com() 
+{
     if (coach.readable()) {
         last_read = coach.getc();
+        /*
         if (last_read == 'A') {
-            indice_a_modifie = 0;
+            wait(0.05);
+            indice_a_modifie = 1;
             last_read = coach.getc();
-            while ((last_read != ';')){
+            while ((last_read != ';')) {
                 message[indice_a_modifie] = last_read;
+                indice_a_modifie ++;
                 coach.printf("|%i|%s|", indice_a_modifie, message);
-                indice_a_modifie ++;
-                last_read = coach.getc();}
+                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();
-            }
-        if (last_read == 'S') {
+        }*/
+        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;
+
+            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();
-            }
-    }}
+        }
+    }
+}
 
-int main(){
+int main()
+{
     setup();
-    while (true){
+    while (true) {
         reception_com();
         asser(front, lat, rot);
-        }
+    }
 
-    }
\ No newline at end of file
+}
\ No newline at end of file