diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c index 4c34d47..c7d7f84 100644 --- a/Core/Src/RobotFunctions.c +++ b/Core/Src/RobotFunctions.c @@ -60,7 +60,7 @@ void robotStateStop() { long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } -float calculatePeriod(int pwm_value) {// Функция для расчета периода вылета мяча на основе ШИМ +float calculatePeriod(int pwm_value) { // Функция для расчета периода вылета мяча на основе ШИМ // Коэффициенты из аппроксимации float a = 100382.255; float b = 0.21895; @@ -147,7 +147,6 @@ void BallEXT() { ballReact_timer = millis(); } - void RobotTick() { BallEXT_Handler(); @@ -314,7 +313,7 @@ uint8_t loadShotFromProgram(CurrentProgram *currentProg) { if (!repeatCountFromShot) { print("Repeat Count Override\n"); shot_.countRepeatShot = 1; - }else if(shot_.countRepeatShot == 0 && currentInfo.mode != ShotMode){ + } else if (shot_.countRepeatShot == 0 && currentInfo.mode != ShotMode) { shot_.countRepeatShot = 1; } currentProg->currentShot.shot = shot_; @@ -348,7 +347,6 @@ uint8_t loadProgramFromMacro(CurrentMacro *currentMacro) { return 0; } - uint8_t programId = currentMacro->macro.programs[currentProgIndexLocal].id; currentMacro->currentProgram.indexGlobal = programId; getProg(programId, &program_); @@ -364,11 +362,12 @@ uint8_t loadProgramFromMacro(CurrentMacro *currentMacro) { && currentMacro->macro.programs[currentProgIndexLocal].speedScrew != 0; - if (needOverride) - { + if (needOverride) { print("Override programs params\n"); - program_.header.countRepeat = currentMacro->macro.programs[currentProgIndexLocal].countRepeat; - program_.header.options = currentMacro->macro.programs[currentProgIndexLocal].options; + program_.header.countRepeat = + currentMacro->macro.programs[currentProgIndexLocal].countRepeat; + program_.header.options = + currentMacro->macro.programs[currentProgIndexLocal].options; for (uint8_t i = 0; i < program_.header.shotCount; i++) { program_.shots[i].speedScrew = currentMacro->macro.programs[currentProgIndexLocal].speedScrew; @@ -378,6 +377,7 @@ uint8_t loadProgramFromMacro(CurrentMacro *currentMacro) { currentMacro->currentProgram.currentShotIndexLocal = 0; currentMacro->currentProgram.currentBallCount = 0; + loadShotFromProgram(¤tMacro->currentProgram); return 1; } @@ -412,15 +412,15 @@ uint8_t nextBallInShot(CurrentShot *shot) { } } - uint8_t nextShotInProgram(CurrentProgram *prog) { + uint8_t shotApply_f = 0; if (nextBallInShot(&prog->currentShot)) { print("\nSwitch shot\n"); if (!loadNextShotInProgram(prog)) { print("loadNextShotInProgram ERR\n"); return 0; } - shotApply(&prog->currentShot); + shotApply_f = 1; } // Проверка на завершение программы @@ -431,18 +431,21 @@ uint8_t nextShotInProgram(CurrentProgram *prog) { print("Program DONE\n"); return 1; } - + if (shotApply_f) + shotApply(&prog->currentShot); return 0; } uint8_t nextProgramInMacro(CurrentMacro *currentMacro) { +// 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(¤tMacro->currentProgram.currentShot); +// shotApply_f = 1; } // Проверка на завершение макро @@ -451,6 +454,8 @@ uint8_t nextProgramInMacro(CurrentMacro *currentMacro) { // print("Macro DONE\n"); // return 1; // } +// if (shotApply_f) +// shotApply(¤tMacro->currentProgram.currentShot); return 0; } @@ -521,7 +526,7 @@ void stopShooting() { } void shotApply(CurrentShot *currentShot) { - Shot* shot = ¤tShot->shot; + Shot *shot = ¤tShot->shot; setPos(shot->rotationAxial, shot->rotationHorizontal, shot->rotationVertical); setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);