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 DRV_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[32];
CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" speed", DRV_SpeedOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedLeft);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" speed L", buf, io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_SpeedRight);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n\r\n");
CLS1_SendStatusStr((unsigned char*)" speed R", buf, io->stdOut);
buf[0]='\0';
CLS1_SendStatusStr((unsigned char*)" pos", DRV_PosOn?(unsigned char*)"on\r\n":(unsigned char*)"off\r\n", io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_PosLeft);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" pos L", buf, io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), Q4CLeft_GetPos());
CLS1_SendStatusStr((unsigned char*)" pos L Q4", buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n",io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), DRV_PosRight);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" pos R", buf, io->stdOut);
buf[0]='\0';
UTIL1_strcatNum32s(buf, sizeof(buf), Q4CRight_GetPos());
CLS1_SendStatusStr((unsigned char*)" pos R Q4", buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n",io->stdOut);
}
static void DRV_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[48];
CLS1_SendStatusStr((unsigned char*)"drive", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" mode", DRV_GetModeStr(DRV_Status.mode), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.left);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: ");
UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(TRUE));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
CLS1_SendStatusStr((unsigned char*)" speed left", buf, io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.speed.right);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps/sec (curr: ");
UTIL1_strcatNum32s(buf, sizeof(buf), TACHO_GetSpeed(FALSE));
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
CLS1_SendStatusStr((unsigned char*)" speed right", buf, io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.left);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CLeft_GetPos());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
CLS1_SendStatusStr((unsigned char*)" pos left", buf, io->stdOut);
UTIL1_Num32sToStr(buf, sizeof(buf), DRV_Status.pos.right);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" (curr: ");
UTIL1_strcatNum32s(buf, sizeof(buf), (int32_t)Q4CRight_GetPos());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)")\r\n");
CLS1_SendStatusStr((unsigned char*)" pos right", buf, io->stdOut);
}
bool DRV_HasTurned(void) {
int16_t pos;
if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) {
return FALSE; /* still messages in command queue, so there is something pending */
}
if (DRV_Status.mode==DRV_MODE_POS) {
#define DRV_TURN_SPEED_LOW 50
int32_t speedL, speedR;
speedL = TACHO_GetSpeed(TRUE);
speedR = TACHO_GetSpeed(FALSE);
if (speedL>-DRV_TURN_SPEED_LOW && speedL<DRV_TURN_SPEED_LOW && speedR>-DRV_TURN_SPEED_LOW && speedR<DRV_TURN_SPEED_LOW) { /* speed close to zero */
pos = Q4CLeft_GetPos();
if (match(pos, DRV_Status.pos.left)) {
pos = Q4CRight_GetPos();
if (match(pos, DRV_Status.pos.right)) {
return TRUE;
}
}
}
return FALSE;
} /* if */
return TRUE;
}
void TURN_TurnSteps(int16_t stepsL, int16_t stepsR) {
int16_t currLPos, currRPos, targetLPos, targetRPos;
uint8_t matchCntr = 100;
currLPos = Q4CLeft_GetPos();
targetLPos = currLPos+stepsL;
currRPos = Q4CRight_GetPos();
targetRPos = currRPos+stepsR;
for(;;) { /* breaks */
if (match(currLPos,targetLPos) && match(currRPos,targetRPos)) {
matchCntr--;
if (matchCntr==0) {
break; /* get out of loop */
}
}
PID_Pos(Q4CLeft_GetPos(), targetLPos, TRUE);
PID_Pos(Q4CRight_GetPos(), targetRPos, FALSE);
WAIT1_WaitOSms(2);
currLPos = Q4CLeft_GetPos();
currRPos = Q4CRight_GetPos();
} /* for */
}
void TACHO_Sample(void) {
/*! \todo Implement/change function as needed, make sure implementation below matches your needs */
static int cnt = 0;
/* get called from the RTOS tick counter. Divide the frequency. */
cnt += TMR_TICK_MS;
if (cnt < TACHO_SAMPLE_PERIOD_MS) {
return;
}
cnt = 0; /* reset counter */
/* left */
TACHO_LeftPosHistory[TACHO_PosHistory_Index] = Q4CLeft_GetPos();
TACHO_RightPosHistory[TACHO_PosHistory_Index] = Q4CRight_GetPos();
TACHO_PosHistory_Index++;
if (TACHO_PosHistory_Index >= NOF_HISTORY) {
TACHO_PosHistory_Index = 0;
}
}
void TACHO_Sample(void) {
#if 1
static int cnt = 0;
/* get called from the RTOS tick counter. Divide the frequency. */
cnt += portTICK_RATE_MS;
if (cnt < TACHO_SAMPLE_PERIOD_MS) {
return;
}
cnt = 0; /* reset counter */
/* left */
TACHO_LeftPosHistory[TACHO_PosHistory_Index] = Q4CLeft_GetPos();
TACHO_RightPosHistory[TACHO_PosHistory_Index] = Q4CRight_GetPos();
TACHO_PosHistory_Index++;
if (TACHO_PosHistory_Index >= NOF_HISTORY) {
TACHO_PosHistory_Index = 0;
}
/* \todo Temporary only! You will need to do this later before doing PID calculation */
TACHO_CalcSpeed();
#else
/*! \todo Implement function */
#endif
}
static void TURN_PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[32];
CLS1_SendStatusStr((unsigned char*)"turn", (unsigned char*)"\r\n", io->stdOut);
#if PL_HAS_QUADRATURE
UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Steps90);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
CLS1_SendStatusStr((unsigned char*)" steps90", buf, io->stdOut);
UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepsFwBw);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" steps\r\n");
CLS1_SendStatusStr((unsigned char*)" stepsfw", buf, io->stdOut);
UTIL1_Num16sToStr(buf, sizeof(buf), Q4CLeft_GetPos());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
UTIL1_strcatNum16u(buf, sizeof(buf), Q4CLeft_NofErrors());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
CLS1_SendStatusStr((unsigned char*)" left pos", buf, io->stdOut);
UTIL1_Num16sToStr(buf, sizeof(buf), Q4CRight_GetPos());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)", ");
UTIL1_strcatNum16u(buf, sizeof(buf), Q4CRight_NofErrors());
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" errors\r\n");
CLS1_SendStatusStr((unsigned char*)" right pos", buf, io->stdOut);
#else
UTIL1_Num16uToStr(buf, sizeof(buf), TURN_DutyPercent);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
CLS1_SendStatusStr((unsigned char*)" duty", buf, io->stdOut);
UTIL1_Num16uToStr(buf, sizeof(buf), TURN_Time90ms);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
CLS1_SendStatusStr((unsigned char*)" time90", buf, io->stdOut);
UTIL1_Num16uToStr(buf, sizeof(buf), TURN_StepFwBwMs);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" ms\r\n");
CLS1_SendStatusStr((unsigned char*)" timefw", buf, io->stdOut);
#endif
}
void TACHO_Sample(void) {
#if 1
static int cnt = 0;
/* get called from the RTOS tick counter. Divide the frequency. */
cnt += portTICK_RATE_MS;
if (cnt < SPEED_CALC_PERIOD_MS) { // Sampling alle 5 ms
return;
}
cnt = 0; /* reset counter */
/* left */
TACHO_LeftPosHistory[TACHO_PosHistory_Index] = Q4CLeft_GetPos();
TACHO_RightPosHistory[TACHO_PosHistory_Index] = Q4CRight_GetPos();
TACHO_PosHistory_Index++;
if (TACHO_PosHistory_Index >= NOF_HISTORY) {
TACHO_PosHistory_Index = 0;
}
/* temporary only */
// TACHO_CalcSpeed();
#else
/*! \todo Implement function */
#endif
}
bool DRV_IsStopped(void) {
Q4CLeft_QuadCntrType leftPos;
Q4CRight_QuadCntrType rightPos;
if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) {
return FALSE; /* still messages in command queue, so there is something pending */
}
/* do *not* use/calculate speed: too slow! Use position encoder instead */
leftPos = Q4CLeft_GetPos();
rightPos = Q4CRight_GetPos();
if (DRV_Status.mode==DRV_MODE_POS) {
if (DRV_Status.pos.left!=(int32_t)leftPos) {
return FALSE;
}
if (DRV_Status.pos.right!=(int32_t)rightPos) {
return FALSE;
}
return TRUE;
} else {
/* ???? what to do otherwise ???? */
return FALSE;
}
}
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 */
}
static portTASK_FUNCTION(TraceTask, pvParameters) {
unsigned char buf[32];
(void)pvParameters;
//ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */
for(;;) {
if (traceChannel==TRACE_TO_NONE) {
/* do nothing */
} else if (traceChannel==TRACE_TO_SHELL) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
#if PL_HAS_MOTOR
if (traceMotor) {
buf[0] = '\0';
if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x");
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x");
}
UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT)));
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_MOTOR_TACHO
if (traceTacho) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:");
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ' ');
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_QUADRATURE
if (traceQuad) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_ACCEL
if (traceAccel) {
int16_t x, y, z;
buf[0] = '\0';
ACCEL_GetValues(&x, &y, &z);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n");
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */
}
FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
} /* for */
}