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;
}
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;
}
}
unsigned short SQUEUE_NofElements(void) {
return (unsigned short)FRTOS1_uxQueueMessagesWaiting(SQUEUE_Queue);
}
unsigned short QUEUE_NofElements(xQueueHandle queue) {
return (unsigned short)FRTOS1_uxQueueMessagesWaiting(queue);
}
/*!
* \brief Peeks a character from the queue without removing it.
* \param queue Queue to be used
* \return '\0' if the queue is empty, otherwise it contains the character.
*/
static unsigned short RSTDIO_NofElements(xQueueHandle queue) {
return (unsigned short)FRTOS1_uxQueueMessagesWaiting(queue);
}
