Travail inachevé avec Arduino

arduino/ACsignals.h

@@ -5,6 +5,8 @@
 #ifndef __ACSIGNALS_H_
 #define __ACSIGNALS_H_
 
+#include "AFsignals.h"
+
 #define AC_BAUDRATE 9600UL
 
 // Structures used everywhere
@@ -62,6 +64,8 @@ struct __attribute__ ((packed)) A2CI_DBGs {
     struct position actuel;
     struct position destination;
     unsigned char movement;
+    struct F2AI_CODERs deltaCoder;
+    uint16_t nbCalcPos;
     // ...
 };
 

arduino/AFsignals.h

@@ -41,8 +41,8 @@ struct __attribute__ ((packed)) F2AD_ERRs {
 // Récupère les valeur des encodeurs
 #define F2AI_CODER 'D'
 struct __attribute__ ((packed)) F2AI_CODERs {
-    int16_t left;
-    int16_t right;
+    int16_t dL;
+    int16_t dR;
 };
 
 // Récupère les valeur des capteurs de distance

arduino/debug.c

@@ -13,7 +13,10 @@ void TaskDebug(void *pvParameters) {
         // Copie des valeurs à envoyer en debug
         memcpy((void*) &debug.actuel, (const void*) &actuel, (unsigned long) sizeof(actuel));
         memcpy((void*) &debug.destination, (const void*) &destination, (unsigned long) sizeof(destination));
+        memcpy((void*) &debug.deltaCoder, (const void*) &deltaCoder, (unsigned long) sizeof(deltaCoder));
         debug.movement = movement;
+        debug.nbCalcPos = nbCalcPos;
+        nbCalcPos = 0;
 
         // Envoi des valeurs
         sendAC(A2CI_DBG, &debug, sizeof(debug));

arduino/position.c

@@ -1,25 +1,51 @@
 #include "position.h"
 #include "AC.h"
+#include "AF.h"
+#include "dimensions.h"
 
-void TaskPosition(void *pvParameters) {
-    (void) pvParameters;
-    TickType_t xLastWakeTime;
-    TickType_t xFrequency = 100 / portTICK_PERIOD_MS;
+void TaskPosition(void* pvParameters)
+{
+    (void)pvParameters;
+
+    const TickType_t xDelay = 500 / portTICK_PERIOD_MS;
 
-    ulTaskNotifyTake(pdFALSE, portMAX_DELAY); // TODO Dummy
-    xLastWakeTime = xTaskGetTickCount();
     for (;;) {
-        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        sendAF(F2AI_CODER, NULL, 0);
+
+        ulTaskNotifyTake(pdFALSE, portMAX_DELAY); // Wait until new information has arrived
+
+        nbCalcPos++;
+
+        float adjacent = DISTANCE_BETWEEN_WHEELS;
+        float opposite = deltaCoder.dR - deltaCoder.dL;
+        float deltaO = atan(opposite / adjacent);
+
+        float deltaD = (deltaCoder.dL + deltaCoder.dR) / 2;
+
+        actuel.o += deltaO;
+        float deltaX = deltaD * cos(actuel.o);
+        float deltaY = deltaD * sin(actuel.o);
+        actuel.x += deltaX;
+        actuel.y += deltaY;
+
+        vTaskDelay(xDelay);
     }
 }
 
-
-
-void configurePosition() {
-    actuel.x = 0;
-    actuel.y = 0;
-    actuel.o = 90;
-
-    xTaskCreate(TaskPosition, "Position", 128, NULL, 2, &tPosition);;
+void onF2AI_CODER()
+{
+    readAF(&deltaCoder, sizeof(struct F2AI_CODERs));
+    vTaskNotifyGiveFromISR(tPosition, NULL);
 }
 
+void configurePosition()
+{
+    actuel.x = 0;
+    actuel.y = 0;
+    actuel.o = 0;
+
+    registerRxHandlerAF(F2AI_CODER, onF2AI_CODER);
+
+    xTaskCreate(TaskPosition, "Position", 128, NULL, 2, &tPosition);
+    ;
+}

arduino/position.h

@@ -9,11 +9,15 @@
 #include <task.h>
 
 #include "ACsignals.h"
+#include "AFsignals.h"
 
 struct position actuel;
+struct F2AI_CODERs deltaCoder;
 
 TaskHandle_t tPosition;
 
+uint16_t nbCalcPos;
+
 void TaskPosition();
 void configurePosition();