mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-05-04 07:10:17 +00:00
404 lines
10 KiB
C
404 lines
10 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"
|
|
|
|
#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;
|
|
}
|
|
|
|
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.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);
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) {
|
|
const uint8_t MIN_PARAM_LENGTH = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) {
|
|
const uint8_t MIN_PARAM_LENGTH = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
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))
|
|
return;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) {
|
|
const uint8_t MIN_PARAM_LENGTH = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
void UART3_StartShot(uint8_t *dataPtr, uint8_t len) {
|
|
const uint8_t MIN_PARAM_LENGTH = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
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;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
void UART3_DeleteShot(uint8_t *dataPtr, uint8_t len) {
|
|
const uint8_t MIN_PARAM_LENGTH = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
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;
|
|
|
|
SendResponse(dataPtr[0], 0, NULL, 0);
|
|
}
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
EEPROM_EARSE();
|
|
|
|
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.isShooting;
|
|
|
|
SendResponse(dataPtr[0], 0, &res, sizeof(res));
|
|
}
|
|
|
|
//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;
|
|
|
|
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;
|
|
|
|
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);
|
|
}
|
|
|
|
//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;
|
|
|
|
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];
|
|
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);
|
|
}
|
|
|
|
|
|
//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;
|
|
|
|
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 res[2];
|
|
res[0] = HIGHBIT(maxAngl);
|
|
res[1] = LOWBIT(maxAngl);
|
|
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 = 0;
|
|
if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
|
|
return;
|
|
|
|
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];
|
|
// res[0] = ;
|
|
|
|
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_HIGH;
|
|
uint8_t total_macro_done_LOW;
|
|
uint8_t total_program_done_HIGH;
|
|
uint8_t total_program_done_LOW;
|
|
uint8_t total_shot_done_HIGH;
|
|
uint8_t total_shot_done_LOW;
|
|
} StatusStruct;
|
|
|
|
StatusStruct res;
|
|
|
|
res.status = currentInfo.state.isShooting;
|
|
res.shot_number = currentInfo.shot.index;
|
|
res.program_number = currentInfo.program.index;
|
|
res.macro_number = currentInfo.macro.index;
|
|
|
|
res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.totalShots);
|
|
res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.totalShots);
|
|
|
|
res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.totalPrograms);
|
|
res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.totalPrograms);
|
|
|
|
res.total_macro_done_HIGH = HIGHBIT(infoBlock.statInfo.totalMacros);
|
|
res.total_macro_done_LOW = LOWBIT(infoBlock.statInfo.totalMacros);
|
|
|
|
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);
|
|
}
|
|
|