static void DriveTask(void *pvParameters) {
portTickType xLastWakeTime;
(void)pvParameters;
xLastWakeTime = xTaskGetTickCount();
for(;;) {
while (GetCmd()==ERR_OK) { /* returns ERR_RXEMPTY if queue is empty */
/* process incoming commands */
}
TACHO_CalcSpeed();
if (DRV_Status.mode==DRV_MODE_SPEED) {
PID_Speed(TACHO_GetSpeed(TRUE), DRV_Status.speed.left, TRUE);
PID_Speed(TACHO_GetSpeed(FALSE), DRV_Status.speed.right, FALSE);
} else if (DRV_Status.mode==DRV_MODE_STOP) {
PID_Speed(TACHO_GetSpeed(TRUE), 0, TRUE);
PID_Speed(TACHO_GetSpeed(FALSE), 0, FALSE);
} else if (DRV_Status.mode==DRV_MODE_POS) {
PID_Pos(Q4CLeft_GetPos(), DRV_Status.pos.left, TRUE);
PID_Pos(Q4CRight_GetPos(), DRV_Status.pos.right, FALSE);
} else if (DRV_Status.mode==DRV_MODE_NONE) {
/* do nothing */
}
FRTOS1_vTaskDelayUntil(&xLastWakeTime, 5/portTICK_RATE_MS);
} /* for */
}
static void T2(void* param) {
TickType_t xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
for(;;) {
LED1_Neg();
FRTOS1_vTaskDelayUntil(&xLastWakeTime, 1000*portTICK_RATE_MS);
}
}
