From 3424b775aa1942a114902385b8f0180f5e255a40 Mon Sep 17 00:00:00 2001 From: DashyFox Date: Fri, 27 Sep 2024 17:28:49 +0300 Subject: [PATCH] fix logic --- Core/Inc/RobotFunctions.h | 4 ++++ Core/Inc/UART3_CMD_Handler.h | 1 + Core/Src/RobotFunctions.c | 32 +++++++++++++++++--------------- Core/Src/UART3_CMD_Handler.c | 23 ++++++++++++++++++++--- Core/Src/UART3_Handler.c | 6 +++--- 5 files changed, 45 insertions(+), 21 deletions(-) diff --git a/Core/Inc/RobotFunctions.h b/Core/Inc/RobotFunctions.h index 77739af..0a0422a 100644 --- a/Core/Inc/RobotFunctions.h +++ b/Core/Inc/RobotFunctions.h @@ -37,6 +37,7 @@ typedef struct CurrentShot { uint8_t indexGlobal; uint16_t currentBallCount; Shot shot; + uint16_t doneCount; } CurrentShot; typedef struct CurrentProgram { @@ -45,6 +46,7 @@ typedef struct CurrentProgram { CurrentShot currentShot; uint8_t currentShotIndexLocal; uint16_t currentBallCount; + uint16_t doneCount; } CurrentProgram; typedef struct CurrentMacro { @@ -53,6 +55,7 @@ typedef struct CurrentMacro { CurrentProgram currentProgram; uint8_t currentProgramIndexLocal; uint16_t currentBallCount; + uint16_t doneCount; } CurrentMacro; //typedef struct CurrentState { @@ -82,6 +85,7 @@ uint8_t prepareProgramm(uint8_t number); uint8_t prepareMacro(uint8_t number); + void startShooting(uint32_t delayTime); void stopShooting(); diff --git a/Core/Inc/UART3_CMD_Handler.h b/Core/Inc/UART3_CMD_Handler.h index c34f855..5043a32 100644 --- a/Core/Inc/UART3_CMD_Handler.h +++ b/Core/Inc/UART3_CMD_Handler.h @@ -28,6 +28,7 @@ void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len); void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len); void UART3_SetServoMinAngle(uint8_t *dataPtr, uint8_t len); void UART3_GetServoMinAngle(uint8_t *dataPtr, uint8_t len); +void UART3_ResetServoMinAngle(uint8_t *dataPtr, uint8_t len); void UART3_MoveServoToInitialPosition(uint8_t *dataPtr, uint8_t len); void UART3_SetStartupDelay(uint8_t *dataPtr, uint8_t len); void UART3_GetStartupDelay(uint8_t *dataPtr, uint8_t len); diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c index b3236d4..5fd7f2e 100644 --- a/Core/Src/RobotFunctions.c +++ b/Core/Src/RobotFunctions.c @@ -143,11 +143,12 @@ void RobotTick() { uint8_t prepareShot(uint8_t number) { Shot shot; + memset(&shot, 0x00, sizeof(shot)); getShot(number, &shot); if (shot.isExist) { currentInfo.mode = ShotMode; currentInfo.shot.shot = shot; - currentInfo.shot.currentBallCount = 0; + currentInfo.shot.indexGlobal = number; return 1; } else { // TODO: sound_ERR(); ledFX_ERR(); @@ -157,13 +158,12 @@ uint8_t prepareShot(uint8_t number) { } uint8_t prepareProgramm(uint8_t number) { Program program; + memset(&program, 0x00, sizeof(program)); getProg(number, &program); if (program.header.shotCount) { // isExist currentInfo.mode = ProgramMode; currentInfo.program.program = program; currentInfo.program.indexGlobal = number; - currentInfo.program.currentShotIndexLocal = 0; - currentInfo.program.currentBallCount = 0; return 1; } else { // TODO: sound_ERR(); ledFX_ERR(); @@ -174,13 +174,12 @@ uint8_t prepareProgramm(uint8_t number) { uint8_t prepareMacro(uint8_t number) { Macro macro; + memset(¯o, 0x00, sizeof(macro)); getMacro(number, ¯o); if (macro.header.programmCount) { currentInfo.mode = MacroMode; currentInfo.macro.macro = macro; currentInfo.macro.indexGlobal = number; - currentInfo.macro.currentProgramIndexLocal = 0; - currentInfo.macro.currentBallCount = 0; return 1; } else { @@ -283,11 +282,13 @@ uint8_t loadNextShotInProgram(CurrentProgram *currentProg) { if (random) { *currentProgramShotId = rand() % currentProg->program.header.shotCount; + } else { if ((*currentProgramShotId) + 1 < currentProg->program.header.shotCount) { ++(*currentProgramShotId); } else { + currentProg->doneCount++; *currentProgramShotId = 0; } } @@ -344,6 +345,7 @@ uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro) { currentMacro->currentProgramIndexLocal++; } else { currentMacro->currentProgramIndexLocal = 0; + currentMacro->doneCount++; } return loadProgramFromMacro(currentMacro); } @@ -365,6 +367,7 @@ uint8_t nextBallInShot(CurrentShot *shot) { return 0; } else { print("Shot DONE\n"); + shot->doneCount++; return 1; } } @@ -394,25 +397,24 @@ uint8_t nextShotInProgram(CurrentProgram *prog) { } uint8_t nextProgramInMacro(CurrentMacro *currentMacro) { -// uint8_t shotApply_f = 0; + uint8_t shotApply_f = 0; if (nextShotInProgram(¤tMacro->currentProgram)) { print("\nSwitch program\n"); if (!loadNextProgramInMacro(currentMacro)) { print("loadNextProgramInMacro ERR\n"); return 0; } - shotApply(¤tMacro->currentProgram.currentShot); -// shotApply_f = 1; + shotApply_f = 1; } // Проверка на завершение макро -// if (currentMacro->currentProgramIndexLocal < currentMacro->macro.header.programmCount) { -// } else { -// print("Macro DONE\n"); -// return 1; -// } -// if (shotApply_f) -// shotApply(¤tMacro->currentProgram.currentShot); + if (currentMacro->doneCount) { + print("Macro DONE\n"); + return 1; + } + + if (shotApply_f) + shotApply(¤tMacro->currentProgram.currentShot); return 0; } diff --git a/Core/Src/UART3_CMD_Handler.c b/Core/Src/UART3_CMD_Handler.c index c4c853d..f60ef6f 100644 --- a/Core/Src/UART3_CMD_Handler.c +++ b/Core/Src/UART3_CMD_Handler.c @@ -54,6 +54,7 @@ void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) { SendResponse(dataPtr[0], 0, NULL, 0); } +// 101 void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) { const uint8_t MIN_PARAM_LENGTH = 5; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) @@ -80,6 +81,7 @@ void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) { SendResponse(dataPtr[0], 0, NULL, 0); } +// 100 void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) { const uint8_t MIN_PARAM_LENGTH = 5; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) @@ -115,7 +117,7 @@ void UART3_StartMacro(uint8_t *dataPtr, uint8_t len) { return; uint8_t macroIndx = dataPtr[1]; - if(prepareMacro(macroIndx)) + if (prepareMacro(macroIndx)) startShooting(infoBlock.hwInfo.timings.preRun); SendResponse(dataPtr[0], 0, NULL, 0); @@ -128,7 +130,7 @@ void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) { return; uint8_t progIndx = dataPtr[1]; - if(prepareProgramm(progIndx)) + if (prepareProgramm(progIndx)) startShooting(infoBlock.hwInfo.timings.preRun); SendResponse(dataPtr[0], 0, NULL, 0); @@ -141,7 +143,7 @@ void UART3_StartShot(uint8_t *dataPtr, uint8_t len) { return; uint8_t shotIndx = dataPtr[1]; - if(prepareShot(shotIndx)) + if (prepareShot(shotIndx)) startShooting(infoBlock.hwInfo.timings.preRun); SendResponse(dataPtr[0], 0, NULL, 0); @@ -302,6 +304,21 @@ void UART3_SetServoMinAngle(uint8_t *dataPtr, uint8_t len) { SendResponse(dataPtr[0], 0, NULL, 0); } +// 204 +void UART3_ResetServoMinAngle(uint8_t *dataPtr, uint8_t len) { + const uint8_t MIN_PARAM_LENGTH = 1; + if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) + return; + + ServoMap servo = dataPtr[1]; + infoBlock.hwInfo.servos[servo].min = 0; + infoBlock.hwInfo.servos[servo].max = 180; + + saveInfoBlock(); + SendResponse(dataPtr[0], 0, NULL, 0); +} + + void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) { const uint8_t MIN_PARAM_LENGTH = 1; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) diff --git a/Core/Src/UART3_Handler.c b/Core/Src/UART3_Handler.c index f48bf1e..6feaebc 100644 --- a/Core/Src/UART3_Handler.c +++ b/Core/Src/UART3_Handler.c @@ -294,9 +294,9 @@ void UART3_CMD_Handler(uint8_t *dataPtr, uint8_t len) { case 203: UART3_MoveServoToInitialPosition(dataPtr, len); break; -// case 204: -// UART3_GetServoOffset(dataPtr, len); -// break; + case 204: + UART3_ResetServoMinAngle(dataPtr, len) ; + break; case 205: UART3_GetServoOffset(dataPtr, len); break;