mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-05-04 07:10:17 +00:00
fix and update save funcs
This commit is contained in:
parent
953593d32a
commit
6746f19cf0
@ -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();
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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() {
|
||||
|
||||
}
|
||||
|
||||
@ -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.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);
|
||||
}
|
||||
@ -152,22 +209,15 @@ void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) {
|
||||
ServoMap servo = dataPtr[1];
|
||||
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
|
||||
int16_t newDef = (dataPtr[2] << 8) | dataPtr[3];
|
||||
|
||||
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;
|
||||
if (newDef < 0)
|
||||
newDef = 0;
|
||||
if (newDef > 180)
|
||||
newDef = 180;
|
||||
|
||||
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();
|
||||
|
||||
SendResponse(dataPtr[0], 0, NULL, 0);
|
||||
@ -190,7 +240,6 @@ void UART3_GetServoOffset(uint8_t *dataPtr, uint8_t len) {
|
||||
SendResponse(dataPtr[0], 0, res, sizeof(res));
|
||||
}
|
||||
|
||||
|
||||
//201
|
||||
void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
|
||||
const uint8_t MIN_PARAM_LENGTH = 3;
|
||||
@ -200,18 +249,17 @@ void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
|
||||
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;
|
||||
|
||||
if (maxAngl > 180)
|
||||
maxAngl = 180;
|
||||
if (maxAngl < 0)
|
||||
maxAngl = 0;
|
||||
currentServo->max = maxAngl;
|
||||
|
||||
saveInfoBlock();
|
||||
SendResponse(dataPtr[0], 0, NULL, 0);
|
||||
}
|
||||
|
||||
|
||||
//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))
|
||||
@ -220,15 +268,7 @@ void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
|
||||
ServoMap servo = dataPtr[1];
|
||||
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
|
||||
|
||||
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 maxAngl = (deviationToMax > deviationToMin) ? deviationToMax : deviationToMin;
|
||||
uint8_t maxAngl = currentServo->max;
|
||||
|
||||
uint8_t res[2];
|
||||
res[0] = HIGHBIT(maxAngl);
|
||||
@ -236,6 +276,41 @@ void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
|
||||
SendResponse(dataPtr[0], 0, res, sizeof(res));
|
||||
}
|
||||
|
||||
//202
|
||||
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];
|
||||
uint16_t minAngl = (dataPtr[2] << 8) | dataPtr[3];
|
||||
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
|
||||
|
||||
if (minAngl > 180)
|
||||
minAngl = 180;
|
||||
if (minAngl < 0)
|
||||
minAngl = 0;
|
||||
currentServo->min = minAngl;
|
||||
|
||||
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) {
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user