static void prvQueueOverwriteTask( void *pvParameters )
{
QueueHandle_t xTaskQueue;
const UBaseType_t uxQueueLength = 1;
uint32_t ulValue, ulStatus = pdPASS, x;
/* The parameter is not used. */
( void ) pvParameters;
/* Create the queue. xQueueOverwrite() should only be used on queues that
have a length of 1. */
xTaskQueue = xQueueCreate( uxQueueLength, ( UBaseType_t ) sizeof( uint32_t ) );
configASSERT( xTaskQueue );
for( ;; )
{
/* The queue is empty. Writing to the queue then reading from the queue
should return the item written. */
ulValue = 10;
xQueueOverwrite( xTaskQueue, &ulValue );
ulValue = 0;
xQueueReceive( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( ulValue != 10 )
{
ulStatus = pdFAIL;
}
/* Now try writing to the queue several times. Each time the value
in the queue should get overwritten. */
for( x = 0; x < qoLOOPS; x++ )
{
/* Write to the queue. */
xQueueOverwrite( xTaskQueue, &x );
/* Check the value in the queue is that written, even though the
queue was not necessarily empty. */
xQueuePeek( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( ulValue != x )
{
ulStatus = pdFAIL;
}
/* There should always be one item in the queue. */
if( uxQueueMessagesWaiting( xTaskQueue ) != uxQueueLength )
{
ulStatus = pdFAIL;
}
}
/* Empty the queue again. */
xQueueReceive( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( uxQueueMessagesWaiting( xTaskQueue ) != 0 )
{
ulStatus = pdFAIL;
}
if( ulStatus != pdFAIL )
{
/* Increment a counter to show this task is still running without
error. */
ulLoopCounter++;
}
#if( configUSE_PREEMPTION == 0 )
taskYIELD();
#endif
}
}
static void prvSendFrontAndBackTest( void *pvParameters )
{
unsigned portLONG ulData, ulData2;
xQueueHandle xQueue;
#ifdef USE_STDIO
void vPrintDisplayMessage( const portCHAR * const * ppcMessageToSend );
const portCHAR * const pcTaskStartMsg = "Queue SendToFront/SendToBack/Peek test started.\r\n";
/* Queue a message for printing to say the task has started. */
vPrintDisplayMessage( &pcTaskStartMsg );
#endif
xQueue = ( xQueueHandle ) pvParameters;
for( ;; )
{
/* The queue is empty, so sending an item to the back of the queue
should have the same efect as sending it to the front of the queue.
First send to the front and check everything is as expected. */
xQueueSendToFront( xQueue, ( void * ) &ulLoopCounter, genqNO_BLOCK );
if( uxQueueMessagesWaiting( xQueue ) != 1 )
{
xErrorDetected = pdTRUE;
}
if( xQueueReceive( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
/* The data we sent to the queue should equal the data we just received
from the queue. */
if( ulLoopCounter != ulData )
{
xErrorDetected = pdTRUE;
}
/* Then do the same, sending the data to the back, checking everything
is as expected. */
if( uxQueueMessagesWaiting( xQueue ) != 0 )
{
xErrorDetected = pdTRUE;
}
xQueueSendToBack( xQueue, ( void * ) &ulLoopCounter, genqNO_BLOCK );
if( uxQueueMessagesWaiting( xQueue ) != 1 )
{
xErrorDetected = pdTRUE;
}
if( xQueueReceive( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( uxQueueMessagesWaiting( xQueue ) != 0 )
{
xErrorDetected = pdTRUE;
}
/* The data we sent to the queue should equal the data we just received
from the queue. */
if( ulLoopCounter != ulData )
{
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* Place 2, 3, 4 into the queue, adding items to the back of the queue. */
for( ulData = 2; ulData < 5; ulData++ )
{
xQueueSendToBack( xQueue, ( void * ) &ulData, genqNO_BLOCK );
}
/* Now the order in the queue should be 2, 3, 4, with 2 being the first
thing to be read out. Now add 1 then 0 to the front of the queue. */
if( uxQueueMessagesWaiting( xQueue ) != 3 )
{
xErrorDetected = pdTRUE;
}
ulData = 1;
xQueueSendToFront( xQueue, ( void * ) &ulData, genqNO_BLOCK );
ulData = 0;
xQueueSendToFront( xQueue, ( void * ) &ulData, genqNO_BLOCK );
/* Now the queue should be full, and when we read the data out we
should receive 0, 1, 2, 3, 4. */
if( uxQueueMessagesWaiting( xQueue ) != 5 )
{
xErrorDetected = pdTRUE;
}
if( xQueueSendToFront( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != errQUEUE_FULL )
{
xErrorDetected = pdTRUE;
}
if( xQueueSendToBack( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != errQUEUE_FULL )
{
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* Check the data we read out is in the expected order. */
for( ulData = 0; ulData < genqQUEUE_LENGTH; ulData++ )
{
/* Try peeking the data first. */
if( xQueuePeek( xQueue, &ulData2, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( ulData != ulData2 )
{
xErrorDetected = pdTRUE;
}
/* Now try receiving the data for real. The value should be the
same. Clobber the value first so we know we really received it. */
ulData2 = ~ulData2;
if( xQueueReceive( xQueue, &ulData2, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( ulData != ulData2 )
{
xErrorDetected = pdTRUE;
}
}
/* The queue should now be empty again. */
if( uxQueueMessagesWaiting( xQueue ) != 0 )
{
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* Our queue is empty once more, add 10, 11 to the back. */
ulData = 10;
if( xQueueSend( xQueue, &ulData, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
ulData = 11;
if( xQueueSend( xQueue, &ulData, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( uxQueueMessagesWaiting( xQueue ) != 2 )
{
xErrorDetected = pdTRUE;
}
/* Now we should have 10, 11 in the queue. Add 7, 8, 9 to the
front. */
for( ulData = 9; ulData >= 7; ulData-- )
{
if( xQueueSendToFront( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
}
/* Now check that the queue is full, and that receiving data provides
the expected sequence of 7, 8, 9, 10, 11. */
if( uxQueueMessagesWaiting( xQueue ) != 5 )
{
xErrorDetected = pdTRUE;
}
if( xQueueSendToFront( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != errQUEUE_FULL )
{
xErrorDetected = pdTRUE;
}
if( xQueueSendToBack( xQueue, ( void * ) &ulData, genqNO_BLOCK ) != errQUEUE_FULL )
{
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* Check the data we read out is in the expected order. */
for( ulData = 7; ulData < ( 7 + genqQUEUE_LENGTH ); ulData++ )
{
if( xQueueReceive( xQueue, &ulData2, genqNO_BLOCK ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( ulData != ulData2 )
{
xErrorDetected = pdTRUE;
}
}
if( uxQueueMessagesWaiting( xQueue ) != 0 )
{
xErrorDetected = pdTRUE;
}
ulLoopCounter++;
}
}
static void prvHighestPriorityPeekTask( void *pvParameters )
{
QueueHandle_t xQueue = ( QueueHandle_t ) pvParameters;
uint32_t ulValue;
#ifdef USE_STDIO
{
void vPrintDisplayMessage( const char * const * ppcMessageToSend );
const char * const pcTaskStartMsg = "Queue peek test started.\r\n";
/* Queue a message for printing to say the task has started. */
vPrintDisplayMessage( &pcTaskStartMsg );
}
#endif
for( ;; )
{
/* Try peeking from the queue. The queue should be empty so we will
block, allowing the high priority task to execute. */
if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
{
/* We expected to have received something by the time we unblock. */
xErrorDetected = pdTRUE;
}
/* When we reach here the high and medium priority tasks should still
be blocked on the queue. We unblocked because the low priority task
wrote a value to the queue, which we should have peeked. Peeking the
data (rather than receiving it) will leave the data on the queue, so
the high priority task should then have also been unblocked, but not
yet executed. */
if( ulValue != 0x11223344 )
{
/* We did not receive the expected value. */
xErrorDetected = pdTRUE;
}
if( uxQueueMessagesWaiting( xQueue ) != 1 )
{
/* The message should have been left on the queue. */
xErrorDetected = pdTRUE;
}
/* Now we are going to actually receive the data, so when the high
priority task runs it will find the queue empty and return to the
blocked state. */
ulValue = 0;
if( xQueueReceive( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
{
/* We expected to receive the value. */
xErrorDetected = pdTRUE;
}
if( ulValue != 0x11223344 )
{
/* We did not receive the expected value - which should have been
the same value as was peeked. */
xErrorDetected = pdTRUE;
}
/* Now we will block again as the queue is once more empty. The low
priority task can then execute again. */
if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
{
/* We expected to have received something by the time we unblock. */
xErrorDetected = pdTRUE;
}
/* When we get here the low priority task should have again written to the
queue. */
if( ulValue != 0x01234567 )
{
/* We did not receive the expected value. */
xErrorDetected = pdTRUE;
}
if( uxQueueMessagesWaiting( xQueue ) != 1 )
{
/* The message should have been left on the queue. */
xErrorDetected = pdTRUE;
}
/* We only peeked the data, so suspending ourselves now should enable
the high priority task to also peek the data. The high priority task
will have been unblocked when we peeked the data as we left the data
in the queue. */
vTaskSuspend( NULL );
/* This time we are going to do the same as the above test, but the
high priority task is going to receive the data, rather than peek it.
This means that the medium priority task should never peek the value. */
if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
{
xErrorDetected = pdTRUE;
}
if( ulValue != 0xaabbaabb )
{
xErrorDetected = pdTRUE;
}
vTaskSuspend( NULL );
}
}
static void prvLowPriorityPeekTask( void *pvParameters )
{
QueueHandle_t xQueue = ( QueueHandle_t ) pvParameters;
uint32_t ulValue;
for( ;; )
{
/* Write some data to the queue. This should unblock the highest
priority task that is waiting to peek data from the queue. */
ulValue = 0x11223344;
if( xQueueSendToBack( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
{
/* We were expecting the queue to be empty so we should not of
had a problem writing to the queue. */
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* By the time we get here the data should have been removed from
the queue. */
if( uxQueueMessagesWaiting( xQueue ) != 0 )
{
xErrorDetected = pdTRUE;
}
/* Write another value to the queue, again waking the highest priority
task that is blocked on the queue. */
ulValue = 0x01234567;
if( xQueueSendToBack( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
{
/* We were expecting the queue to be empty so we should not of
had a problem writing to the queue. */
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* All the other tasks should now have successfully peeked the data.
The data is still in the queue so we should be able to receive it. */
ulValue = 0;
if( xQueueReceive( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
{
/* We expected to receive the data. */
xErrorDetected = pdTRUE;
}
if( ulValue != 0x01234567 )
{
/* We did not receive the expected value. */
}
/* Lets just delay a while as this is an intensive test as we don't
want to starve other tests of processing time. */
vTaskDelay( qpeekSHORT_DELAY );
/* Unsuspend the other tasks so we can repeat the test - this time
however not all the other tasks will peek the data as the high
priority task is actually going to remove it from the queue. Send
to front is used just to be different. As the queue is empty it
makes no difference to the result. */
vTaskResume( xMediumPriorityTask );
vTaskResume( xHighPriorityTask );
vTaskResume( xHighestPriorityTask );
#if( configUSE_PREEMPTION == 0 )
taskYIELD();
#endif
ulValue = 0xaabbaabb;
if( xQueueSendToFront( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
{
/* We were expecting the queue to be empty so we should not of
had a problem writing to the queue. */
xErrorDetected = pdTRUE;
}
#if configUSE_PREEMPTION == 0
taskYIELD();
#endif
/* This time we should find that the queue is empty. The high priority
task actually removed the data rather than just peeking it. */
if( xQueuePeek( xQueue, &ulValue, qpeekNO_BLOCK ) != errQUEUE_EMPTY )
{
/* We expected to receive the data. */
xErrorDetected = pdTRUE;
}
/* Unsuspend the highest and high priority tasks so we can go back
and repeat the whole thing. The medium priority task should not be
suspended as it was not able to peek the data in this last case. */
vTaskResume( xHighPriorityTask );
vTaskResume( xHighestPriorityTask );
/* Lets just delay a while as this is an intensive test as we don't
want to starve other tests of processing time. */
vTaskDelay( qpeekSHORT_DELAY );
}
}
void MOTOR_Tasks ( void )
{
//debugU("RUNNING");
// motor_sendmsg(1, 1000);
// motor_sendmsg(1, 1615);
// motor_sendmsg(3, 1350);
while(1)
{
if(motorData.theQueue != 0)
{
if(xQueuePeek(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY );
motorData.state = motorData.rxMessage.command;
// motorData.state = 1;
// debugUInt(motorData.state);
// debugUInt(motorData.duration);
// communication_sendIntMsg(motorData.rxMessage.command, motorData.rxMessage.duration);
/* Check the application's current state. */
switch ( motorData.state )
{
/* Application's initial state. */
case MOTOR_STATE_INIT: //state 0, do nothing
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
break;
}
case MOTOR_STATE_STRAIGHT: //state 1, go straight.
{
debugU("Straight");
//stop command
if(motorData.rxMessage.int2 == 0)
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_OC1_Stop();
DRV_OC0_Stop();
float time = (float)motorData.n * 0.05;
float Ldistance = MMPERTICK * motorData.LTestTick;
float Rdistance = MMPERTICK * motorData.RTestTick;
motorData.LTestTick = 0;
motorData.RTestTick = 0;
debugUFloat(time);
debugU("Right:\n");
debugUFloat(motorData.RSet);
debugUFloat(Rdistance);
debugU("Left:\n");
debugUFloat(motorData.LSet);
debugUFloat(Ldistance);
debugUFloat(motorData.ratio);
motorData.LSet = 0.0;
motorData.RSet = 0.0;
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
}
else //just go straight
{
motorData.LSet = MAXSPEED;
motorData.RSet = MAXSPEED;
motorData.LOCSet = MAXSPEEDOCSET;
motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Start();
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
}
break;
}
case MOTOR_STATE_TURNRIGHT: //state 2, turn right
{
debugU("Right");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.LSet = angular * (float)outRad;
//motorData.LSet = MAXSPEED;
motorData.LOCSet = (int)(motorData.LSet * 6);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);// - 100);
if(outRad < 11)
{
motorData.RSet = 0.0;
motorData.ROCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
// DRV_OC0_Start();
DRV_OC0_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.RSet = (temp - 10.0) * angular;
// motorData.RSet = ( motorData.ratio * (MAXSPEED));
motorData.ROCSet = (int)(motorData.RSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
DRV_OC0_Start();
}
DRV_OC1_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
case MOTOR_STATE_TURNLEFT: //state 3 turn left.
{
debugU("Left");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.RSet = angular * (float)outRad;
motorData.ROCSet = (int)(motorData.RSet * 6);
// motorData.RSet = MAXSPEED;
// motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
if(outRad < 11)
{
motorData.LSet = 0.0;
motorData.LOCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
// DRV_OC1_Start();
DRV_OC1_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.LSet = (temp - 10.0) * angular;
motorData.LOCSet = (int)(motorData.LSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
DRV_OC1_Start();
}
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
/* The default state should never be executed. */
default:
{
DRV_TMR0_Stop();
DRV_TMR2_Stop();
DRV_TMR0_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
motorData.duration = 0;
/* TODO: Handle error in application's state machine. */
break;
}
} //end switch
}
}
else
{
crash("No Motor MsgQ");
}
}
}
void UsbSerial::wait() {
char c;
xQueuePeek(rx_queue, &c, portMAX_DELAY);
}
void vMainMappingTask( void *pvParameters )
{
// Initialize the buffers used for mapping operations. Each sensor has its
// own buffers that are allocated on the heap.
point_buffer_t *PointBuffers = pvPortMalloc(NUMBER_OF_SENSORS * sizeof(point_buffer_t));
line_buffer_t *LineBuffers = pvPortMalloc(NUMBER_OF_SENSORS * sizeof(line_buffer_t));
// Set initial lengths to 0
for (uint8_t i = 0; i < NUMBER_OF_SENSORS; i++) {
PointBuffers[i].len = 0;
LineBuffers[i].len = 0;
}
// Initialize the repo for storing the completely merged line segments
line_repo_t *LineRepo = pvPortMalloc(sizeof(line_repo_t));
LineRepo->len = 0;
// Verify allocation
configASSERT(PointBuffers && LineBuffers && LineRepo );
// Set task to run at a fixed frequency
/*
TickType_t xLastWakeTime;
const TickType_t xFrequency = 100 / portTICK_PERIOD_MS;
xLastWakeTime = xTaskGetTickCount();
*/
while (1)
{
//vTaskDelayUntil(&xLastWakeTime, xFrequency);
if (gHandshook == TRUE && gPaused == FALSE)
{
// Read the robots current pose
pose_t Pose;
xQueuePeek(globalPoseQ, &Pose, 10 / portTICK_PERIOD_MS);
// Convert to centimeters
Pose.x /= 10.0;
Pose.y /= 10.0;
// Put inside [0,2pi)
func_wrap_to_2pi(&Pose.theta);
// Block here waiting for measurement from the sensor tower.
measurement_t Measurement;
if (xQueueReceive(measurementQ, &Measurement, 200 / portTICK_PERIOD_MS) == pdTRUE) {
// Append new IR measurements to the end of each PB
mapping_update_point_buffers(PointBuffers, Measurement, Pose);
}
// Check for notification from sensor tower task. Do not wait.
if (ulTaskNotifyTake(pdTRUE, 0) == 1) {
// Run create and merge functions for each of the sensors' buffers
for (uint8_t j = 0; j < NUMBER_OF_SENSORS; j++) {
mapping_line_create(&PointBuffers[j], &LineBuffers[j]);
mapping_line_merge(&LineBuffers[j], LineRepo);
}
// Merge the repo with itself until it cannot be reduced further
uint8_t lastRepoLen;
do {
lastRepoLen = LineRepo->len;
LineRepo = mapping_repo_merge(LineRepo);
} while (LineRepo->len < lastRepoLen);
}
line_t LineOut = { 0 };
if (LineRepo->len > 0) {
LineOut = LineRepo->buffer[LineRepo->len-1];
LineRepo->len--;
}
#ifdef SEND_LINE
// Send update to server. LineOut contains all zeroes if one was not available from the LineRepo.
send_line(ROUND(Pose.x), ROUND(Pose.y), ROUND(Pose.theta*RAD2DEG), LineOut);
#endif /* SEND_LINE */
}
else {
// If disconnected and/or paused
vTaskDelay(200 / portTICK_PERIOD_MS);
}
}
}
/*
* Defines the 'dice' tasks as described at the top of main.c
*/
void vDiceTask( void *pvParameters )
{
unsigned char ucDiceValue, ucIndex;
unsigned long ulDiceRunTime;
extern void vSuspendFlashTasks( unsigned char ucIndex, short sSuspendTasks );
/* Two instances of this task are created so the task parameter is used
to pass in a constant that indicates whether this task is controlling
the left side or right side display. The constant is used as an index
into the arrays defined at file scope within this file. */
ucIndex = ( unsigned char ) pvParameters;
/* A binary semaphore is used to signal button push events. Create the
semaphore before it is used. */
vSemaphoreCreateBinary( xSemaphores[ ucIndex ] );
/* Make sure the semaphore starts in the wanted state - no button pushes
pending. This call will just clear any button pushes that are latched.
Passing in 0 as the block time means the call will not wait for any further
button pushes but instead return immediately. */
prvButtonHit( ucIndex, 0 );
/* Seed the random number generator. */
srand( ( unsigned char ) diceSHAKE_TIME );
/* Start the task proper. A loop will be performed each time a button is
pushed. The task will remain in the blocked state (sleeping) until a
button is pushed. */
for( ;; )
{
/* Wait for a button push. This task will enter the Blocked state
(will not run again) until after a button has been pushed. */
prvButtonHit( ucIndex, portMAX_DELAY );
/* The next line will only execute after a button has been pushed -
initialise the variable used to control the time the dice is shaken
for. */
ulDiceRunTime = diceSHAKE_TIME;
/* Suspend the flash tasks so this task has exclusive access to the
display. */
vSuspendFlashTasks( ucIndex, pdTRUE );
/* Clear the display and pause for a short time, before starting to
shake. */
*pucDisplayOutput[ ucIndex ] = 0xff;
vTaskDelay( diceSHORT_PAUSE_BEFORE_SHAKE );
/* Keep generating and displaying random numbers until the shake time
expires. */
while( ulDiceRunTime > 0 )
{
ulDiceRunTime--;
/* Generate and display a random number. */
ucDiceValue = rand() % 6 + 1;
dice7SEG_Value( ucIndex ) = ( dice7SEG_Value( ucIndex ) | 0xf7 ) & cDisplaySegments[ ucIndex ][ ucDiceValue ];
/* Block/sleep for a very short time before generating the next
random number. */
vTaskDelay( diceDELAY_BETWEEN_RANDOM_NUMBERS_ms );
}
/* Clear any button pushes that are pending because a button bounced, or
was pressed while the dice were shaking. Again a block time of zero is
used so the function does not wait for any pushes but instead returns
immediately. */
prvButtonHit( ucIndex, 0 );
/* Delay for a short while to display the dice shake result. Use a queue
peek here instead of a vTaskDelay() allows the delay to be interrupted by
a button push. If a button is pressed xQueuePeek() will return but the
button push will remain pending to be read again at the top of this for
loop. It is safe to uses a queue function on a semaphore handle as
semaphores are implemented as macros that uses queues, so the two are
basically the same thing. */
xQueuePeek( xSemaphores[ ucIndex ], NULL, diceDELAY_WHILE_DISPLAYING_RESULT );
/* Clear the display then resume the tasks or co-routines that were using
the segments of the display. */
*pucDisplayOutput[ ucIndex ] = 0xff;
vSuspendFlashTasks( ucIndex, pdFALSE );
}
}
void APP_WIFLY_Tasks ( void )
{
while(1)
{
// debugChar(0x22);
// app_wifly_sendmsg(2,"TEST");
//check if queue exists
if(appData.theQueue != 0)
{
//receive a message and store into rxMessage ... block 5 ticks if empty queue
if(xQueuePeek(appData.theQueue, &(appData.rxChar), portMAX_DELAY ))
{
xQueueReceive(appData.theQueue, &(appData.rxChar), portMAX_DELAY );
//received messageQ message will tell use the state which will be used
//##appData.state = appData.rxMessage.type;
appData.state = 2;
//run the state according to the message's state
switch ( appData.state )
{
/* Application's initial state. */
case APP_STATE_INIT: //0
break;
case APP_STATE_READ: //1
{
debugU("Read");
debugChar(0x0F);
app_wifly_UartRx();
appData.state = 0;
app_wifly_sendmsg(2,appData.rxBuffer);
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
break;
}
case APP_STATE_WRITE: //2
{
debugU("Write");
debugChar(0xF0);
app_wifly_UartTxChar(appData.rxChar);
appData.state = 0;
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
//appData.rxChar = ' ';
break;
}
case APP_STATE_RECEIVE: //3
{
debugU("Receiving");
debugChar(appData.rxBufferIndex);
if(appData.rxBufferIndex < appData.rxBufferSize) //if we are within buffer bounds, copy char
appData.rxBuffer[appData.rxBufferIndex] = appData.rxMessage.string[0];
appData.rxBufferIndex++;
appData.state = 0;
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
break;
}
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
}//end of case
}//end of receive message
}//end of check message queue != 0
else //attempt to create the queue again if it doesn't exist
{
//something terrible happens normally so have it exit rather than recreate
debugChar(0xFF);
//run exit interrupt to OS... or forever loop message
return;
//appData.theQueue = xQueueCreate(10, sizeof(APP_STATES));
}
}//end of while(1)
}
void SENSORCOMMUNICATION_Tasks ( void )
{
int roverSimCounter = 0;
while(1)
{
//check if queue exists
if(sensorcommunicationData.sensortheQueue != 0)
{
//receive a message and store into rxMessage ... block 5 ticks if empty queue
if(xQueuePeek(sensorcommunicationData.sensortheQueue, (void*)&(sensorcommunicationData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(sensorcommunicationData.sensortheQueue, (void*)&(sensorcommunicationData.rxMessage), portMAX_DELAY);
//received messageQ message will tell use the state which will be used
sensorcommunicationData.state = sensorcommunicationData.rxMessage.type;
//communicationData.state = 1;
//run the state according to the message's state
switch ( sensorcommunicationData.state )
{
/* Application's initial state. */
case SENSORCOMMUNICATION_STATE_INIT: //0
break;
case SENSORCOMMUNICATION_STATE_RECEIVE: //1
//debugU("COM rx: ");
//debugUInt(communicationData.rxMessage.msg);
if(sensorcommunicationData.rxMessage.msg == STARTBYTE) //received start transmit bit
{
if(sensorcommunicationData.sensorrxByteCount > 0) //we had part of a previous message...
{
debugU("NACK");
//NACK;
}
sensorcommunicationData.sensorrxBuffer[0] = sensorcommunicationData.rxMessage.msg;
sensorcommunicationData.sensorrxByteCount = 1;
}
else if( sensorcommunicationData.sensorrxByteCount > 0) //continuing message
{
sensorcommunicationData.sensorrxBuffer[sensorcommunicationData.sensorrxByteCount] = sensorcommunicationData.rxMessage.msg;
debugUInt(CharToInt(sensorcommunicationData.rxMessage.msg));
sensorcommunicationData.sensorrxByteCount++;
if(sensorcommunicationData.sensorrxByteCount == 10)
{
//check start byte
if(sensorcommunicationData.sensorrxBuffer[0] == STARTBYTE)
{
//###CHECK seq number
int i = 0;
while(sensorcommunicationData.sensorrxBuffer[1] != sensorcommunicationData.sensorRxMsgSeq)
{
sensorcommunicationData.sensorRxMsgSeq++;
sensorcommunicationData.msgErr++;
debugU("Lost a seq number\r # dropped: ");
debugUInt(sensorcommunicationData.msgErr);
i++;
if(i == 10)
{
crash("E: COM too many lost rx seqnum\n");
}
}
int x = 0;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[2]) * 1000;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[3]) * 100;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[4]) * 10;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[5]);
int y = 0;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[6]) * 1000;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[7]) * 100;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[8]) * 10;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[9]);
if (x != 0 && y != 0)
{
roverLocation[0] = x;
roverLocation[1] = y;
}
communication_sendIntMsg(0, 1000);
// else if (roverSimCounter < 11)
// communication_sendIntMsg(1, 1);
// else if (roverSimCounter == 2)
// communication_sendIntMsg(1, 10);
// else if (roverSimCounter == 3)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 4)
// communication_sendIntMsg(1, 6);
// else if (roverSimCounter == 5)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 6)
// communication_sendIntMsg(1, 10);
// else if (roverSimCounter == 7)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 8)
// communication_sendIntMsg(1, 4);
//
// else // Tel the rover to not do anything...
// communication_sendIntMsg(0, 0);
roverSimCounter++;
debugUInt(roverSimCounter);
debugU("sensor msg period: ");
debugUInt(sensor_debugGetTime());
debugU("Sensor1: ");
//char msg[12];
//sprintf(msg, "%d", x);
//debugU(msg);
debugUInt(roverLocation[0]);
debugU("Sensor2: ");
//sprintf(msg, "%d", y);
//debugU(msg);
debugUInt(roverLocation[1]);
debugU("\n");
//communication_sendIntMsg(x,y);
sensorcommunicationData.sensorrxByteCount = 0;
sensorcommunicationData.sensorRxMsgSeq++;
if(sensorcommunicationData.sensorRxMsgSeq == 0x7F)
sensorcommunicationData.sensorRxMsgSeq = 0x00;
//ACK
//debugU("\nsensor comm ACK\n");
//#ifdef TEST
// testData.count++; // Increment message count
// int j =0;
// for (j; j<10; j++)
// {
// testData.sensorrxBuffer[j] = sensorcommunicationData.sensorrxBuffer[j];
// }
// // Receive buffer contents recorded.
//
// communication_sendIntMsg(x, y);
//#else
// if(sensorcommunicationData.sensorRxMsgSeq % 4 == 0)
// communication_sendIntMsg(3, 100);
// else if(sensorcommunicationData.sensorRxMsgSeq % 4 == 2)
// communication_sendIntMsg(2, 100);
//#endif
sensorcommunicationData.state = 0;
}//end if check start byte
}//end if bytecount == 10
}//end if bytecount > 0
else //failed to receive start bit and catch all... nack and reset message
{
//NACK
debugU("\nsensor NACK");
sensorcommunicationData.sensorrxByteCount = 0; //no start byte and bytecount != 0, so unknown char drop it
}
break;
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
}//end of case
}//end of receive message
}//end of check message queue != 0
else //attempt to create the queue again if it doesn't exist
{
//something terrible happens normally so have it exit rather than recreate
crash("E: No Comm Q");
//run exit interrupt to OS... or forever loop message
return;
//communicationData.sensortheQueue = xQueueCreate(10, sizeof(APP_STATES));
}
}//end of while(1)
}
int kbhit(void) {
u16 key;
int ret = xQueuePeek(KeyQueue, &key, 0);
return (ret == pdPASS);
}
/*-----------------------------------------------------------------------------------*/
void vTaskGPIOcalc(void *pvParameters)
{
xTCPTypeDef uTCPt;
portBASE_TYPE xStatus_TaskGPIOcalc;
xDataDriveTypeDef uDRIVE_temp, uDRIVE_new;
xTCPTypeDef uTCPcalc;
char msg[] = {0x00, 0x00, 0x00, 0x00};
// начальная инициализация
T_step = 2*tau_step;
calc = 1;
// vTaskPrioritySet( NULL, ( configMAX_PRIORITIES - 2) );
TIM_Cmd(TIM3, ENABLE);
for (;;) {
// Проблемы с семафором
// захват светофора. Ждем до последнего
xSemaphoreTake(xBinarySemaphore, portMAX_DELAY);
if (calc != 1)
{
// проверка наличия команд в буфере
if (uxQueueMessagesWaiting(qDRIVE) > 0) {
if (uxQueueMessagesWaiting(qDRIVE) == BUF_COM_SDRIVE)
{
xStatus_TaskGPIOcalc = uxQueueMessagesWaiting(qDRIVE);
}
// считываем первое значение из очереди
xStatus_TaskGPIOcalc = xQueueReceive(qDRIVE, &uDRIVE_temp, 0 ); // тайм-аут не нужен. В очереди точно есть элемент
// определяем состояние портов, изменяем текущий шаг в uDRIVE, записываем в начало очереди
// ПРОЦЕДУРЫ расчета нового положения и сосотояния портов
if (uDRIVE_temp.num_com == G00) fProc_GOO(&uDRIVE_temp);
if (uDRIVE_temp.num_com == G01) fProc_GO1(&uDRIVE_temp);
// if (uDRIVE_temp.num_com == G01) Proc_GO1(); // e.t.c
// проверка окончания выполенния команды
if((uDRIVE_temp.STEP_X == uDRIVE_temp.step_x)&&
(uDRIVE_temp.STEP_Y == uDRIVE_temp.step_y)&&
(uDRIVE_temp.STEP_Z == uDRIVE_temp.step_z))
{
// проверяем есть ли дальше команды
if (uxQueueMessagesWaiting(qDRIVE) > 0) {
// задаем период повторения для следующей команды
// считываем без удаления
xStatus_TaskGPIOcalc = xQueuePeek(qDRIVE, &uDRIVE_new, 0 );
T_step = configTICK_RATE_HZ / uDRIVE_new.SPEED ;
// нужен ли переброс знака
if (uDRIVE_new.DIR_X!=uDRIVE_temp.DIR_X) CDX = 1;
else CDX = 0;
if (uDRIVE_new.DIR_Y!=uDRIVE_temp.DIR_Y) CDY = 1;
else CDY = 0;
if (uDRIVE_new.DIR_Z!=uDRIVE_temp.DIR_Z) CDZ = 1;
else CDZ = 0;
}
else // если ничего больше нет
{
// сброс всех степов
BSRRL_val = GPIOE->BSRRL;
BSRRL_val &= 0xFFF1; // Зануляем только степы
BSRRH_val = GPIOE->BSRRH;; // в резете устанавливаем степы
BSRRH_val |= 0x000E;
//оставляем срабатывание TaskSW
T_step = 2*tau_step;
}
// выдача сообщения
msg[0] = 0x43;
msg[0] |= 0x80;
uTCPcalc.ucLen = 4;
uTCPcalc.ucDATA = msg;
xStatus_TaskGPIOcalc = xQueueSend(qTCPt, &uTCPcalc, portMAX_DELAY);
}
else {
xStatus_TaskGPIOcalc = xQueueSendToFront(qDRIVE, &uDRIVE_temp, 0); //запись в начало очереди нового сотояния команды перемещения
}
}
calc = 1; // расчет выполнен
// понижаем приоритет задачи
vTaskPrioritySet( NULL, ( tskIDLE_PRIORITY) );
} // calc != 1
} // for (;;)
}
void MOTOR_Tasks ( void )
{
while(1)
{
if(motorData.theQueue != 0)
{
if(xQueuePeek(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY );
/* int i = 0;
while( motorData.rxMessage.SeqNum != motorData.RxSeqNum)
{
i++;
if(i == MOTORQUEUELENGTH)
{
motorData.RxSeqNum++; //drop a message of that seq number by incrementing
i = 0;
}
xQueueSendToBack(motorData.theQueue, (void*)&(motorData.rxMessage), 0); //send to back
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), 0 ); //get another
}//*/
motorData.state = motorData.rxMessage.command;
// motorData.state = 1;
// debugUInt(motorData.state);
// debugUInt(motorData.duration);
communication_sendIntMsg(motorData.rxMessage.command, motorData.rxMessage.duration);
/* Check the application's current state. */
switch ( motorData.state )
{
/* Application's initial state. */
case MOTOR_STATE_INIT: //state 0, do nothing
{
// DRV_TMR1_Stop();
// DRV_TMR1_CounterClear();
// DRV_OC1_Stop();
// DRV_OC0_Stop();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_STRAIGHT: //state 1, go straight.
{
//TMRCount < OCSetting = high... 150/200 = 75% duty cycle
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, 150);
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, 150);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_TURNLEFT: //state 2, turn left.
{
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 1);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_TURNRIGHT: //state 3, turn right
{
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 1);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_LEFTFEEDBACK:
{
motorData.Lfeedback = motorData.rxMessage.duration;
break;
}
case MOTOR_STATE_RIGHTFEEDBACK:
{
motorData.Rfeedback = motorData.rxMessage.duration;
break;
}
/* TODO: implement your application state machine.*/
case MOTOR_STATE_Straight2: //state 1, go straight.
{
//TMRCount < OCSetting = high... 150/200 = 75% duty cycle
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, 150);
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, 150);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
// DRV_TMR1_Start();
// DRV_OC1_Start();
// DRV_OC0_Start();
motorData.duration = motorData.rxMessage.duration;
break;
}
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
} //end switch
}
}
else
{
crash("No Motor MsgQ");
}
}
}
