uint8_t ESC_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
int32_t val;
const unsigned char *p;
ESC_MotorID id;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"esc help")==0) {
ESC_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"esc status")==0) {
ESC_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"esc on", sizeof("esc on")-1)==0) {
ESC_isMotorOn = TRUE;
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"esc off", sizeof("esc off")-1)==0) {
for(id=0; id<ESC_MOTOR_NOF; id++) {
ESC_SetSpeedPercent(&ESC_motors[id], 0);
}
ESC_isMotorOn = FALSE;
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"esc duty ", sizeof("esc duty ")-1)==0) {
if (!ESC_isMotorOn) {
CLS1_SendStr((unsigned char*)"Motors are OFF, cannot set duty.\r\n", io->stdErr);
res = ERR_FAILED;
} else {
p = cmd+sizeof("esc duty ")-1;
if (UTIL1_strncmp((char*)p, (char*)"fl ", sizeof("fl ")-1)==0) {
id = ESC_MOTOR_FRONT_LEFT;
} else if (UTIL1_strncmp((char*)p, (char*)"fr ", sizeof("fr ")-1)==0) {
id = ESC_MOTOR_FRONT_RIGHT;
} else if (UTIL1_strncmp((char*)p, (char*)"bl ", sizeof("bl ")-1)==0) {
id = ESC_MOTOR_BACK_LEFT;
} else if (UTIL1_strncmp((char*)p, (char*)"br ", sizeof("br ")-1)==0) {
id = ESC_MOTOR_BACK_RIGHT;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, motor must be either fl, fr, bl or br\r\n", io->stdErr);
res = ERR_FAILED;
}
if (res==ERR_OK) {
p = cmd+sizeof("esc duty xx ")-1;
if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=0 && val<=100) {
ESC_SetSpeedPercent(&ESC_motors[id], (ESC_SpeedPercent)val);
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range 0..100\r\n", io->stdErr);
res = ERR_FAILED;
}
}
}
}
return res;
}
void ESC_ChangeSpeedPercent(ESC_MotorDevice *motor, ESC_SpeedPercent relPercent) {
relPercent += motor->currSpeedPercent; /* make absolute number */
if (relPercent>100) { /* check for overflow */
relPercent = 100;
} else if (relPercent<-100) { /* and underflow */
relPercent = -100;
}
ESC_SetSpeedPercent(motor, relPercent); /* set speed. This will care about the direction too */
}
void ESC_Init(void) {
ESC_MotorID id;
ESC_isMotorOn = TRUE;
ESC_motors[ESC_MOTOR_FRONT_LEFT].SetRatio16 = ESC_SetRatio16_FR;
ESC_motors[ESC_MOTOR_FRONT_RIGHT].SetRatio16 = ESC_SetRatio16_FR;
ESC_motors[ESC_MOTOR_BACK_RIGHT].SetRatio16 = ESC_SetRatio16_FR;
ESC_motors[ESC_MOTOR_BACK_LEFT].SetRatio16 = ESC_SetRatio16_FR;
for(id=0; id<ESC_MOTOR_NOF; id++) {
ESC_SetSpeedPercent(&ESC_motors[id], 0);
}
PWMFR_Enable();
}
void ESC_Init(void) {
ESC_MotorID id;
ESC_isMotorOn = TRUE;
ESC_motors[ESC_MOTOR_FRONT_LEFT].device = PWM1_DeviceData;
ESC_motors[ESC_MOTOR_FRONT_LEFT].SetOffsetTicks = ESC_SetOffsetTicks_FL;
ESC_motors[ESC_MOTOR_FRONT_LEFT].SetDutyUS = ESC_SetDutyUS_FL;
ESC_motors[ESC_MOTOR_FRONT_RIGHT].device = PWM2_DeviceData;
ESC_motors[ESC_MOTOR_FRONT_RIGHT].SetOffsetTicks = ESC_SetOffsetTicks_FR;
ESC_motors[ESC_MOTOR_FRONT_RIGHT].SetDutyUS = ESC_SetDutyUS_FR;
ESC_motors[ESC_MOTOR_BACK_RIGHT].device = PWM3_DeviceData;
ESC_motors[ESC_MOTOR_BACK_RIGHT].SetOffsetTicks = ESC_SetOffsetTicks_BR;
ESC_motors[ESC_MOTOR_BACK_RIGHT].SetDutyUS = ESC_SetDutyUS_BR;
ESC_motors[ESC_MOTOR_BACK_LEFT].device = PWM4_DeviceData;
ESC_motors[ESC_MOTOR_BACK_LEFT].SetOffsetTicks = ESC_SetOffsetTicks_BL;
ESC_motors[ESC_MOTOR_BACK_LEFT].SetDutyUS = ESC_SetDutyUS_BL;
for(id=0; id<ESC_MOTOR_NOF; id++) {
ESC_SetSpeedPercent(&ESC_motors[id], 0);
}
}