From 6746f19cf058d5079d0eca018970650ee048c251 Mon Sep 17 00:00:00 2001 From: DashyFox Date: Thu, 19 Sep 2024 23:42:38 +0300 Subject: [PATCH] fix and update save funcs --- Core/Inc/EEPROM.h | 11 +- Core/Inc/RobotFunctions.h | 11 +- Core/Src/EEPROM.c | 94 ++++++++++++--- Core/Src/RobotFunctions.c | 35 ++---- Core/Src/UART3_CMD_Handler.c | 223 +++++++++++++++++++++++------------ Core/Src/UART3_Handler.c | 2 +- 6 files changed, 248 insertions(+), 128 deletions(-) diff --git a/Core/Inc/EEPROM.h b/Core/Inc/EEPROM.h index a3d70c5..f38740d 100644 --- a/Core/Inc/EEPROM.h +++ b/Core/Inc/EEPROM.h @@ -49,7 +49,7 @@ typedef struct __attribute__((packed)) { } Shot; typedef struct __attribute__((packed)) { - unsigned char isExist; + unsigned char shotCount; //isExist unsigned char countRepeat; unsigned char options; } ProgramHeader; @@ -65,7 +65,7 @@ typedef struct __attribute__((packed)) { } Program; typedef struct __attribute__((packed)) { - unsigned char isExist; + unsigned char programmCount; //isExist } MacroHeader; typedef struct __attribute__((packed)) { @@ -126,18 +126,23 @@ MemoryStatus EEPROM_INIT(); MemoryStatus EEPROM_EARSE(); MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number, - uint8_t *writeData, uint16_t dataSize); + uint8_t *writeData, uint16_t dataSize, uint16_t blockSize); MemoryStatus FLASH_ReadBlock(uint16_t startAddr, uint8_t number, uint8_t *readData, uint16_t dataSize); +MemoryStatus FLASH_DelBlock(uint16_t startAddr, uint8_t number, + uint16_t dataSize); MemoryStatus saveShot(unsigned char number, Shot *shot); MemoryStatus getShot(unsigned char number, Shot *shot); +MemoryStatus delShot(unsigned char number); MemoryStatus saveProg(unsigned char number, Program *prog); MemoryStatus getProg(unsigned char number, Program *prog); +MemoryStatus delProg(unsigned char number); MemoryStatus saveMacro(unsigned char number, Macro *macro); MemoryStatus getMacro(unsigned char number, Macro *macro); +MemoryStatus delMacro(unsigned char number); MemoryStatus saveInfoBlock(); MemoryStatus getInfoBlock(); diff --git a/Core/Inc/RobotFunctions.h b/Core/Inc/RobotFunctions.h index 442325c..cee7e7d 100644 --- a/Core/Inc/RobotFunctions.h +++ b/Core/Inc/RobotFunctions.h @@ -57,6 +57,8 @@ void doShot(Shot*); void doShotForever(uint8_t number); uint8_t prepareShot(uint8_t number); +//uint8_t prepareProgramm(uint8_t number); +//uint8_t prepareMacro(uint8_t number); void startShooting(); void stopShooting(); @@ -76,13 +78,4 @@ void setScrewkSpeed(uint8_t speed); //(-v) 0 .. 100(stop) .. 200(+v) void setRollersSpeed(uint8_t up, uint8_t down); //(-v) 0 . 100(stop) . 200(+v) -void startProgram(); // shot sequence -void startMacro(); // shot sequence - - - - - - - #endif /* INC_ROBOTFUNCTIONS_H_ */ diff --git a/Core/Src/EEPROM.c b/Core/Src/EEPROM.c index d165e2a..93c51cd 100644 --- a/Core/Src/EEPROM.c +++ b/Core/Src/EEPROM.c @@ -127,10 +127,9 @@ MemoryStatus EEPROM_INIT() { } //************************************************************// - MemoryStatus saveInfoBlock() { return FLASH_WriteBlock(START_ADR_STAT, 0, (uint8_t*) &infoBlock, - sizeof(InfoBlock)); + sizeof(InfoBlock), sizeof(InfoBlock)); } MemoryStatus getInfoBlock() { @@ -146,7 +145,7 @@ MemoryStatus getInfoBlock() { MemoryStatus saveShot(unsigned char number, Shot *shot) { if (FLASH_WriteBlock(START_ADR_SHOT, number, (uint8_t*) shot, - SHOT_BLOCKSIZE) == EEPROM_OK) { + SHOT_BLOCKSIZE, SHOT_BLOCKSIZE) == EEPROM_OK) { return EEPROM_OK; } return EEPROM_FAIL; @@ -163,9 +162,16 @@ MemoryStatus getShot(unsigned char number, Shot *shot) { return EEPROM_OK; } +MemoryStatus delShot(unsigned char number) { + if (FLASH_DelBlock(START_ADR_SHOT, number, SHOT_BLOCKSIZE) == EEPROM_OK) { + return EEPROM_OK; + } + return EEPROM_FAIL; +} + MemoryStatus saveProg(unsigned char number, Program *prog) { MemoryStatus result = EEPROM_OK; - for (uint16_t i = 0; i < MAX_SHOT_COUNT_IN_PROGRAMS; ++i) { + for (uint16_t i = 0; i < prog->header.shotCount; ++i) { Shot shot; MemoryStatus stat = getShot(prog->shots[i].id, &shot); if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) { @@ -177,7 +183,10 @@ MemoryStatus saveProg(unsigned char number, Program *prog) { } } - if (FLASH_WriteBlock(START_ADR_PROGRAM, number, (uint8_t*) prog, + uint16_t totalSize = sizeof(ProgramHeader) + + sizeof(ProgramShot) * prog->header.shotCount; + + if (FLASH_WriteBlock(START_ADR_PROGRAM, number, (uint8_t*) prog, totalSize, PROGRAM_BLOCKSIZE) != EEPROM_OK) { return EEPROM_FAIL; } @@ -190,28 +199,39 @@ MemoryStatus getProg(unsigned char number, Program *prog) { return EEPROM_FAIL; } - if (!prog->header.isExist) { + if (!prog->header.shotCount) { return EEPROM_MISSING_ELEMENT; } return EEPROM_OK; } +MemoryStatus delProg(unsigned char number) { + if (FLASH_DelBlock(START_ADR_PROGRAM, number, PROGRAM_BLOCKSIZE) + == EEPROM_OK) { + return EEPROM_OK; + } + return EEPROM_FAIL; +} + MemoryStatus saveMacro(unsigned char number, Macro *macro) { MemoryStatus result = EEPROM_OK; - for (uint16_t i = 0; i < MAX_PROGRAM_COUNT_IN_MACRO; ++i) { + for (uint16_t i = 0; i < macro->header.programmCount; ++i) { Program prog; MemoryStatus stat = getProg(macro->programs[i].id, &prog); if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) { return EEPROM_FAIL; } - if (!prog.header.isExist) { + if (!prog.header.shotCount) { result = EEPROM_MISSING_ELEMENT; // todo: добавить в запросы для загрузки программ } } - if (FLASH_WriteBlock(START_ADR_MACRO, number, (uint8_t*) macro, + uint16_t totalSize = sizeof(MacroHeader) + + sizeof(MacroProgram) * macro->header.programmCount; + + if (FLASH_WriteBlock(START_ADR_MACRO, number, (uint8_t*) macro, totalSize, MACRO_BLOCKSIZE) != EEPROM_OK) { return EEPROM_FAIL; } @@ -225,19 +245,26 @@ MemoryStatus getMacro(unsigned char number, Macro *macro) { return EEPROM_FAIL; } - if (!macro->header.isExist) { + if (!macro->header.programmCount) { return EEPROM_MISSING_ELEMENT; } return EEPROM_OK; } +MemoryStatus delMacro(unsigned char number) { + if (FLASH_DelBlock(START_ADR_MACRO, number, MACRO_BLOCKSIZE) == EEPROM_OK) { + return EEPROM_OK; + } + return EEPROM_FAIL; +} + MemoryStatus EEPROM_EARSE() { uint16_t addr = 0; uint16_t old_addr = 0; do { uint8_t Buf[255]; memset(Buf, 0xFF, sizeof(Buf)); - FLASH_WriteBlock(addr, 0, Buf, (uint8_t) sizeof(Buf)); + FLASH_WriteBlock(addr, 0, Buf, sizeof(Buf), sizeof(Buf)); old_addr = addr; addr += sizeof(Buf); } while (old_addr <= addr); @@ -246,7 +273,42 @@ MemoryStatus EEPROM_EARSE() { } MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number, - uint8_t *writeData, uint16_t dataSize) { + uint8_t *writeData, uint16_t dataSize, uint16_t blockSize) { + HAL_StatusTypeDef result; + + // Проверка на корректность входных данных + if ((startAddr == START_ADR_SHOT && number >= MAX_SHOT_COUNT) + || (startAddr == START_ADR_PROGRAM && number >= MAX_PROGRAM_COUNT) + || (startAddr == START_ADR_MACRO && number >= MAX_MACRO_COUNT)) { + return EEPROM_OUT_OF_RANGE; + } + + uint16_t blockAddr16 = (uint16_t) (startAddr + + (uint16_t) (number * blockSize)); + uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) }; + + unsigned char Buf[dataSize + 2]; + memset(Buf, 0x00, sizeof(Buf)); + Buf[0] = blockAddr[0]; + Buf[1] = blockAddr[1]; + + for (unsigned char i = 0; i < dataSize; i++) + Buf[i + 2] = writeData[i]; + + result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf, + (dataSize + 2), 10); + print("Written "); + printNumber(dataSize); + print("bytes\n"); + HAL_Delay(1); + if (result != HAL_OK) { + return EEPROM_FAIL; + } + return EEPROM_OK; +} + +MemoryStatus FLASH_DelBlock(uint16_t startAddr, uint8_t number, + uint16_t dataSize) { HAL_StatusTypeDef result; // Проверка на корректность входных данных @@ -260,16 +322,14 @@ MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number, + (uint16_t) (number * dataSize)); uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) }; - unsigned char Buf[dataSize + 2]; + unsigned char Buf[2 + 1]; memset(Buf, 0x00, sizeof(Buf)); Buf[0] = blockAddr[0]; Buf[1] = blockAddr[1]; - - for (unsigned char i = 0; i < dataSize; i++) - Buf[i + 2] = writeData[i]; + Buf[2] = 0; result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf, - (dataSize + 2), 10); + sizeof(Buf), 10); HAL_Delay(1); if (result != HAL_OK) { return EEPROM_FAIL; diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c index 31ec6b1..f1ec1d2 100644 --- a/Core/Src/RobotFunctions.c +++ b/Core/Src/RobotFunctions.c @@ -169,6 +169,7 @@ uint8_t prepareShot(uint8_t number) { return 1; } else { // TODO: sound_ERR(); ledFX_ERR(); + print("shot not exist\n\n"); return 0; } } @@ -179,24 +180,18 @@ void setPosSingle(ServoMap servo, uint8_t value) { if (servo == SERVO_AXIAL) inv = !inv; - uint8_t deviationToMax = currentServo->max > currentServo->def - ? currentServo->max - currentServo->def - : currentServo->def - currentServo->max; - - uint8_t deviationToMin = currentServo->def > currentServo->min - ? currentServo->def - currentServo->min - : currentServo->min - currentServo->def; - - uint8_t maxDeviation = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin; - uint8_t minLimit = (currentServo->def >= maxDeviation) ? currentServo->def - maxDeviation : 0; - uint8_t maxLimit = (currentServo->def + maxDeviation <= 180) ? currentServo->def + maxDeviation : 180; - if (value > maxLimit) { - value = maxLimit; - } else if (value < minLimit) { - value = minLimit; - } if (inv) value = 180 - value; + + if(value > 90){ + value = map(value, 91, 180, currentServo->def, currentServo->max); + }else if(value < 90) { + value = map(value, 89, 0, currentServo->def, currentServo->min); + } else { + value = currentServo->def; + } + + SetServo(servo, value); } @@ -256,11 +251,3 @@ void setRollersSpeed(uint8_t up, uint8_t down) { Vz1 = 200 - up; // invert Vz2 = down; } -// shot sequence -void startProgram() { - -} -// shot sequence -void startMacro() { - -} diff --git a/Core/Src/UART3_CMD_Handler.c b/Core/Src/UART3_CMD_Handler.c index e82d7de..f4bb33f 100644 --- a/Core/Src/UART3_CMD_Handler.c +++ b/Core/Src/UART3_CMD_Handler.c @@ -39,29 +39,70 @@ void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) { uint8_t shotIndx = dataPtr[1]; Shot shot; + shot.isExist = 1; shot.countRepeatShot = dataPtr[2]; - shot.speedRollerTop = dataPtr[3]+100; - shot.speedRollerBottom = dataPtr[4]+100; - shot.speedScrew = map(dataPtr[6], 0, 120, 0, 100); - shot.rotationAxial = map(dataPtr[5], -99, 99, 0, 180); - shot.rotationHorizontal = map(dataPtr[6], -99, 99, 90-45, 90+45); + shot.speedRollerTop = dataPtr[3] + 100; + shot.speedRollerBottom = dataPtr[4] + 100; + shot.speedScrew = map(dataPtr[5], 0, 120, 0, 100); + shot.rotationAxial = map(dataPtr[6], -99, 99, 0, 180); + shot.rotationHorizontal = map(dataPtr[7], -99, 99, 90 - 45, 90 + 45); + shot.rotationVertical = 180 - dataPtr[8]; + + saveShot(shotIndx, &shot); SendResponse(dataPtr[0], 0, NULL, 0); } void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 0; + const uint8_t MIN_PARAM_LENGTH = 5; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + Program prog; + uint8_t progIndx = dataPtr[1]; + prog.header.shotCount = (len - 3) / sizeof(ProgramShot); + prog.header.countRepeat = dataPtr[2]; + prog.header.options = dataPtr[3]; + + if (dataPtr[4] != 0xFF && dataPtr[5]) { + for (uint8_t i = 0; i < prog.header.shotCount; i++) { + uint8_t pos = 4 + i * sizeof(ProgramShot); + prog.shots[i].id = dataPtr[pos + 0]; + prog.shots[i].speedScrew = dataPtr[pos + 1]; + } + } else { + delProg(progIndx); + } + + saveProg(progIndx, &prog); + SendResponse(dataPtr[0], 0, NULL, 0); } void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 0; + const uint8_t MIN_PARAM_LENGTH = 5; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + Macro macro; + uint8_t macroIndx = dataPtr[1]; + macro.header.programmCount = (len - 1) / sizeof(MacroProgram); + + if (/**/dataPtr[2] != 0xFF && // + dataPtr[3] != 0xFF && // + dataPtr[4] != 0xFF && // + 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]; + } + } else { + delMacro(macroIndx); + } + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -82,10 +123,14 @@ void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) { } void UART3_StartShot(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 0; + const uint8_t MIN_PARAM_LENGTH = 1; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + uint8_t shotIndx = dataPtr[1]; + prepareShot(shotIndx); + startShooting(); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -98,10 +143,13 @@ void UART3_Stop(uint8_t *dataPtr, uint8_t len) { } void UART3_DeleteShot(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 0; + const uint8_t MIN_PARAM_LENGTH = 1; if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + uint8_t shotIndex = dataPtr[1]; + delShot(shotIndex); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -110,6 +158,9 @@ void UART3_DeleteProgram(uint8_t *dataPtr, uint8_t len) { if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + uint8_t progIndex = dataPtr[1]; + delProg(progIndex); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -118,6 +169,9 @@ void UART3_DeleteMacro(uint8_t *dataPtr, uint8_t len) { if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + uint8_t macroIndex = dataPtr[1]; + delMacro(macroIndex); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -127,7 +181,10 @@ void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) { if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) return; + getInfoBlock(); EEPROM_EARSE(); + saveInfoBlock(); + SendResponse(dataPtr[0], 0, NULL, 0); } @@ -145,97 +202,115 @@ void UART3_GetDeviceStatus(uint8_t *dataPtr, uint8_t len) { //200 void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 3; - if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) - return; + const uint8_t MIN_PARAM_LENGTH = 3; + if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) + return; - ServoMap servo = dataPtr[1]; - ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; - int16_t newDef = (dataPtr[2] << 8) | dataPtr[3]; - newDef+=90; // from center + ServoMap servo = dataPtr[1]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + int16_t newDef = (dataPtr[2] << 8) | dataPtr[3]; - if (newDef < 0) newDef = 0; - if (newDef > 180) newDef = 180; + newDef += 90; // from center + if (newDef < 0) + newDef = 0; + if (newDef > 180) + newDef = 180; - int16_t maxDeviation = (currentServo->max > currentServo->def) - ? currentServo->max - currentServo->def - : currentServo->def - currentServo->min; + currentServo->def = newDef; - currentServo->def = newDef; - int16_t newMax = currentServo->def + maxDeviation; - int16_t newMin = currentServo->def - maxDeviation; - if (newMax > 180) newMax = 180; - if (newMin < 0) newMin = 0; - currentServo->max = newMax; - currentServo->min = newMin; - saveInfoBlock(); + saveInfoBlock(); - SendResponse(dataPtr[0], 0, NULL, 0); + SendResponse(dataPtr[0], 0, NULL, 0); } //204 void UART3_GetServoOffset(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 1; - if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) - return; + const uint8_t MIN_PARAM_LENGTH = 1; + if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) + return; - ServoMap servo = dataPtr[1]; - ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + ServoMap servo = dataPtr[1]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; - int16_t def = currentServo->def - 90; // offset from center + int16_t def = currentServo->def - 90; // offset from center - uint8_t res[2]; - res[0] = HIGHBIT(def); - res[1] = LOWBIT(def); - SendResponse(dataPtr[0], 0, res, sizeof(res)); + uint8_t res[2]; + res[0] = HIGHBIT(def); + res[1] = LOWBIT(def); + SendResponse(dataPtr[0], 0, res, sizeof(res)); } - //201 void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 3; - if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) - return; + const uint8_t MIN_PARAM_LENGTH = 3; + if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) + return; - ServoMap servo = dataPtr[1]; - uint16_t maxAngl = (dataPtr[2] << 8) | dataPtr[3]; - ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; - int16_t newMax = currentServo->def + maxAngl; - int16_t newMin = currentServo->def - maxAngl; - if (newMax > 180) newMax = 180; - if (newMin < 0) newMin = 0; - currentServo->max = newMax; - currentServo->min = newMin; - saveInfoBlock(); - SendResponse(dataPtr[0], 0, NULL, 0); + ServoMap servo = dataPtr[1]; + uint16_t maxAngl = (dataPtr[2] << 8) | dataPtr[3]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + + if (maxAngl > 180) + maxAngl = 180; + if (maxAngl < 0) + maxAngl = 0; + currentServo->max = maxAngl; + + 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)) + return; + + ServoMap servo = dataPtr[1]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + + uint8_t maxAngl = currentServo->max; + + uint8_t res[2]; + res[0] = HIGHBIT(maxAngl); + res[1] = LOWBIT(maxAngl); + SendResponse(dataPtr[0], 0, res, sizeof(res)); +} //202 -void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) { - const uint8_t MIN_PARAM_LENGTH = 1; - if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) - return; +void UART3_SetServoMinAngle(uint8_t *dataPtr, uint8_t len) { + const uint8_t MIN_PARAM_LENGTH = 3; + if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH)) + return; - ServoMap servo = dataPtr[1]; - ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + ServoMap servo = dataPtr[1]; + uint16_t minAngl = (dataPtr[2] << 8) | dataPtr[3]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; - uint8_t deviationToMax = currentServo->max > currentServo->def - ? currentServo->max - currentServo->def - : currentServo->def - currentServo->max; + if (minAngl > 180) + minAngl = 180; + if (minAngl < 0) + minAngl = 0; + currentServo->min = minAngl; - uint8_t deviationToMin = currentServo->def > currentServo->min - ? currentServo->def - currentServo->min - : currentServo->min - currentServo->def; - - uint8_t maxAngl = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin; - - uint8_t res[2]; - res[0] = HIGHBIT(maxAngl); - res[1] = LOWBIT(maxAngl); - SendResponse(dataPtr[0], 0, res, sizeof(res)); + saveInfoBlock(); + SendResponse(dataPtr[0], 0, NULL, 0); } +void UART3_GetServoMinAngle(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]; + ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo]; + + uint8_t minAngl = currentServo->min; + + uint8_t res[2]; + res[0] = HIGHBIT(minAngl); + res[1] = LOWBIT(minAngl); + SendResponse(dataPtr[0], 0, res, sizeof(res)); +} //203 void UART3_MoveServoToInitialPosition(uint8_t *dataPtr, uint8_t len) { diff --git a/Core/Src/UART3_Handler.c b/Core/Src/UART3_Handler.c index 1d9499c..fa911af 100644 --- a/Core/Src/UART3_Handler.c +++ b/Core/Src/UART3_Handler.c @@ -250,7 +250,7 @@ void UART3_CMD_Handler(uint8_t *dataPtr, uint8_t len) { UART3_SetServoMaxAngle(dataPtr, len); break; case 202: - UART3_GetServoMaxAngle(dataPtr, len); + UART3_SetServoMinAngle(dataPtr, len); break; case 203: UART3_MoveServoToInitialPosition(dataPtr, len);