Diff: main.cpp

Revision:

0:8762773cddf9

Child:

1:c5e9c436f8d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 27 07:46:40 2020 +0000
@@ -0,0 +1,93 @@
+
+/* #include <mbed_events.h> */
+#include "mbed.h"
+#include "AX12.h"
+
+DigitalOut alivenessLED(LED1);
+DigitalOut testLed(LED2);
+
+Serial coach(D1, D0);
+
+const float rapport_vitesse = 0.019383809*0.026315;
+const float root3 = 1.7321;
+AX12 myax12a (PC_4, PC_5, 2);
+AX12 myax12b (PC_4, PC_5, 3);
+AX12 myax12c (PC_4, PC_5, 4);
+
+int i = 0;
+char last_read;
+int indice_a_modifie = 0;
+char message[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);
+    }
+    
+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 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(){
+    if (coach.readable()) {
+        last_read = coach.getc();
+        if (last_read == 'A') {
+            indice_a_modifie = 0;
+            last_read = coach.getc();
+            while ((last_read != ';')){
+                message[indice_a_modifie] = last_read;
+                coach.printf("|%i|%s|", indice_a_modifie, message);
+                indice_a_modifie ++;
+                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') {
+            send_stat();
+            }
+    }}
+
+int main(){
+    setup();
+    while (true){
+        reception_com();
+        asser(front, lat, rot);
+        }
+
+    }
\ No newline at end of file