168 lines
4.2 KiB
C
168 lines
4.2 KiB
C
/**
|
|
* @author Rémi Heredero
|
|
* @version 1.0.0
|
|
* @date September 2023
|
|
* @file steering.c
|
|
*/
|
|
|
|
#include "steering.h"
|
|
#include "can_message.h"
|
|
#include "car.h"
|
|
//#include "drive.h"
|
|
|
|
void STEERING_init(STEERING* me){
|
|
me->state = STST_INIT;
|
|
ALIVE_init(&me->myChecker, 3);
|
|
ALIVE_onSetup(&me->myChecker, CM_STEERING_SETUP, &ALWAYSFALSE);
|
|
ALIVE_onWait(&me->myChecker, STEERING_emitStart, me);
|
|
ALIVE_onDead(&me->myChecker, STEERING_emitStop, me);
|
|
ALIVE_onBorn(&me->myChecker, STEERING_emitResurrect, me);
|
|
me->wait.f = NULL;
|
|
me->run.f = NULL;
|
|
me->dead.f = NULL;
|
|
}
|
|
|
|
void STEERING_startBehaviour(STEERING* me){
|
|
POST(me, &STEERING_processEvent, evSTinit, 100, 0);
|
|
}
|
|
|
|
bool STEERING_processEvent(Event* ev) {
|
|
bool processed = false;
|
|
STEERING* me = (STEERING*)Event_getTarget(ev);
|
|
STEERING_STATES oldState = me->state;
|
|
evIDT evid = Event_getId(ev);
|
|
uint64_t data = Event_getData(ev);
|
|
|
|
switch (me->state) { // onState
|
|
case STST_INIT:
|
|
if (ev->id == evSTinit) {
|
|
me->state = STST_WAIT;
|
|
ALIVE_startBehaviourChecker(&me->myChecker); // Start alive checker
|
|
}
|
|
break;
|
|
|
|
case STST_WAIT:
|
|
if (ev->id == evSTstart) {
|
|
me->state = STST_RUN;
|
|
}
|
|
// ALIVE_setAliveTime(&me->myChecker, KART_CST.STEERING_ALIVE_TIME);
|
|
// ALIVE_emitBorn(&me->myChecker, 500, 0);
|
|
// ALIVE_emitReady(&me->myChecker, 1000, 0);
|
|
break;
|
|
|
|
case STST_RUN:
|
|
if (ev->id == evSTstop) {
|
|
me->state = STST_DEAD;
|
|
}
|
|
|
|
if (ev->id == evSTpollDir) {
|
|
//if(drive()->state == STDR_RUN) {
|
|
CM_STEERING_SET(&eKart.position);
|
|
//}
|
|
}
|
|
break;
|
|
|
|
case STST_DEAD:
|
|
if (ev->id == evSTresurrect) {
|
|
me->state = STST_WAIT;
|
|
}
|
|
break;
|
|
}
|
|
|
|
if(oldState != me->state){
|
|
switch (oldState) { // onExit
|
|
case STST_INIT:
|
|
break;
|
|
|
|
case STST_WAIT:
|
|
break;
|
|
|
|
case STST_RUN:
|
|
break;
|
|
|
|
case STST_DEAD:
|
|
break;
|
|
}
|
|
|
|
switch (me->state) { // onEntry
|
|
case STST_INIT:
|
|
break;
|
|
|
|
case STST_WAIT:
|
|
if (me->wait.f != NULL) {
|
|
me->wait.f(me->wait.p);
|
|
}
|
|
ALIVE_setAliveTime(&me->myChecker, KART_CST.STEERING_ALIVE_TIME);
|
|
ALIVE_emitBorn(&me->myChecker, 500, 0);
|
|
ALIVE_emitReady(&me->myChecker, 1000, 0);
|
|
break;
|
|
|
|
case STST_RUN:
|
|
if (me->run.f != NULL) {
|
|
me->run.f(me->run.p);
|
|
}
|
|
break;
|
|
|
|
case STST_DEAD:
|
|
if (me->dead.f != NULL) {
|
|
me->dead.f(me->dead.p);
|
|
}
|
|
break;
|
|
}
|
|
|
|
processed = true;
|
|
}
|
|
return processed;
|
|
}
|
|
|
|
/*************
|
|
* Callbacks *
|
|
*************/
|
|
|
|
void STEERING_onWait(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
|
|
me->wait.f = f;
|
|
me->wait.p = p;
|
|
}
|
|
|
|
void STEERING_onRun(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
|
|
me->run.f = f;
|
|
me->run.p = p;
|
|
}
|
|
|
|
void STEERING_onDead(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
|
|
me->dead.f = f;
|
|
me->dead.p = p;
|
|
}
|
|
|
|
/************
|
|
* EMITTERS *
|
|
************/
|
|
|
|
void STEERING_emitStart(void* p) {
|
|
STEERING* me = (STEERING*) p;
|
|
POST(me, &STEERING_processEvent, evSTstart, 0, 0);
|
|
}
|
|
|
|
void STEERING_emitStop(void* p) {
|
|
STEERING* me = (STEERING*) p;
|
|
POST(me, &STEERING_processEvent, evSTstop, 0, 0);
|
|
}
|
|
|
|
void STEERING_emitResurrect(void* p) {
|
|
STEERING* me = (STEERING*) p;
|
|
POST(me, &STEERING_processEvent, evSTresurrect, 0, 0);
|
|
}
|
|
|
|
void STEERING_emitPollDir(void* p) {
|
|
STEERING* me = (STEERING*) p;
|
|
POST(me, &STEERING_processEvent, evSTpollDir, 0, 0);
|
|
}
|
|
|
|
/***********
|
|
* SETTERS *
|
|
***********/
|
|
|
|
void STEERING_setMyChecker(STEERING* me, ALIVE v) {
|
|
me->myChecker = v;
|
|
}
|