void DRV_Init(void) {
DRV_EnableDisable(FALSE);
DRV_SpeedLeft = 0;
DRV_SpeedRight = 0;
DRV_EnableDisablePos(FALSE);
DRV_PosLeft = 30000;
DRV_PosRight = 30000;
Q4CLeft_SetPos(30000);
Q4CRight_SetPos(30000);
if (FRTOS1_xTaskCreate(
DriveTask, /* pointer to the task */
"Drive", /* task name for kernel awareness debugging */
configMINIMAL_STACK_SIZE, /* task stack size */
(void*)NULL, /* optional task startup argument */
tskIDLE_PRIORITY, /* initial priority */
(xTaskHandle*)NULL /* optional task handle to create */
) != pdPASS) {
/*lint -e527 */
for(;;){} /* error! probably out of memory */
/*lint +e527 */
}
}
/*!
* \brief Parses a command
* \param cmd Command string to be parsed
* \param handled Sets this variable to TRUE if command was handled
* \param io I/O stream to be used for input/output
* \return Error code, ERR_OK if everything was fine
*/
uint8_t Q4CLeft_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io)
{
uint8_t res=ERR_OK;
if (UTIL1_strcmp((const char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((const char *)cmd, "Q4CLeft help")==0) {
CLS1_SendHelpStr((const unsigned char*)"Q4CLeft", (const unsigned char*)"Q4CLeft command group\r\n", io->stdOut);
CLS1_SendHelpStr((const unsigned char*)" help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
CLS1_SendHelpStr((const unsigned char*)" reset", (const unsigned char*)"Reset the current position counter\r\n", io->stdOut);
*handled = TRUE;
} else if (UTIL1_strcmp((const char*)cmd, CLS1_CMD_STATUS)==0 || UTIL1_strcmp((const char*)cmd, "Q4CLeft status")==0) {
CLS1_SendStr((const unsigned char*)"Q4CLeft:\r\n", io->stdOut);
CLS1_SendStatusStr((const unsigned char*)" pos", (const unsigned char*)"", io->stdOut);
#if Q4CLeft_CNTR_BITS==16
CLS1_SendNum16u(Q4CLeft_currPos, io->stdOut);
#elif Q4CLeft_CNTR_BITS==32
CLS1_SendNum32u(Q4CLeft_currPos, io->stdOut);
#else
#error "unknown counter size!"
#endif
CLS1_SendStr((const unsigned char*)", ", io->stdOut);
#if Q4CLeft_CNTR_BITS==16
CLS1_SendNum16s((int16_t)Q4CLeft_currPos, io->stdOut);
#elif Q4CLeft_CNTR_BITS==32
CLS1_SendNum32s((int32_t)Q4CLeft_currPos, io->stdOut);
#else
#error "unknown counter size!"
#endif
CLS1_SendStr((const unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((const unsigned char*)" C1 C2", (const unsigned char*)"", io->stdOut);
if (Q4CLeft_GET_C1_PIN()!=0) {
CLS1_SendStr((const unsigned char*)"1 ", io->stdOut);
} else {
CLS1_SendStr((const unsigned char*)"0 ", io->stdOut);
}
if (Q4CLeft_GET_C2_PIN()!=0) {
CLS1_SendStr((const unsigned char*)"1\r\n", io->stdOut);
} else {
CLS1_SendStr((const unsigned char*)"0\r\n", io->stdOut);
}
CLS1_SendStatusStr((const unsigned char*)" errors", (const unsigned char*)"", io->stdOut);
CLS1_SendNum16u(Q4CLeft_nofErrors, io->stdOut);
CLS1_SendStr((const unsigned char*)"\r\n", io->stdOut);
*handled = TRUE;
} else if (UTIL1_strcmp((const char*)cmd, "Q4CLeft reset")==0) {
Q4CLeft_SetPos(0);
Q4CLeft_nofErrors = 0;
*handled = TRUE;
}
return res;
}
static portTASK_FUNCTION(DriveTask, pvParameters) {
(void)pvParameters; /* parameter not used */
bool prevOnS, prevOnP;
uint32_t i;
int32_t PosL,PosR;
uint32_t dL,dR;
uint32_t sL,sR;
//traceLabel usrEvent;
prevOnS = DRV_SpeedOn;
prevOnP = DRV_PosOn;
sL = 30000;
sR = 30000;
#if PL_HAS_RTOS_TRACE
//usrEvent = xTraceOpenLabel("drive");
#endif
for(;;) {
#if PL_HAS_RTOS_TRACE
//RTOSTRC1_vTraceUserEvent(usrEvent);
#endif
/*! \todo extend this for your own needs and with a position PID */
TACHO_CalcSpeed();
if (prevOnS && !DRV_SpeedOn) { /* speed turned off */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
PID_Start(); /* reset values */
} else if (DRV_SpeedOn && !DRV_PosOn) { /*speed control*/
PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
} else if (DRV_PosOn && !DRV_SpeedOn) { /*pos control*/
PosL = Q4CLeft_GetPos();
PosR = Q4CRight_GetPos();
dL = ((PosL-DRV_PosLeft)<=0) ? -(PosL-DRV_PosLeft) : (PosL-DRV_PosLeft);
dR = ((PosR-DRV_PosRight)<=0) ? -(PosR-DRV_PosRight) : (PosR-DRV_PosRight);
if(dL>100){
if((DRV_PosLeft-PosL)>0){
sL += 15;
} else{
sL -= 15;
}
} else{
sL = DRV_PosLeft;
}
if(dR>100){
if((DRV_PosRight-PosR)>0){
sR += 15;
} else{
sR -= 15;
}
} else{
sR = DRV_PosRight;
}
PID_Pos(PosL, sL, TRUE); //left
PID_Pos(PosR, sR, FALSE); //right
if((PosR < 1000) || (PosR > 63000)){ /*range safety*/
DRV_PosRight = 30000;
Q4CRight_SetPos(30000);
}
if((PosL < 1000) || (PosL > 63000)){
DRV_PosLeft = 30000;
Q4CLeft_SetPos(30000);
}
} else if(prevOnP && !DRV_PosOn){ /*pos turned off*/
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
for(i=0;i<100000;i++);
DRV_PosLeft = 30000;
DRV_PosRight = 30000;
Q4CLeft_SetPos(30000);
Q4CRight_SetPos(30000);
}
prevOnS = DRV_SpeedOn;
prevOnP = DRV_PosOn;
FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
} /* for */
}
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
const unsigned char *p;
int32_t val1, val2;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) {
DRV_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) {
DRV_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed ", sizeof("drive speed ")-1)==0) {
p = cmd+sizeof("drive speed");
if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
if (DRV_SetSpeed(val1, val2)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
} else {
CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
res = ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
Q4CLeft_SetPos(0);
Q4CRight_SetPos(0);
if (DRV_SetPos(0, 0)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) {
p = cmd+sizeof("drive pos");
if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
if (DRV_SetPos(val1, val2)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
} else {
CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
res = ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) {
p = cmd+sizeof("drive mode");
if (UTIL1_strcmp((char*)p, (char*)"none")==0) {
if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"stop")==0) {
if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"speed")==0) {
if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"pos")==0) {
if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) {
res = ERR_FAILED;
}
} else {
res = ERR_FAILED;
}
if (res!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
}
return res;
}