diff --git a/ADIS_tinyK22_SplitFlap/source/splitflap.c b/ADIS_tinyK22_SplitFlap/source/splitflap.c index 93ec587..3cd0dda 100644 --- a/ADIS_tinyK22_SplitFlap/source/splitflap.c +++ b/ADIS_tinyK22_SplitFlap/source/splitflap.c @@ -17,6 +17,8 @@ #include "McuLog.h" #include "lib/dict.h" #include "McuUtility.h" +#include "McuLib.h" +#include "MotOffsetTable.h" #define FLAP_QUEUE_LENGTH (5) /* number of elements in queue */ #define FLAP_QUEUE_SELEM_SIZE (sizeof(Flap_t)) /* size of element */ @@ -128,14 +130,15 @@ void SF_Deinit(SF_Handle_t instance){ /**********************/ bool SF_MoveMotorToZeroPosition(SF_Handle_t instance){ int numStepsMoved = 0; - uint16_t offsetSteps = 0; + int32_t offsetSteps = 0; // check if the hwId needs an offset init position - for (int i = 0; i < sizeof(offsetStepsPerHw)/sizeof(offsetStepsPerHw[0]); ++i) { - if(offsetStepsPerHw[i][0] == ((SF_t*)instance)->hwId){ - offsetSteps = offsetStepsPerHw[i][1]; - McuLog_info("SF_MoveMotorToZeroPosition found offsetSteps=%i for Splitflap <%i> with hwId=%i", offsetSteps, ((SF_t*)instance)->id, ((SF_t*)instance)->hwId); - break; - } + if(MotOffset_Get(((SF_t*)instance)->hwId, &offsetSteps) == ERR_OK){ + McuLog_info("SF_MoveMotorToZeroPosition found offsetSteps=%i for Splitflap <%i> with hwId=%i", offsetSteps, ((SF_t*)instance)->id, ((SF_t*)instance)->hwId); + } + // check if positive offset! + if(offsetSteps < 0){ + McuLog_info("SF_MoveMotorToZeroPosition found negative offset for Splitflap with hwId=%i. Offset set to zero.", ((SF_t*)instance)->hwId); + offsetSteps = 0; } if(SF_IS_RDY_TO_MOVE((SF_t*)instance)){ @@ -296,7 +299,8 @@ static void SF_Task(void *pv){ // parse parameter SF_Handle_t instance = (SF_Handle_t)pv; SF_t* splitflap = (SF_t*)instance; - Flap_t nextFlap = " "; + char nextFlap = ' '; + Flap_t flapBuffer = ""; bool initSuccess = false; McuLog_info("Splitflap: Task for Splitflap <%i> started.", splitflap->id); @@ -321,27 +325,28 @@ static void SF_Task(void *pv){ // action: check if any moves need to be made case SF_STATE_READY: // check if anything is in queue once (poll once, don't get item) - if(xQueuePeek(splitflap->flapQueue, &nextFlap, 0) != pdPASS){ + if(xQueuePeek(splitflap->flapQueue, &flapBuffer, 0) != pdPASS){ /* failed to receive => queue empty */ continue; } - McuLog_info("Splitflap <%i> recognized new flap '%c' in queue to move to.", splitflap->id, nextFlap[0]); + McuLog_info("Splitflap <%i> recognized new flap '%c' in queue to move to.", splitflap->id, flapBuffer[0]); // new flap to move to is available // check if ongoing move if(OngoingMoveMutex_Lock(instance)){ // now get the queue item - if(xQueueReceive(splitflap->flapQueue, &nextFlap, 0) != pdPASS){ + if(xQueueReceive(splitflap->flapQueue, &flapBuffer, 0) != pdPASS){ /* failed to receive => queue empty but previously an item was there? error */ McuLog_error("Failed to receive Queue item of Splitflap <%i>.", splitflap->id); OngoingMoveMutex_Unlock(instance); continue; // ignore & proceed } + nextFlap = flapBuffer[0]; //=> execute move - McuLog_info("Splitflap <%i> moving to flap '%c'.", splitflap->id, nextFlap[0]); - SF_MoveToFlap(instance, nextFlap); - McuLog_info("Splitflap <%i> move done to flap '%c'.", splitflap->id, nextFlap[0]); + McuLog_info("Splitflap <%i> moving to flap '%c'.", splitflap->id, nextFlap); + SF_MoveToFlap(instance, &nextFlap); + McuLog_info("Splitflap <%i> move done to flap '%c'.", splitflap->id, nextFlap); vTaskDelay(pdMS_TO_TICKS(100)); // wait for logger to finish OngoingMoveMutex_Unlock(instance); diff --git a/ADIS_tinyK22_SplitFlap/source/splitflap_positions.h b/ADIS_tinyK22_SplitFlap/source/splitflap_positions.h index 7e826b1..0d6a01c 100644 --- a/ADIS_tinyK22_SplitFlap/source/splitflap_positions.h +++ b/ADIS_tinyK22_SplitFlap/source/splitflap_positions.h @@ -17,8 +17,5 @@ Flap_t SF_Letters[] = { " ", "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", " "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "!", "?", ":"}; -/* registers offset steps for initialization per hwId. [0] = hwId. [1] = offsetSteps */ -HardwareIdentifier_t offsetStepsPerHw[][2] = { {6, 0}, {22, 13}}; - #endif /* SPLITFLAP_POSITIONS_H_ */