180 lines
4.5 KiB
C
180 lines
4.5 KiB
C
/**
|
|
* @author Rémi Heredero
|
|
* @version 1.0.0
|
|
* @date August 2023
|
|
* @file drive.c
|
|
*/
|
|
|
|
#include "drive.h"
|
|
#include "car.h"
|
|
#include "can_message.h"
|
|
//#include "steering.h"
|
|
//#include "factory/factory.h"
|
|
|
|
void DRIVE_init(DRIVE* me){
|
|
me->state = STDR_INIT;
|
|
ALIVE_init(&me->myChecker, 2);
|
|
ALIVE_onSetup(&me->myChecker, CM_DRIVE_SETUP, &ALWAYSTRUE);
|
|
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, 3000, 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);
|
|
// STEERING_STATES steeringState = steering()->state;
|
|
|
|
|
|
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) {
|
|
//if(steeringState == STST_RUN) {
|
|
CM_DRIVE_POWER(&eKart.torque);
|
|
if (KART_CST.DRIVE_STOP_TIME == 0) KART_CST.DRIVE_STOP_TIME = 1;
|
|
DRIVE_emitPollTorque(me, KART_CST.DRIVE_STOP_TIME*5, 0);
|
|
//}
|
|
}
|
|
|
|
if (ev->id == evDRpollSpeed) {
|
|
CM_DISPLAY_SPEED(&eKart.speed);
|
|
}
|
|
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 (KART_CST.DRIVE_STOP_TIME == 0) KART_CST.DRIVE_STOP_TIME = 1;
|
|
DRIVE_emitPollTorque(me, KART_CST.DRIVE_STOP_TIME*5, 0);
|
|
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;
|
|
}
|