static portTASK_FUNCTION(DriveTask, pvParameters) {
(void)pvParameters; /* parameter not used */
bool prevOn;
//traceLabel usrEvent;
prevOn = DRV_SpeedOn;
#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 (prevOn && !DRV_SpeedOn) { /* 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) {
PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
}
prevOn = DRV_SpeedOn;
FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
} /* for */
}
void LF_StartFollowing(void) {
#if PL_APP_LINE_MAZE
if (!MAZE_IsSolved()) {
MAZE_Init();
} else {
LF_solvedIdx = 0;
}
#endif
PID_Start();
if (REF_CanUseSensor()) {
LF_currState = STATE_FOLLOW_SEGMENT;
} else {
CLS1_SendStr((unsigned char*)"Sensors not ready!\r\n", CLS1_GetStdio()->stdOut);
}
}
static uint8_t GetCmd(void) {
DRV_Command cmd;
portBASE_TYPE res;
res = FRTOS1_xQueueReceive(DRV_Queue, &cmd, 0);
if (res==errQUEUE_EMPTY) {
return ERR_RXEMPTY; /* no command */
}
/* process command */
FRTOS1_taskENTER_CRITICAL();
if (cmd.cmd==DRV_SET_MODE) {
PID_Start(); /* reset PID, especially integral counters */
DRV_Status.mode = cmd.u.mode;
} else if (cmd.cmd==DRV_SET_SPEED) {
DRV_Status.speed.left = cmd.u.speed.left;
DRV_Status.speed.right = cmd.u.speed.right;
} else if (cmd.cmd==DRV_SET_POS) {
DRV_Status.pos.left = cmd.u.pos.left;
DRV_Status.pos.right = cmd.u.pos.right;
}
FRTOS1_taskEXIT_CRITICAL();
return ERR_OK;
}
void LF_StartFollowing(void) {
PID_Start();
LF_currState = STATE_FOLLOW_SEGMENT;
DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
}
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 */
}
