diff --git a/306-controller_interface.X/app/can_message.c b/306-controller_interface.X/app/can_message.c index 2b812b1..5703115 100644 --- a/306-controller_interface.X/app/can_message.c +++ b/306-controller_interface.X/app/can_message.c @@ -97,7 +97,7 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){ // TODO set alive times // start alives CM_DISPLAY_SETUP(NULL); - CM_STEERING_SETUP(&ALWAYSFALSE); + CM_STEERING_SETUP(&ALWAYS0); } @@ -161,6 +161,7 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){ // posX posY button - calcTorque(incomeData.separate.byte1); calcPosition(incomeData.separate.byte0); + STEERING_emitPollDir(steering()); } @@ -225,6 +226,7 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){ if(idMsg == 0x1) { // STEERING_GET_CENTER // valHH valH valL valLL eKart.center = revertData.full; + ALIVE_emitReady(&steering()->myChecker, 0, 0); } if(idMsg == 0x2) { // STEERING_GET_POSITION @@ -232,11 +234,6 @@ void CM_processIncome(uint8_t idSender, uint8_t idMsg, bool rtr, uint32_t data){ } - if(idMsg == 0x3) { // STEERING_READY - // - - - - - - } - if(idMsg == 0xF) { // STEERING_ALIVE // statusH statusL - - ALIVE_ISALIVE(&steering()->myChecker); @@ -340,7 +337,7 @@ void CM_STEERING_SETUP(void* p) { // reset/init homing setCenter aliveTime // TODO not working have to fix it BYTES_4 tmpData; - uint8_t choice = (uint8_t) p; + uint8_t choice = *(uint8_t*) p; switch (choice) { case 1: tmpData.separate.byte0 = 1; diff --git a/306-controller_interface.X/app/drive.c b/306-controller_interface.X/app/drive.c index 7cf4373..70d5adf 100644 --- a/306-controller_interface.X/app/drive.c +++ b/306-controller_interface.X/app/drive.c @@ -24,7 +24,7 @@ void DRIVE_init(DRIVE* me){ } void DRIVE_startBehaviour(DRIVE* me){ - POST(me, &DRIVE_processEvent, evDRinit, 100, 0); + POST(me, &DRIVE_processEvent, evDRinit, 3000, 0); } bool DRIVE_processEvent(Event* ev) { diff --git a/306-controller_interface.X/app/eeprom.c b/306-controller_interface.X/app/eeprom.c index 4efdbf8..013c1b9 100644 --- a/306-controller_interface.X/app/eeprom.c +++ b/306-controller_interface.X/app/eeprom.c @@ -24,10 +24,10 @@ void MEM_init(){ uint8_t check = MEM_read_1_byte(0x0); if(check != 0x42){ KART_CST.CONTROL_STEERING_MODE = 0; - KART_CST.CONTROL_ALIVE_TIME = 50; + KART_CST.CONTROL_ALIVE_TIME = 250; // should be 50 KART_CST.CONTROL_SPEED_FACTOR = 111111; // 111'111 KART_CST.CONTROL_POWER_FACTOR = 10000; // 10'000 - KART_CST.CONTROL_STEERING_FACTOR = 400000000; // 400'000'000 + KART_CST.CONTROL_STEERING_FACTOR = 5600240; // 5'600'024 KART_CST.CONTROL_MAX_SPEED_FW = 50; KART_CST.CONTROL_MAX_SPEED_BW = 25; diff --git a/306-controller_interface.X/app/kartculator.c b/306-controller_interface.X/app/kartculator.c index 88a490b..aa43fef 100644 --- a/306-controller_interface.X/app/kartculator.c +++ b/306-controller_interface.X/app/kartculator.c @@ -18,6 +18,10 @@ void calcTorque(uint8_t joy_pos) { void calcPosition(uint8_t joy_pos){ int32_t calcPosition; calcPosition = (int8_t) joy_pos; + calcPosition *= (int32_t) KART_CST.CONTROL_STEERING_FACTOR; + calcPosition /= 1000; + calcPosition += eKart.center; + eKart.position = (uint32_t) calcPosition; } void calcSpeed(int32_t rpm) { diff --git a/306-controller_interface.X/app/steering.c b/306-controller_interface.X/app/steering.c index 74fa712..45c7648 100644 --- a/306-controller_interface.X/app/steering.c +++ b/306-controller_interface.X/app/steering.c @@ -10,20 +10,26 @@ #include "car.h" //#include "drive.h" +void alive_born(void* p){ + STEERING* me = (STEERING*) p; + STEERING_emitResurrect(me); + //CM_STEERING_SETUP(&ALWAYS3); +} + void STEERING_init(STEERING* me){ me->state = STST_INIT; ALIVE_init(&me->myChecker, 3); - ALIVE_onSetup(&me->myChecker, CM_STEERING_SETUP, &ALWAYSFALSE); + ALIVE_onSetup(&me->myChecker, CM_STEERING_SETUP, &ALWAYS2); ALIVE_onWait(&me->myChecker, STEERING_emitStart, me); ALIVE_onDead(&me->myChecker, STEERING_emitStop, me); - ALIVE_onBorn(&me->myChecker, STEERING_emitResurrect, me); + ALIVE_onBorn(&me->myChecker, alive_born, 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); + POST(me, &STEERING_processEvent, evSTinit, 3000, 0); } bool STEERING_processEvent(Event* ev) { @@ -94,7 +100,6 @@ bool STEERING_processEvent(Event* ev) { } 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: