add drive management

2023-08-31 20:02:41 +02:00
parent 2ece901b08
commit a20667399b
8 changed files with 459 additions and 40 deletions
--- a/306-controller_interface.X/app/drive.c
+++ b/306-controller_interface.X/app/drive.c
@@ -0,0 +1,171 @@
+/**
+ * @author R<>mi Heredero
+ * @version 1.0.0
+ * @date August 2023
+ * @file drive.c
+ */
+
+#include "drive.h"
+#include "car.h"
+#include "can_message.h"
+
+void DRIVE_init(DRIVE* me){
+    me->state = STDR_INIT;
+    ALIVE_init(&me->myChecker);
+    ALIVE_onSetup(&me->myChecker, CM_DRIVE_SETUP, NULL);
+    ALIVE_onWait(&me->myChecker, DRIVE_emitStart, me);
+    ALIVE_onDead(&me->myChecker, DRIVE_emitStop, me);
+    ALIVE_onBorn(&me->myChecker, DRIVE_emitResurrect, me);
+    me->wait.f = NULL;
+    me->run.f = NULL;
+    me->dead.f = NULL;
+}
+
+void DRIVE_startBehaviour(DRIVE* me){
+    POST(me, &DRIVE_processEvent, evDRinit, 0, 0);
+}
+
+bool DRIVE_processEvent(Event* ev) {
+    bool processed = false;
+    DRIVE* me = (DRIVE*)Event_getTarget(ev);
+    DRIVE_STATES oldState = me->state;
+    evIDT evid = Event_getId(ev);
+    uint64_t data = Event_getData(ev);
+    
+    switch (me->state) {        // onState
+        case STDR_INIT:
+            if (ev->id == evDRinit) {
+                me->state = STDR_WAIT;
+                ALIVE_startBehaviourChecker(&me->myChecker); // Start alive checker
+            }
+            break;
+        
+        case STDR_WAIT:
+            if (ev->id == evDRstart) {
+                me->state = STDR_RUN;
+            }
+            ALIVE_setAliveTime(&me->myChecker, KART_CST.DRIVE_ALIVE_TIME);
+            ALIVE_emitBorn(&me->myChecker, 100, 0);      // Born after 100 ms
+            ALIVE_emitReady(&me->myChecker, 200, 0);     // Ready after 200 ms
+            break;
+        
+        case STDR_RUN:
+            if (ev->id == evDRstop) {
+                me->state = STDR_DEAD;
+            }
+            
+            if (ev->id == evDRpollTorque) {
+                CM_DRIVE_POWER(&eKart.torque);
+            }
+            
+            if (ev->id == evDRpollSpeed) {
+                CM_DISPLAY_SPEED(&eKart.speed);
+            }
+            
+            DRIVE_emitPollTorque(me, KART_CST.DRIVE_STOP_TIME*9, 0);
+            break;
+        
+        case STDR_DEAD:
+            if (ev->id == evDRresurrect) {
+                me->state = STDR_WAIT;
+            }
+            break;
+    }
+
+    if(oldState != me->state){
+        switch (oldState) {     // onExit
+            case STDR_INIT:
+                break;
+            
+            case STDR_WAIT:
+                break;
+            
+            case STDR_RUN:
+                break;
+            
+            case STDR_DEAD:
+                break;
+        }
+
+        switch (me->state) {    // onEntry
+            case STDR_INIT:
+                break;
+            
+            case STDR_WAIT:
+                if (me->wait.f != NULL) {
+                    me->wait.f(me->wait.p);
+                }
+                break;
+            
+            case STDR_RUN:
+                if (me->run.f != NULL) {
+                    me->run.f(me->run.p);
+                }
+                break;
+            
+            case STDR_DEAD:
+                if (me->dead.f != NULL) {
+                    me->dead.f(me->dead.p);
+                }
+                break;
+        }
+
+        processed = true;
+    }
+    return processed;
+}
+
+/*************
+ * Callbacks *
+ *************/
+
+void DRIVE_onWait(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
+    me->wait.f = f;
+    me->wait.p = p;
+}
+
+void DRIVE_onRun(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
+    me->run.f = f;
+    me->run.p = p;
+}
+
+void DRIVE_onDead(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
+    me->dead.f = f;
+    me->dead.p = p;
+}
+
+/************
+ * EMITTERS *
+ ************/
+
+void DRIVE_emitStart(void* p) {
+    DRIVE* me = (DRIVE*) p;
+    POST(me, &DRIVE_processEvent, evDRstart, 0, 0);
+}
+
+void DRIVE_emitStop(void* p) {
+    DRIVE* me = (DRIVE*) p;
+    POST(me, &DRIVE_processEvent, evDRstop, 0, 0);
+}
+
+void DRIVE_emitResurrect(void* p) {
+    DRIVE* me = (DRIVE*) p;
+    POST(me, &DRIVE_processEvent, evDRresurrect, 0, 0);
+}
+
+void DRIVE_emitPollSpeed(void* p) {
+    DRIVE* me = (DRIVE*) p;
+    POST(me, &DRIVE_processEvent, evDRpollSpeed, 0, 0);
+}
+
+void DRIVE_emitPollTorque(DRIVE* me, uint16_t t, int64_t data) {
+    POST(me, &DRIVE_processEvent, evDRpollTorque, t, data);
+}
+
+/***********
+ * SETTERS *
+ ***********/
+
+void DRIVE_setMyChecker(DRIVE* me, ALIVE v) {
+    me->myChecker = v;
+}