PingPong/Core/Src/UART3_CMD_Handler.c
2024-10-09 15:10:36 +03:00

569 lines
14 KiB
C

/*
* UART3_CMD_Handler.c
*
* Created on: Sep 12, 2024
* Author: DashyFox
*/
#include "UART3_CMD_Handler.h"
#include "Print.h"
#include "RobotFunctions.h"
#include "EEPROM.h"
#include "Indicator.h"
#include "SoundMelody.h"
#define HIGHBIT(b) (((b)>>8)&0xff)
#define LOWBIT(b) ((b)&0xff)
extern CurrentInfo currentInfo;
extern InfoBlock infoBlock;
extern void SendResponse(uint8_t command, uint8_t result, uint8_t *data,
uint8_t data_length);
uint8_t checkLen(uint8_t cmd, uint8_t current_length, uint8_t required_length) {
if (current_length < required_length) {
print("Invalid length for command ");
printNumber(cmd);
print(" len = ");
printNumber(current_length);
print("\n");
return 0;
}
return 1;
}
static void NULL_FUNC(){};
static void (*onSave)() = NULL_FUNC;
static void resetOnSave(){onSave = NULL_FUNC;};
uint8_t isSyncFirstSave;
static void onLedIdle(){
isSyncFirstSave = 0;
for (int i = 0; i < 9; ++i) {
led_writeMirror(i, 1);
}
HAL_Delay(25);
led_clear();
resetOnSave();
melody(melody_start);
}
static void onSaveCallback(){
if(isSyncFirstSave){
isSyncFirstSave = 0;
led_PingPong_startMirror(3, 9);
for (int i = 0; i < 3; ++i) {
led_writeMirror(i, 1);
}
}
led_PingPong_next();
}
// 10
void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 8;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
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[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;
saveShot(shotIndx, &shot);
onSaveCallback();
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))
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] != 0xFF) {
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);
onSaveCallback();
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))
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 + 1];
macro.programs[i].countRepeat = dataPtr[pos + 2];
macro.programs[i].options = dataPtr[pos + 3];
}
saveMacro(macroIndx, &macro);
} else {
delMacro(macroIndx);
}
onSaveCallback();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 100
void UART3_StartMacro(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t macroIndx = dataPtr[1];
if (prepareMacro(macroIndx))
startShooting(infoBlock.hwInfo.timings.preRun);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 101
void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t progIndx = dataPtr[1];
if (prepareProgramm(progIndx))
startShooting(infoBlock.hwInfo.timings.preRun);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 102
void UART3_StartShot(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t shotIndx = dataPtr[1];
if (prepareShot(shotIndx))
startShooting(infoBlock.hwInfo.timings.preRun);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 110
void UART3_Stop(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
stopShooting();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 13
void UART3_DeleteShot(uint8_t *dataPtr, uint8_t len) {
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);
}
// 14
void UART3_DeleteProgram(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t progIndex = dataPtr[1];
delProg(progIndex);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 15
void UART3_DeleteMacro(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t macroIndex = dataPtr[1];
delMacro(macroIndex);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 120
void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
isSyncFirstSave = 1;
led_setIdleCallback(onLedIdle);
onSave = onSaveCallback;
getInfoBlock();
EEPROM_EARSE();
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 180
void UART3_GetDeviceStatus(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t res = currentInfo.state == RUN;
SendResponse(dataPtr[0], 0, &res, sizeof(res));
}
// 200
void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 2;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
ServoMap servo = dataPtr[1];
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
uint8_t newDef = dataPtr[2];
printNumber(newDef);
print("\n");
newDef += 90; // from center
if (newDef > 180)
newDef = 180;
currentServo->def = newDef;
printNumber(newDef);
print("\n");
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 205
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;
ServoMap servo = dataPtr[1];
ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
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));
}
// 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;
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);
}
// 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);
}
// 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))
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));
}
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) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
setPosDefaultSingle(dataPtr[1]);
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 206
void UART3_SetStartupDelay(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 2;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
infoBlock.hwInfo.timings.preRun = (dataPtr[1] << 8) | dataPtr[2];
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 207
void UART3_GetStartupDelay(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t res[2];
res[0] = HIGHBIT(infoBlock.hwInfo.timings.preRun);
res[1] = LOWBIT(infoBlock.hwInfo.timings.preRun);
SendResponse(dataPtr[0], 0, res, sizeof(res));
}
// 210 !!!!!!!!
void UART3_SetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
infoBlock.hwInfo.motors.speed_Rollers_min = dataPtr[1];
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 211 !!!!!!!!!!!
void UART3_GetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t res[1] = {infoBlock.hwInfo.motors.speed_Rollers_min};
SendResponse(dataPtr[0], 0, res, sizeof(res));
}
// 212
void UART3_SetMinScrewSpeed(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
infoBlock.hwInfo.motors.speed_Screw_min = dataPtr[1];
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 215
void UART3_GetMinScrewSpeed(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t res[1];
res[0] = infoBlock.hwInfo.motors.speed_Screw_min;
SendResponse(dataPtr[0], 0, res, sizeof(res));
}
// 214
void UART3_SetServoInvertFlag(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 1;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t inv = dataPtr[1];
infoBlock.hwInfo.servos[SERVO_AXIAL].invert = inv;
infoBlock.hwInfo.servos[SERVO_HORIZONTAL].invert = inv;
infoBlock.hwInfo.servos[SERVO_VERTICAL].invert = inv;
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}
// 215
void UART3_GetServoInvertFlag(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
uint8_t res[1];
res[0] = (infoBlock.hwInfo.servos[SERVO_AXIAL].invert
|| infoBlock.hwInfo.servos[SERVO_HORIZONTAL].invert
|| infoBlock.hwInfo.servos[SERVO_VERTICAL].invert);
SendResponse(dataPtr[0], 0, res, sizeof(res));
}
// 181
void UART3_ReadStatistics(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
typedef struct __attribute__((packed)) {
uint8_t status;
uint8_t macro_number;
uint8_t program_number;
uint8_t shot_number;
uint8_t total_macro_done_LOW;
uint8_t total_macro_done_HIGH;
uint8_t total_program_done_LOW;
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 || currentInfo.state == PRERUN_WAIT;
res.status = isRun;
res.shot_number = 0xFF;
res.program_number = 0xFF;
res.macro_number = 0xFF;
switch (currentInfo.mode) {
case ShotMode:
res.shot_number = isRun ? currentInfo.shot.indexGlobal : 0xFF;
break;
case ProgramMode:
res.program_number = isRun ? currentInfo.program.indexGlobal : 0xFF;
break;
case MacroMode:
res.macro_number = isRun ? currentInfo.macro.indexGlobal : 0xFF;
break;
default:
break;
}
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.shotInMacro);
res.total_macro_done_LOW = LOWBIT(infoBlock.statInfo.shotInMacro);
SendResponse(dataPtr[0], 0, (uint8_t*) &res, sizeof(res));
}
// 121
void UART3_ResetStatistics(uint8_t *dataPtr, uint8_t len) {
const uint8_t MIN_PARAM_LENGTH = 0;
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
return;
memset(&infoBlock.statInfo, 0x00, sizeof(infoBlock.statInfo));
saveInfoBlock();
SendResponse(dataPtr[0], 0, NULL, 0);
}