From 5b4db505c236bf3867f70e8ba762fda723930714 Mon Sep 17 00:00:00 2001 From: DashyFox Date: Sun, 22 Sep 2024 21:38:17 +0300 Subject: [PATCH] programm works --- Core/Inc/EEPROM.h | 4 +- Core/Inc/RobotFunctions.h | 8 +- Core/Src/EEPROM.c | 10 +- Core/Src/RobotFunctions.c | 290 +++++++++++++++++++++++++---------- Core/Src/UART3_CMD_Handler.c | 61 +++++--- TODO.md | 7 +- 6 files changed, 263 insertions(+), 117 deletions(-) diff --git a/Core/Inc/EEPROM.h b/Core/Inc/EEPROM.h index f38740d..63f4ac5 100644 --- a/Core/Inc/EEPROM.h +++ b/Core/Inc/EEPROM.h @@ -106,8 +106,8 @@ typedef struct __attribute__((packed)) { } HardwareInit_t; typedef struct __attribute__((packed)) { - uint32_t totalShots; - uint32_t totalPrograms; + uint32_t shotsInShot; + uint32_t shotsInProgram; uint32_t totalMacros; } Statistics; diff --git a/Core/Inc/RobotFunctions.h b/Core/Inc/RobotFunctions.h index 186ff47..4f6c8f8 100644 --- a/Core/Inc/RobotFunctions.h +++ b/Core/Inc/RobotFunctions.h @@ -36,14 +36,16 @@ typedef enum State{ typedef struct CurrentShot { uint8_t index; - uint16_t currentRepeatCount; + uint16_t currentBallCount; Shot shot; } CurrentShot; typedef struct CurrentProgram { uint8_t index; Program program; - uint8_t shot_index; + CurrentShot currentShot; + uint8_t programShotId; + uint16_t currentBallCount; } CurrentProgram; typedef struct CurrentMacro { @@ -75,7 +77,7 @@ void BallEXT(); void RobotTick(); uint8_t prepareShot(uint8_t number); -//uint8_t prepareProgramm(uint8_t number); +uint8_t prepareProgramm(uint8_t number); //uint8_t prepareMacro(uint8_t number); diff --git a/Core/Src/EEPROM.c b/Core/Src/EEPROM.c index 2af86b0..a0857fe 100644 --- a/Core/Src/EEPROM.c +++ b/Core/Src/EEPROM.c @@ -46,8 +46,8 @@ MemoryStatus EEPROM_INIT() { infoBlock.hwInfo.servos[SERVO_VERTICAL].def = 90; infoBlock.hwInfo.servos[SERVO_VERTICAL].max = 180; - infoBlock.statInfo.totalShots = 0; - infoBlock.statInfo.totalPrograms = 0; + infoBlock.statInfo.shotsInShot = 0; + infoBlock.statInfo.shotsInProgram = 0; infoBlock.statInfo.totalMacros = 0; status = saveInfoBlock(&infoBlock); @@ -112,11 +112,11 @@ MemoryStatus EEPROM_INIT() { CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer)); snprintf(buffer, sizeof(buffer), "Total Shots: %lu\n", - infoBlock.statInfo.totalShots); + infoBlock.statInfo.shotsInShot); CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer)); snprintf(buffer, sizeof(buffer), "Total Programs: %lu\n", - infoBlock.statInfo.totalPrograms); + infoBlock.statInfo.shotsInProgram); CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer)); snprintf(buffer, sizeof(buffer), "Total Macros: %lu\n\n", @@ -263,7 +263,7 @@ MemoryStatus EEPROM_EARSE() { uint16_t old_addr = 0; do { uint8_t Buf[255]; - memset(Buf, 0xFF, sizeof(Buf)); + memset(Buf, 0x00, sizeof(Buf)); FLASH_WriteBlock(addr, 0, Buf, sizeof(Buf), sizeof(Buf)); old_addr = addr; addr += sizeof(Buf); diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c index a49559f..90bca75 100644 --- a/Core/Src/RobotFunctions.c +++ b/Core/Src/RobotFunctions.c @@ -25,12 +25,22 @@ uint8_t ballDetected = 0; uint32_t ballReact_timer = 0; uint8_t isDelayTimerRun = 0; uint32_t startDelay_timer; -Shot* postDelayShot = NULL; +Shot *postDelayShot = NULL; uint32_t lastIndicationTime = 0; uint32_t noBallTimer = 0; uint32_t noBallTimeout = 0xFFFFFFFF; void BallEXT_Handler(); +Shot* getShotFromProgram(CurrentProgram *currentProg); +void robotStateStart(uint32_t delayTime); +void robotStatePause(); +void robotStateStop(); +float calculatePeriod(int pwm_value); +uint8_t nextBallinShot(CurrentShot *shot); +void shotApply(Shot *shot); +uint8_t loadNextShotInProgram(CurrentProgram *currentProg); +uint8_t loadShotFromProgram(CurrentProgram *currentProg); +Shot* getShotFromProgram(CurrentProgram *currentProg); void robotStateStart(uint32_t delayTime) { currentInfo.state = PRERUN_WAIT; @@ -51,55 +61,75 @@ void BallEXT() { ballReact_timer = millis(); } - // Функция для расчета периода вылета мяча на основе ШИМ float calculatePeriod(int pwm_value) { - // Коэффициенты из аппроксимации - float a = 100382.255; - float b = 0.21895; - float c = 883.456; + // Коэффициенты из аппроксимации + float a = 100382.255; + float b = 0.21895; + float c = 883.456; - // Расчет периода на основе экспоненциальной формулы - float period = a * expf(-b * pwm_value) + c; - return period; + // Расчет периода на основе экспоненциальной формулы + float period = a * expf(-b * pwm_value) + c; + return period; } void RobotTick() { - BallEXT_Handler(); + BallEXT_Handler(); - // No Ball Handler - if(currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout){ - robotStateStop(); - setScrewkSpeed(0); - setRollersSpeed(100, 100); - setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def, - infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def, + // No Ball Handler + if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) { + robotStateStop(); + setScrewkSpeed(0); + setRollersSpeed(100, 100); + setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def, + infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def, infoBlock.hwInfo.servos[SERVO_VERTICAL].min); - print("NO BALL!!!"); - } + print("NO BALL!!!"); + } - // PreRun delay - if (isDelayTimerRun) { - uint32_t elapsedTime = millis() - startDelay_timer; + // PreRun delay + if (isDelayTimerRun) { + uint32_t elapsedTime = millis() - startDelay_timer; - if (elapsedTime > currentInfo.startDelay) { - isDelayTimerRun = 0; - if (currentInfo.state == PRERUN_WAIT) { - currentInfo.state = RUN; - shotApply(postDelayShot); - } - } else { - uint32_t intervalStep = currentInfo.startDelay / NUMLEDS; - if (elapsedTime - lastIndicationTime >= intervalStep) { - lastIndicationTime = elapsedTime; - uint8_t progress = (elapsedTime * 100) / currentInfo.startDelay; + if (elapsedTime > currentInfo.startDelay) { + isDelayTimerRun = 0; + if (currentInfo.state == PRERUN_WAIT) { + currentInfo.state = RUN; + shotApply(postDelayShot); + } + } else { + uint32_t intervalStep = currentInfo.startDelay / NUMLEDS; + if (elapsedTime - lastIndicationTime >= intervalStep) { + lastIndicationTime = elapsedTime; + uint8_t progress = (elapsedTime * 100) / currentInfo.startDelay; // indicate(progress); - print("delay: "); - printNumber(progress); - print("\n"); - } - } - } + print("delay: "); + printNumber(progress); + print("\n"); + } + } + } +} + +uint8_t nextBallinShot(CurrentShot *shot) { + + print("shot->shot.countRepeatShot "); + printNumber(shot->shot.countRepeatShot); + print("\n"); + + if (shot->currentBallCount + 1 < shot->shot.countRepeatShot + || !shot->shot.countRepeatShot) { + // nextBall + print("Shot "); + printNumber(shot->currentBallCount); + print("\n\n"); + + shot->currentBallCount++; + return 0; + } else { + print("Shot DONE\n"); + return 1; + } } void BallEXT_Handler() { @@ -122,24 +152,30 @@ void BallEXT_Handler() { break; case ShotMode: - if (currentInfo.shot.currentRepeatCount + 1 - < currentInfo.shot.shot.countRepeatShot - || !currentInfo.shot.shot.countRepeatShot) { - // nextRepeatCount - print("Shot "); - printNumber(currentInfo.shot.currentRepeatCount); - print("\n\n"); - saveInfoBlock(); - currentInfo.shot.currentRepeatCount++; - } else { - // shotDone + if (nextBallinShot(¤tInfo.shot)) { stopShooting(); - print("Shot DONE\n"); } - infoBlock.statInfo.totalShots++; + infoBlock.statInfo.shotsInShot++; + saveInfoBlock(); break; case ProgramMode: + if (nextBallinShot(¤tInfo.program.currentShot)) { + if (!loadNextShotInProgram(¤tInfo.program)) { + stopShooting(); + print("loadNextShotInProgram ERR\n"); + } + shotApply(getShotFromProgram(¤tInfo.program)); + } + if (currentInfo.program.currentBallCount + 1 + < currentInfo.program.program.header.countRepeat) { + currentInfo.program.currentBallCount++; + } else { + stopShooting(); + print("Program DONE\n"); + } + infoBlock.statInfo.shotsInProgram++; + saveInfoBlock(); break; case MacroMode: @@ -230,38 +266,93 @@ void shotApply(Shot *shot) { shot->rotationVertical); setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom); setScrewkSpeed(shot->speedScrew); + noBallTimeout = calculatePeriod( + shot->speedScrew) * NOBALL_TIMEOUT_MULTIPLIER; noBallTimer = millis(); + print("Fire!\n"); + print("isExist "); + printNumber(shot->isExist); + print("\ncountRepeatShot; "); + printNumber(shot->countRepeatShot); + print("\nspeedRollerTop; "); + printNumber(shot->speedRollerTop); + print("\nspeedRollerBottom; "); + printNumber(shot->speedRollerBottom); + print("\nspeedScrew; "); + printNumber(shot->speedScrew); + print("\nrotationAxial; "); + printNumber(shot->rotationAxial); + print("\nrotationHorizontal; "); + printNumber(shot->rotationHorizontal); + print("\nrotationVertical; "); + printNumber(shot->rotationVertical); + print("\nnoBallTimeout: "); + printNumber(noBallTimeout); + print("\n"); + print("\n\n"); +} + +uint8_t loadShotFromProgram(CurrentProgram *currentProg) { + Shot shot_; + uint8_t currentProgramShotId = currentProg->programShotId; + if (currentProgramShotId >= currentProg->program.header.shotCount) { + print("Shot index out of range\n"); + // TODO: sound_ERR(); ledFX_ERR(); + return 0; + } + uint8_t shotId = currentProg->program.shots[currentProgramShotId].id; + currentProg->currentShot.index = shotId; + getShot(shotId, &shot_); + if (!shot_.isExist) { + print("Shot in program is NULL\n"); + // TODO: sound_ERR(); ledFX_ERR(); + return 0; + } + shot_.speedScrew = + currentProg->program.shots[currentProgramShotId].speedScrew; + uint8_t repeatCountFromShot = currentProg->program.header.options & 2U; + if (!repeatCountFromShot) { + print("Repeat Count Override\n"); + shot_.countRepeatShot = 1; + } + currentProg->currentShot.shot = shot_; + print("REPEAT "); + printNumber(shot_.countRepeatShot); + print("\n"); + currentProg->currentShot.currentBallCount = 0; + return 1; +} + +uint8_t loadNextShotInProgram(CurrentProgram *currentProg) { + uint8_t random = currentProg->program.header.options & 1U; + uint8_t *currentProgramShotId = ¤tProg->programShotId; + + if (random) { + *currentProgramShotId = rand() % currentProg->program.header.shotCount; + } else { + if ((*currentProgramShotId) + 1 + < currentProg->program.header.shotCount) { + ++(*currentProgramShotId); + } else { + *currentProgramShotId = 0; + } + } + return loadShotFromProgram(currentProg); +} + +Shot* getShotFromProgram(CurrentProgram *currentProg) { + return ¤tProg->currentShot.shot; } void startShooting(uint32_t delayTime) { + print("StartShooting\n"); switch (currentInfo.mode) { case ShotMode: - print("StartShooting\n"); if (currentInfo.shot.shot.isExist) { - print("Fire!\n"); - - print("isExist "); - printNumber(currentInfo.shot.shot.isExist); - print("\ncountRepeatShot; "); - printNumber(currentInfo.shot.shot.countRepeatShot); - print("\nspeedRollerTop; "); - printNumber(currentInfo.shot.shot.speedRollerTop); - print("\nspeedRollerBottom; "); - printNumber(currentInfo.shot.shot.speedRollerBottom); - print("\nspeedScrew; "); - printNumber(currentInfo.shot.shot.speedScrew); - print("\nrotationAxial; "); - printNumber(currentInfo.shot.shot.rotationAxial); - print("\nrotationHorizontal; "); - printNumber(currentInfo.shot.shot.rotationHorizontal); - print("\nrotationVertical; "); - printNumber(currentInfo.shot.shot.rotationVertical); - print("\n\n"); postDelayShot = ¤tInfo.shot.shot; -#if PRE_RUN_DELAY_MODE == 0 -#elif PRE_RUN_DELAY_MODE == 1 - setPosFromShot(postDelayShot); -#endif + if (PRE_RUN_DELAY_MODE) { + setPosFromShot(postDelayShot); + } robotStateStart(delayTime); } else { print("Current Shot is NULL\n"); @@ -269,10 +360,29 @@ void startShooting(uint32_t delayTime) { } break; case ProgramMode: + if (currentInfo.program.program.header.shotCount) { + if (!loadShotFromProgram(¤tInfo.program)) { + print("loadShotFromProgram ERR\n"); + break; + } + postDelayShot = getShotFromProgram(¤tInfo.program); + if (PRE_RUN_DELAY_MODE) { + setPosFromShot(postDelayShot); + } + robotStateStart(delayTime); + } else { + print("Current Program is NULL\n"); + // TODO: sound_ERR(); ledFX_ERR(); + } break; case MacroMode: + if (currentInfo.macro.macro.header.programmCount) { + } else { + print("Current Macro is NULL\n"); + // TODO: sound_ERR(); ledFX_ERR(); + } break; default: break; @@ -292,11 +402,7 @@ uint8_t prepareShot(uint8_t number) { if (shot.isExist) { currentInfo.mode = ShotMode; currentInfo.shot.shot = shot; - currentInfo.shot.currentRepeatCount = 0; - noBallTimeout = calculatePeriod(shot.speedScrew)*NOBALL_TIMEOUT_MULTIPLIER; - print("noBallTimeout: "); - printNumber(noBallTimeout); - print("\n"); + currentInfo.shot.currentBallCount = 0; return 1; } else { // TODO: sound_ERR(); ledFX_ERR(); @@ -305,6 +411,23 @@ uint8_t prepareShot(uint8_t number) { } } +uint8_t prepareProgramm(uint8_t number) { + Program program; + getProg(number, &program); + if (program.header.shotCount) { // isExist + currentInfo.mode = ProgramMode; + currentInfo.program.program = program; + currentInfo.program.index = number; + currentInfo.program.programShotId = 0; + currentInfo.program.currentBallCount = 0; + return 1; + } else { + // TODO: sound_ERR(); ledFX_ERR(); + print("program not exist\n\n"); + return 0; + } +} + void setPosSingle(ServoMap servo, uint8_t value) { ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; uint8_t inv = currentServo->invert; @@ -331,8 +454,9 @@ void setPos(uint8_t axial, uint8_t horizontal, uint8_t vertical) { setPosSingle(SERVO_VERTICAL, vertical); } -void setPosFromShot(Shot* shot){ - setPos(shot->rotationAxial, shot->rotationHorizontal, shot->rotationVertical); +void setPosFromShot(Shot *shot) { + setPos(shot->rotationAxial, shot->rotationHorizontal, + shot->rotationVertical); } void setPosDefault() { diff --git a/Core/Src/UART3_CMD_Handler.c b/Core/Src/UART3_CMD_Handler.c index 0701a1b..8678b51 100644 --- a/Core/Src/UART3_CMD_Handler.c +++ b/Core/Src/UART3_CMD_Handler.c @@ -45,11 +45,9 @@ void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) { shot.speedRollerTop = dataPtr[3] + 100; shot.speedRollerBottom = dataPtr[4] + 100; shot.speedScrew = map(dataPtr[5], 0, 120, 0, 100); - shot.rotationAxial = map((int8_t)dataPtr[6], -99, 99, 0, 180); - shot.rotationHorizontal = map((int8_t)dataPtr[7], -99, 99, 0, 180); - shot.rotationVertical = 180 - (int8_t)dataPtr[8] - 90; - - + shot.rotationAxial = map((int8_t) dataPtr[6], -99, 99, 0, 180); + shot.rotationHorizontal = map((int8_t) dataPtr[7], -99, 99, 0, 180); + shot.rotationVertical = 180 - (int8_t) dataPtr[8] - 90; saveShot(shotIndx, &shot); @@ -97,10 +95,10 @@ void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) { dataPtr[5] != 0xFF) { for (uint8_t i = 0; i < macro.header.programmCount; i++) { uint8_t pos = 2 + i * sizeof(MacroProgram); - macro.programs[i].id = dataPtr[pos+0]; - macro.programs[i].speedScrew = dataPtr[pos+2]; - macro.programs[i].countRepeat = dataPtr[pos+3]; - macro.programs[i].options = dataPtr[pos+4]; + macro.programs[i].id = dataPtr[pos + 0]; + macro.programs[i].speedScrew = dataPtr[pos + 2]; + macro.programs[i].countRepeat = dataPtr[pos + 3]; + macro.programs[i].options = dataPtr[pos + 4]; } } else { delMacro(macroIndx); @@ -109,7 +107,6 @@ void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) { SendResponse(dataPtr[0], 0, NULL, 0); } - void UART3_StartMacro(uint8_t *dataPtr, uint8_t len) { const uint8_t MIN_PARAM_LENGTH = 0; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) @@ -123,6 +120,10 @@ void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) { if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + uint8_t progIndx = dataPtr[1]; + prepareProgramm(progIndx); + startShooting(infoBlock.hwInfo.timings.preRun); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -196,7 +197,6 @@ void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) { EEPROM_EARSE(); saveInfoBlock(); - SendResponse(dataPtr[0], 0, NULL, 0); } @@ -222,7 +222,7 @@ void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) { uint8_t newDef = dataPtr[2]; printNumber(newDef); - print("\n"); + print("\n"); newDef += 90; // from center if (newDef > 180) @@ -452,26 +452,41 @@ void UART3_ReadStatistics(uint8_t *dataPtr, uint8_t len) { uint8_t macro_number; uint8_t program_number; uint8_t shot_number; - uint8_t total_macro_done_HIGH; uint8_t total_macro_done_LOW; - uint8_t total_program_done_HIGH; + uint8_t total_macro_done_HIGH; uint8_t total_program_done_LOW; - uint8_t total_shot_done_HIGH; + uint8_t total_program_done_HIGH; uint8_t total_shot_done_LOW; + uint8_t total_shot_done_HIGH; } StatusStruct; StatusStruct res; + uint8_t isRun = currentInfo.state == RUN; + res.status = isRun; - res.status = currentInfo.state == RUN; - res.shot_number = currentInfo.shot.index; - res.program_number = currentInfo.program.index; - res.macro_number = currentInfo.macro.index; + res.shot_number = 0xFF; + res.program_number = 0xFF; + res.macro_number = 0xFF; - res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.totalShots); - res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.totalShots); + switch (currentInfo.mode) { + case ShotMode: + res.shot_number = isRun ? currentInfo.shot.index : 0xFF; + break; + case ProgramMode: + res.program_number = isRun ? currentInfo.program.index : 0xFF; + break; + case MacroMode: + res.macro_number = isRun ? currentInfo.macro.index : 0xFF; + break; + default: + break; + } - res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.totalPrograms); - res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.totalPrograms); + res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInShot); + res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.shotsInShot); + + res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInProgram); + res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.shotsInProgram); res.total_macro_done_HIGH = HIGHBIT(infoBlock.statInfo.totalMacros); res.total_macro_done_LOW = LOWBIT(infoBlock.statInfo.totalMacros); diff --git a/TODO.md b/TODO.md index 77fd9c7..7542f64 100644 --- a/TODO.md +++ b/TODO.md @@ -47,5 +47,10 @@ IR: Ошибки: - V В некоторый момент PID регулятор выдаёт 0 и двигатель не запускается не зависимо от входного значенияPWM + V+- В некоторый момент PID регулятор выдаёт 0 и двигатель не запускается не зависимо от входного значенияPWM + Оранж: + При сбоях UART вешает его и иногда не переинициализирует даже после рестарта контроллера + Программы: + Есл стоит "Настроить темп ударов", во все удары приходят нули. + \ No newline at end of file