From a20667399bbb659305a3bc5ecf5f9bb217a3dccb Mon Sep 17 00:00:00 2001 From: Klagarge Date: Thu, 31 Aug 2023 20:02:41 +0200 Subject: [PATCH] add drive management --- 306-controller_interface.X/app/can_message.c | 15 +- 306-controller_interface.X/app/drive.c | 171 ++++++++++++++++++ 306-controller_interface.X/app/drive.h | 130 +++++++++++++ .../app/factory/factory.c | 39 +--- .../app/factory/factory.h | 5 +- .../nbproject/configurations.xml | 2 + UML/drive.pdf | Bin 0 -> 2392 bytes UML/drive.uxf | 137 ++++++++++++++ 8 files changed, 459 insertions(+), 40 deletions(-) create mode 100644 306-controller_interface.X/app/drive.c create mode 100644 306-controller_interface.X/app/drive.h create mode 100644 UML/drive.pdf create mode 100644 UML/drive.uxf diff --git a/306-controller_interface.X/app/can_message.c b/306-controller_interface.X/app/can_message.c index 763192a..e32b8b2 100644 --- a/306-controller_interface.X/app/can_message.c +++ b/306-controller_interface.X/app/can_message.c @@ -171,8 +171,9 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data){ KART_CST.JOYSTICK_PARAM2 = MEM_read_1_byte(MEMADD_JOYSTICK_PARAM2); KART_CST.JOYSTICK_ALIVE_TIME = MEM_read_1_byte(MEMADD_JOYSTICK_ALIVE_TIME); ALIVE_setAliveTime(ALjoy(), KART_CST.JOYSTICK_ALIVE_TIME); - ALIVE_emitBorn(ALjoy(), 0, 0); - ALIVE_emitReady(ALjoy(), 100, 0); + ALIVE_emitResurrect(ALjoy(), 0, 0); + ALIVE_emitBorn(ALjoy(), 100, 0); + ALIVE_emitReady(ALjoy(), 200, 0); } } break; @@ -204,18 +205,18 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, uint32_t data){ rpm.separate.byte2 = tmpData.separate.byte1; rpm.separate.byte3 = tmpData.separate.byte0; calcSpeed(rpm.full); + DRIVE_emitPollSpeed(drive()); } if(idMsg == 0xF) { // DRIVE_ALIVE // statusH statusL - - - ALIVE_ISALIVE(ALdrive()); - if(ALdrive()->state == STAL_DEAD){ + ALIVE_ISALIVE(&drive()->myChecker); + if(drive()->myChecker.state == STAL_DEAD){ KART_CST.DRIVE_SPEED_TIME = MEM_read_1_byte(MEMADD_DRIVE_SPEED_TIME); KART_CST.DRIVE_STOP_TIME = MEM_read_1_byte(MEMADD_DRIVE_STOP_TIME); KART_CST.DRIVE_ALIVE_TIME = MEM_read_1_byte(MEMADD_DRIVE_ALIVE_TIME); - ALIVE_setAliveTime(ALdrive(), KART_CST.DRIVE_ALIVE_TIME); - ALIVE_emitBorn(ALdrive(), 0, 0); - ALIVE_emitReady(ALdrive(), 100, 0); + ALIVE_emitResurrect(&drive()->myChecker, 0, 0); + ALIVE_emitBorn(&drive()->myChecker, 0, 0); } // TODO drive say ALIVE diff --git a/306-controller_interface.X/app/drive.c b/306-controller_interface.X/app/drive.c new file mode 100644 index 0000000..f926b1f --- /dev/null +++ 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; +} diff --git a/306-controller_interface.X/app/drive.h b/306-controller_interface.X/app/drive.h new file mode 100644 index 0000000..647e66e --- /dev/null +++ b/306-controller_interface.X/app/drive.h @@ -0,0 +1,130 @@ +/** + * @author Rémi Heredero + * @version 1.0.0 + * @date August 2023 + * @file drive.h + */ +#ifndef DRIVE_H +#define DRIVE_H + +#include "../xf/xf.h" +#include "../middleware/alive.h" + +typedef enum { + STDR_INIT, + STDR_WAIT, + STDR_RUN, + STDR_DEAD +} DRIVE_STATES; + +typedef enum { + evDRinit = 100, + evDRstart, + evDRstop, + evDRresurrect, + evDRpollSpeed, + evDRpollTorque +} DRIVE_EVENTS; + +typedef void (*DRIVE_CALLBACK_FUNCTION)(void*); +typedef struct { + DRIVE_CALLBACK_FUNCTION f; // function + void* p; // param(s) +} DRIVE_CALLBACK; + +typedef struct { + DRIVE_STATES state; + ALIVE myChecker; + DRIVE_CALLBACK wait; + DRIVE_CALLBACK run; + DRIVE_CALLBACK dead; +} DRIVE; + +/** + * Initialize the DRIVE + * @param me the DRIVE itself + */ +void DRIVE_init(DRIVE* me); + +/** + * Start the DRIVE state machine + * @param me the DRIVE itself + */ +void DRIVE_startBehaviour(DRIVE* me); + +/** + * Process the event + * @param ev the event to process + * @return true if the event is processed + */ +bool DRIVE_processEvent(Event* ev); + +/************* + * Callbacks * + *************/ + +/** + * Set the callback function to call when the DRIVE is entering state wait + * @param me the DRIVE itself + * @param f the function to call + * @param p the param(s) to pass to the function + */ +void DRIVE_onWait(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p); + +/** + * Set the callback function to call when the DRIVE is entering state run + * @param me the DRIVE itself + * @param f the function to call + * @param p the param(s) to pass to the function + */ +void DRIVE_onRun(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p); + +/** + * Set the callback function to call when the DRIVE is entering state dead + * @param me the DRIVE itself + * @param f the function to call + * @param p the param(s) to pass to the function + */ +void DRIVE_onDead(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p); + +/************ + * EMITTERS * + ************/ + +/** + * Emit the start event + * @param p the DRIVE itself + */ +void DRIVE_emitStart(void* p); + +/** + * Emit the stop event + * @param p the DRIVE itself + */ +void DRIVE_emitStop(void* p); + +/** + * Emit the resurrect event + * @param p the DRIVE itself + */ +void DRIVE_emitResurrect(void* p); + +/** + * Emit the pollSpeed event + * @param p the DRIVE itself + */ +void DRIVE_emitPollSpeed(void* p); + +/** + * Emit the pollTorque event + * @param p the DRIVE itself + */ +void DRIVE_emitPollTorque(DRIVE* me, uint16_t t, int64_t data); + +/*********** + * SETTERS * + ***********/ + + + +#endif diff --git a/306-controller_interface.X/app/factory/factory.c b/306-controller_interface.X/app/factory/factory.c index a2905fa..12121f8 100644 --- a/306-controller_interface.X/app/factory/factory.c +++ b/306-controller_interface.X/app/factory/factory.c @@ -42,8 +42,8 @@ ALIVE* ALjoy(){ return &theFactory.ALjoy_; } -ALIVE* ALdrive(){ - return &theFactory.ALdrive_; +DRIVE* drive(){ + return &theFactory.drive_; } @@ -79,20 +79,7 @@ void Factory_init() { ALIVE_setAliveTime(ALcontroller(), KART_CST.CONTROL_ALIVE_TIME); ALIVE_init(ALjoy()); - ALIVE_init(ALdrive()); -} - -void foo(void* p){ - static int16_t speed = 0; - static bool up = true; - if(up) { - speed += 3; - if(speed >100) up = false; - } else { - speed -= 3; - if(speed < -100) up = true; - } - CM_DRIVE_POWER(&speed); + DRIVE_init(drive()); } //connect objects if required @@ -110,31 +97,21 @@ void Factory_build() { ALIVE_onBorn(ALjoy(), LED_on, l1()); ALIVE_onDead(ALjoy(), LED_off, l1()); - ALIVE_onSetup(ALdrive(), CM_DRIVE_SETUP, NULL); - ALIVE_setAliveTime(ALdrive(), KART_CST.DRIVE_ALIVE_TIME); - ALIVE_onBorn(ALdrive(), LED_on, l2()); - ALIVE_onDead(ALdrive(), LED_off, l2()); + DRIVE_onRun(drive(), LED_on, l2()); + DRIVE_onDead(drive(), LED_off, l2()); - BLINKER_setTimeOn(b1(), (KART_CST.DRIVE_STOP_TIME*9)/2); - BLINKER_setTimeOff(b1(), (KART_CST.DRIVE_STOP_TIME*9)/2); - BLINKER_onOn(b1(), foo, NULL); } //start all state machines void Factory_start() { CAN_startBehaviour(); - - ALIVE_startBehaviourSender(ALcontroller()); - ALIVE_startBehaviourChecker(ALjoy()); + //ALIVE_startBehaviourSender(ALcontroller()); + //ALIVE_startBehaviourChecker(ALjoy()); ALIVE_emitBorn(ALjoy(), 100, 0); ALIVE_emitReady(ALjoy(), 200, 0); - ALIVE_startBehaviourChecker(ALdrive()); - ALIVE_emitBorn(ALdrive(), 100, 0); - ALIVE_emitReady(ALdrive(), 200, 0); - BLINKER_startBehaviour(b1()); - BLINKER_emitBlink(b1(), 300); + DRIVE_startBehaviour(drive()); } diff --git a/306-controller_interface.X/app/factory/factory.h b/306-controller_interface.X/app/factory/factory.h index 1b81d90..1a35e49 100644 --- a/306-controller_interface.X/app/factory/factory.h +++ b/306-controller_interface.X/app/factory/factory.h @@ -15,6 +15,7 @@ #include "../car.h" #include "../can_message.h" #include "../eeprom.h" +#include "../drive.h" #include "../../board/led/led.h" #include "../../board/button/button.h" #include "../../middleware/alive.h" @@ -36,7 +37,7 @@ typedef struct { ALIVE ALcontroller_; ALIVE ALjoy_; - ALIVE ALdrive_; + DRIVE drive_; } Factory; @@ -59,7 +60,7 @@ BLINKER* b1(); ALIVE* ALcontroller(); ALIVE* ALjoy(); -ALIVE* ALdrive(); +DRIVE* drive(); #endif \ No newline at end of file diff --git a/306-controller_interface.X/nbproject/configurations.xml b/306-controller_interface.X/nbproject/configurations.xml index 77dd0d1..cf78aa9 100644 --- a/306-controller_interface.X/nbproject/configurations.xml +++ b/306-controller_interface.X/nbproject/configurations.xml @@ -10,6 +10,7 @@ app/can_message.h app/kartculator.h app/eeprom.h + app/drive.h board/led/led.h @@ -48,6 +49,7 @@ app/can_message.c app/kartculator.c app/eeprom.c + app/drive.c board/led/led.c diff --git a/UML/drive.pdf b/UML/drive.pdf new file mode 100644 index 0000000000000000000000000000000000000000..5935edf4a27a1a7ea9aa25b683ee6bcadde9a440 GIT binary patch literal 2392 zcmai0Yg7~076!Q@Mz0SPDIz*3ph(F)@?aE*H%$OhLU@QE??8qagbXGj1fx}0h*+@} zZ!K3)%1bX60fT@jZ!6S-8Z4rsMa5u2p%kUy1BK2|T3pJy_xzcC_P1xBZ}0D{J!9#? zb+(16bc*F;t)`7)2S5NG7D;h%0NpT&NWL9_5hesWi=*V26m*Uf$T2P!jtemsOD30M zf@q5JM5kXgf0^BavD&)JE7+07s<)+zHwO(;3VzD(D(>QxeAlXW&2BMUHp^(dcBr&u z*4IV;>_ujmR83kFmE({y;$m|!pJw{?A$K1C{5$c8c~x`Si|qE9W05|Xdt4no7z-)Y zMx~hyi}atloQTabDf+zm^LM;LZ~FM1c}>SRv>|L8t#~QQHp=4y^19rG|2}EAD32AQ zZT@}Rp3}P73E8reRNKMP+$Ck0D#^OL!lR)_?eSy5jP2J7;`QtGDvQhG&m}dS-o-MP zCbk)d)}+9ntoC;c;|M=pNsG&gy`UP4mZYFOPP( zVCLWPbEDM}TUdYSRd0ISyK2Them5mUX>M=a0JLmgl!; zYGr1g2)BKiCIN^@YuaHXTn0uwyiqc9gxlulrkvy4emDQB*ye#>{lVe&<Mh*gDq3_4PvZUK z4MJ7tl+WMZFt5BdxpQv0e##e*64wOyji^Bz=lY|)gVu=X!6-(F>nfR|gi}hDtlGqx z4B);M2wcN<@o=+MU{aYFmjqf{gB;?+DJN_$A@>^YdIUmf5K4n#6r#~-HW0EJf>v9D zJY4uc1Ez{mO0ftEM0}|!1c%85Fc}OQ1Bd`7eXt!w+zWC6lD~aS8X?bzzG>nZFwwsc z;~3NALqEy~K`^282k+WVb1qT($8qS#J`{P6i(D!YM`2R(cg0)03nN+pdf_;sIw`Lg z=qic8feG;ln+pUuU`zx!NMwtq@b Wk;w&8`Gf}WIuxZ?S~~HaDSrTd$f(Q! literal 0 HcmV?d00001 diff --git a/UML/drive.uxf b/UML/drive.uxf new file mode 100644 index 0000000..0f5e6c9 --- /dev/null +++ b/UML/drive.uxf @@ -0,0 +1,137 @@ + + + 15 + + Relation + + 660 + 90 + 210 + 120 + + lt=-> +evInit +/startAliveChecker + + 10.0;10.0;10.0;60.0 + + + UMLSpecialState + + 660 + 75 + 30 + 30 + + type=initial + + + + UMLState + + 540 + 405 + 270 + 105 + + RUN +-- +/entry: LED ON +-- +emitPollTorqueEv + + + + Relation + + 735 + 450 + 195 + 270 + + lt=-> +m1= evPollTorque\n/sendTorque + + 10.0;100.0;10.0;160.0;110.0;160.0;110.0;10.0;50.0;10.0 + + + UMLState + + 495 + 180 + 360 + 135 + + WAIT +-- +ALIVE_emitBorn(ALjoy(), 0, 0); +ALIVE_emitReady(ALjoy(), 100, 0); +setAliveTime + + + + Relation + + 660 + 300 + 120 + 135 + + lt=-> +m1=evStart\n(onWait) + + 10.0;10.0;10.0;70.0 + + + Relation + + 735 + 450 + 195 + 180 + + lt=-> +m1= evPollSpeed\n/sendSpeed + + 10.0;40.0;10.0;100.0;110.0;100.0;110.0;10.0;50.0;10.0 + + + Relation + + 390 + 240 + 345 + 555 + + lt=-> +m1= evResurrect\n(onBorn) + + 130.0;300.0;130.0;350.0;10.0;350.0;10.0;10.0;70.0;10.0 + + + UMLState + + 450 + 600 + 270 + 90 + + DEAD +-- +/entry: LED OFF + + + + Relation + + 570 + 495 + 135 + 135 + + lt=-> +m1=evStop\n(onDead) + + 10.0;10.0;10.0;70.0 + +