/*---------------------------------------------------------------------------/
/ Queue managing function
/---------------------------------------------------------------------------*/
void xQueueInit(xCOM USARTx)
{
if((USARTx == USART1 ? xUSART1_TxQueue : xUSART6_TxQueue) != serINVALID_QUEUE)
xQueueReset(USARTx == USART1 ? xUSART1_TxQueue : xUSART6_TxQueue);
if((USARTx == USART1 ? xUSART1_RxQueue : xUSART6_RxQueue) != serINVALID_QUEUE)
xQueueReset(USARTx == USART1 ? xUSART1_RxQueue : xUSART6_RxQueue);
}
void APP1_Initialize ( void )
{
/* Place the App state machine in its initial state. */
app1Data.state = APP1_STATE_INIT;
BSP_Initialize();
BSP_LED_LightShowSet(BSP_LED_EASY_CONFIGURATION);
app1Data.newPotSamp = 0;
app1Data.potTimer = 0;
app1Data.newVoltageSamp = 0;
app1Data.currIsLVD = true;
// Queue for switch data
app1Data.switchQueue = xQueueCreate( 20, sizeof(mySwitchMessage) );
if(app1Data.switchQueue == NULL) {
; // Handle this
}
// Queue for LED light show data
// This will hold the requested Light Show state only
app1Data.lightShowQueue = xQueueCreate( 1, 4 );
if(app1Data.lightShowQueue == NULL) {
; // Handle this
}
// Queue for potentiometer data
// This will hold the latest potentiometer data
app1Data.potentiometerQueue = xQueueCreate( 20, sizeof(app1Data.potValue) );
if(app1Data.potentiometerQueue == NULL) {
; // Handle this
}
xQueueReset(app1Data.switchQueue);
xQueueReset(app1Data.lightShowQueue);
xQueueReset(app1Data.potentiometerQueue);
}
void IDT_MapIntersection(char* leftPath, char* rightPath, char* forwardPath)
{
MoveDistance(1, 100);
StandardMessage msg;
xQueueReset(MsgQueue_MapSensor_Interrupt);
xQueueReceive( MsgQueue_MapSensor_Interrupt, &msg, 500 );
*leftPath = (char)(msg.data == 15 || msg.data == 47);
*rightPath = (char)(msg.data == 60 || msg.data == 62);
MoveDistance(39, 600);
xQueueReset(MsgQueue_MapSensor_Interrupt);
xQueueReceive( MsgQueue_MapSensor_Interrupt, &msg, 500 );
*forwardPath = (msg.data != 0);
}
//------------------------------------------------------------------------------------
portBASE_TYPE FreeRTOS_UART_ioctl( Peripheral_Descriptor_t pxPeripheral, uint32_t ulRequest, void *pvValue )
{
Peripheral_Control_t * const pxPeripheralControl = ( Peripheral_Control_t * const ) pxPeripheral;
portBASE_TYPE xReturn = pdPASS;
UART_device_control_t *pUart;
pUart = pxPeripheralControl->phDevice;
switch( ulRequest )
{
case ioctlUART_ENABLE:
cbi(UARTCTL_PORT, UARTCTL);
break;
case ioctlUART_DISABLE:
sbi(UARTCTL_PORT, UARTCTL);
break;
case ioctlOBTAIN_BUS_SEMPH:
// Espero el semaforo en forma persistente.
while ( xSemaphoreTake(pxPeripheralControl->xBusSemaphore, ( TickType_t ) 1 ) != pdTRUE )
taskYIELD();
break;
case ioctlRELEASE_BUS_SEMPH:
xSemaphoreGive( pxPeripheralControl->xBusSemaphore );
break;
case ioctlSET_TIMEOUT:
pxPeripheralControl->xBlockTime = *((u08 *)pvValue);
break;
case ioctl_UART_CLEAR_RX_BUFFER:
if ( pUart->rxBufferType == QUEUE) {
xQueueReset(pUart->rxStruct);
} else {
xFifoReset(pUart->rxStruct);
}
break;
case ioctl_UART_CLEAR_TX_BUFFER:
if ( pUart->txBufferType == QUEUE) {
xQueueReset(pUart->txStruct);
} else {
xFifoReset(pUart->txStruct);
}
break;
default :
xReturn = pdFAIL;
break;
}
return xReturn;
}
AJ_Status AJ_QueueReset(struct AJ_Queue* q)
{
if (q && q->q) {
xQueueReset(q->q);
}
return AJ_OK;
}
void runMeasureCycle(int sensor_id){
static uint32_t echo_time;
static BaseType_t qResult;
uint32_t pin = id2pin[sensor_id];
uint32_t echo = id2echo[sensor_id];
rpi_gpio_ev_detect_enable(echo, GPIO_EV_RISING_EDGE | GPIO_EV_FALLING_EDGE);
//vTaskDelay(1);
xQueueReset(sonar_data_q);
rpi_gpio_set_val(pin, 1);
microDelay(10);
//vTaskDelay(1);
rpi_gpio_set_val(pin, 0);
qResult = xQueueReceive(sonar_data_q, &echo_time, 50);
if(pdTRUE == qResult){
sonar_data[sensor_id] = (float)echo_time / 1000000.0 * SSPEED_CM_S * 0.5435;
} else {
sonar_data[sensor_id] = -1.0;
}
rpi_gpio_ev_detect_disable(echo, GPIO_EV_RISING_EDGE | GPIO_EV_FALLING_EDGE);
}
unsigned char sensorcommunication_getByteISR()
{
COMMUNICATION_MESSAGE theMessage;
if(xQueueReceiveFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0))
{
int i = 0;
if( theMessage.type == sensorcommunicationData.sensorIntRxMsgSeq)
{
sensorcommunicationData.sensorRxMsgSeq++;
return theMessage.msg;
}
else
{
while( theMessage.type != sensorcommunicationData.sensorIntRxMsgSeq)
{
i++;
if(i == COMMUNICATIONQUEUESIZE)
{
xQueueReset(sensorcommunicationData.sensorIntQueue); //clear queue. we dropped the packet
}
xQueueSendToBackFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0); //send to back
xQueueReceiveFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0 ); //get another
}
debugU("COM message loaded\n");
return theMessage.msg;
}
}
}
mcual_i2c_status_t mcual_i2c_transmit(mcual_i2c_id_t id, uint8_t addr, uint8_t * txbuf, uint8_t tx_size, uint8_t * rxbuf, uint8_t rx_size)
{
(void)rxbuf;
(void)rx_size;
I2C_TypeDef * reg = mcual_i2c_get_register(id);
int i;
addr <<= 1;
xEventGroupClearBits(transfer_done[id], 0xFF);
xQueueReset(tx_queues[id]);
xQueueSend(tx_queues[id], &addr, portMAX_DELAY);
for(i = 0; i < tx_size; i += 1)
{
xQueueSend(tx_queues[id], txbuf + i, portMAX_DELAY);
}
reg->CR2 |= I2C_CR2_ITEVTEN | I2C_CR2_ITERREN;
reg->CR1 = I2C_CR1_START | I2C_CR1_PE;
EventBits_t result = xEventGroupWaitBits(transfer_done[id], 0xFF, pdFALSE, pdFALSE, 1000 / portTICK_PERIOD_MS);
I2C1->CR1 = I2C_CR1_STOP;
if(result & (1 << MCUAL_I2C_SUCCESS))
{
return MCUAL_I2C_SUCCESS;
}
return MCUAL_I2C_FAIL;
}
void APP_Tasks ( void )
{
IDT_UpdateDistance(&appData.rightEncoderCount, &appData.leftEncoderCount);
StandardMessage messageFromMapSensorInterrupt;
if( xQueueReceive( MsgQueue_MapSensor_Interrupt, &messageFromMapSensorInterrupt, 0) )
{
IDT_CorrectDirection(messageFromMapSensorInterrupt.data);
if( IDT_CheckForEnd(messageFromMapSensorInterrupt.data) )
{
EventData intersectionInformation;
intersectionInformation.distFromLastIntersection = ClosestValueFromTicks( (appData.rightEncoderCount < appData.leftEncoderCount) ? appData.rightEncoderCount : appData.leftEncoderCount );
intersectionInformation.forwardPathExists = 0;
intersectionInformation.rightPathExists = 0;
intersectionInformation.leftPathExists = 0;
intersectionInformation.absoluteDirection = '0';
intersectionInformation = MakeATurn(intersectionInformation);
appData.rightEncoderCount = 80; //SinceLastIntersection
appData.leftEncoderCount = 80;
}
else if( IDT_CheckForIntersection(messageFromMapSensorInterrupt.data) )
{
IDT_MapIntersection(&appData.leftPath, &appData.rightPath, &appData.forwardPath);
EventData intersectionInformation;
intersectionInformation.distFromLastIntersection = ClosestValueFromTicks( (appData.rightEncoderCount < appData.leftEncoderCount) ? appData.rightEncoderCount : appData.leftEncoderCount );
intersectionInformation.forwardPathExists = appData.forwardPath;
intersectionInformation.rightPathExists = appData.rightPath;
intersectionInformation.leftPathExists = appData.leftPath;
intersectionInformation.absoluteDirection = '0';
intersectionInformation = MakeATurn(intersectionInformation);
appData.rightEncoderCount = 20; //SinceLastIntersection
appData.leftEncoderCount = 20;
}
xQueueReset(MsgQueue_MapSensor_Interrupt);
}
/* Check the application's current state. */
switch ( appData.state )
{
/* Application's initial state. */
case APP_STATE_INIT:
{
//moveRobot(500,500);
break;
}
//deals w/ finding intersections and getting notifications.
/* TODO: implement your application state machine.*/
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
}
}
void prvBaseController( void *prvParameters ) {
uint8_t ucPlatformControl;
( void ) prvParameters;
LPC_GPIO2->FIODIR |= ( 1 << 6 ) | ( 1 << 7 ) | ( 1 << 8 ); // #1 Forward - Reverse - EN
LPC_GPIO2->FIODIR |= ( 1 << 10 ) | ( 1 << 11 ) | ( 1 << 12); // #2 Forward - Reverse - EN
for( ;; ) {
xQueueReceive( xPlatformGPIOQueue, &ucPlatformControl, portMAX_DELAY );
/* Send the platform [F]orward by sending forward output to motors 1 and 2. */
if ( 'F' == ucPlatformControl ) {
LPC_GPIO2->FIOCLR = ( 1 << 7 );
LPC_GPIO2->FIOSET = ( 1 << 6 ) | ( 1 << 8 );
LPC_GPIO2->FIOCLR = ( 1 << 11);
LPC_GPIO2->FIOSET = ( 1 << 10 ) | ( 1 << 12 );
}
/* Send the platform in re[V]erse by sending reverse output to motors 1 and 2. */
else if ( 'V' == ucPlatformControl ) {
LPC_GPIO2->FIOCLR = ( 1 << 6 );
LPC_GPIO2->FIOSET = ( 1 << 7 ) | ( 1 << 8 );
LPC_GPIO2->FIOCLR = ( 1 << 10);
LPC_GPIO2->FIOSET = ( 1 << 11 ) | ( 1 << 12 );
}
/* Turn the platform [L]eft by forwarding motor 2, reversing motor. 1 */
else if ( 'L' == ucPlatformControl ) {
LPC_GPIO2->FIOCLR = ( 1 << 6 );
LPC_GPIO2->FIOSET = ( 1 << 7 ) | ( 1 << 8 );
LPC_GPIO2->FIOCLR = ( 1 << 11 );
LPC_GPIO2->FIOSET = ( 1 << 10 ) | ( 1 << 12 );
}
/* Turn the platform [R]ight by forwarding motor 1, reversing motor. 2 */
else if ( 'R' == ucPlatformControl ) {
LPC_GPIO2->FIOCLR = ( 1 << 7 );
LPC_GPIO2->FIOSET = ( 1 << 6 ) | ( 1 << 8 );
LPC_GPIO2->FIOCLR = ( 1 << 10 );
LPC_GPIO2->FIOSET = ( 1 << 11 ) | ( 1 << 12 );
}
/* [S]top motors 1 & 2, bring the platform to a halt. */
else if ( 'S' == ucPlatformControl ) {
LPC_GPIO2->FIOSET = ( 1 << 8 );
LPC_GPIO2->FIOCLR = ( 1 << 6 ) | ( 1 << 7 );
LPC_GPIO2->FIOSET = ( 1 << 12 );
LPC_GPIO2->FIOCLR = ( 1 << 10 ) | ( 1 << 11 );
/* A 0x00 byte continues the current operation of the motors, anything else is
* an invalid command. Clear ENs and wait for next command */
} else if ( 0x00 != ucPlatformControl ) {
LPC_GPIO2->FIOCLR = ( 1 << 8 ) | ( 1 << 12 );
}
/* Reset the GPIO queue to wait for the next instruction */
xQueueReset( xPlatformGPIOQueue );
}
}
int crtpReset(void)
{
xQueueReset(txQueue);
if (link->reset) {
link->reset();
}
return 0;
}
/**
* @brief UART Event Task Entry
*
* @param param task parameter
*/
static void uart_event_task_entry(void *param)
{
esp_modem_dte_t *esp_dte = (esp_modem_dte_t *)param;
uart_event_t event;
while (1) {
if (xQueueReceive(esp_dte->event_queue, &event, pdMS_TO_TICKS(100))) {
switch (event.type) {
case UART_DATA:
esp_handle_uart_data(esp_dte);
break;
case UART_FIFO_OVF:
ESP_LOGW(MODEM_TAG, "HW FIFO Overflow");
uart_flush_input(esp_dte->uart_port);
xQueueReset(esp_dte->event_queue);
break;
case UART_BUFFER_FULL:
ESP_LOGW(MODEM_TAG, "Ring Buffer Full");
uart_flush_input(esp_dte->uart_port);
xQueueReset(esp_dte->event_queue);
break;
case UART_BREAK:
ESP_LOGW(MODEM_TAG, "Rx Break");
break;
case UART_PARITY_ERR:
ESP_LOGE(MODEM_TAG, "Parity Error");
break;
case UART_FRAME_ERR:
ESP_LOGE(MODEM_TAG, "Frame Error");
break;
case UART_PATTERN_DET:
esp_handle_uart_pattern(esp_dte);
break;
default:
ESP_LOGW(MODEM_TAG, "unknown uart event type: %d", event.type);
break;
}
}
/* Drive the event loop */
esp_event_loop_run(esp_dte->event_loop_hdl, pdMS_TO_TICKS(50));
}
vTaskDelete(NULL);
}
void TurnLeft()
{
char rightEncoderCount = 0;
char leftEncoderCount = 0;
xQueueReset(MsgQueue_MapEncoder_Interrupt);
moveRobot(-600,600);
while(leftEncoderCount < 56)
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
moveRobot(0,0);
}
int Uart3Ctrl(const Uart3CtrlCmd_TypeDef cmd, void *arg)
{
switch(cmd)
{
case Uart3CTRL_ClearRxQueue:
xQueueReset(uart3_rx_queue);
break;
default:
break;
}
return 0;
}
void TurnAround()
{
char rightEncoderCount = 0;
char leftEncoderCount = 0;
xQueueReset(MsgQueue_MapEncoder_Interrupt);
moveRobot(600,-600);
while(rightEncoderCount < 112)
{
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
}
moveRobot(0,0);
}
/*
Deallocates a mailbox. If there are messages still present in the
mailbox when the mailbox is deallocated, it is an indication of a
programming error in lwIP and the developer should be notified.
*/
void
sys_mbox_free(sys_mbox_t *mbox)
{
uint32_t mbox_message_num = 0;
if ( (NULL == mbox) || (NULL == *mbox) ) {
return;
}
mbox_message_num = uxQueueMessagesWaiting((*mbox)->os_mbox);
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("mbox free: mbox=%p os_mbox=%p owner=%p msg_num=%d\n",
*mbox, (*mbox)->os_mbox, (*mbox)->owner, mbox_message_num));
#if ESP_THREAD_SAFE
if ((*mbox)->owner) {
if (0 == mbox_message_num) {
/*
* If mbox->owner is not NULL, it indicates the mbox is recvmbox or acceptmbox,
* we need to post a NULL message to mbox in case some application tasks are blocked
* on this mbox
*/
if (sys_mbox_trypost(mbox, NULL) != ERR_OK) {
/* Should never be here because post a message to empty mbox should always be successful */
ESP_LOGW(TAG, "WARNING: failed to post NULL msg to mbox\n");
} else {
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("mbox free: post null successfully\n"));
}
}
(*mbox)->owner = NULL;
} else {
if (mbox_message_num > 1) {
ESP_LOGW(TAG, "WARNING: mbox has %d message, potential memory leaking\n", mbox_message_num);
}
if (mbox_message_num > 0) {
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("mbox free: reset mbox queue\n"));
xQueueReset((*mbox)->os_mbox);
}
/* For recvmbox or acceptmbox, free them in netconn_free() when all sockets' API are returned */
vQueueDelete((*mbox)->os_mbox);
free(*mbox);
*mbox = NULL;
}
#else
vQueueDelete((*mbox)->os_mbox);
free(*mbox);
*mbox = NULL;
#endif
}
void APP2_Initialize ( void )
{
/* Place the App state machine in its initial state. */
app2Data.state = APP2_STATE_INIT;
/* Device Layer Handle */
app2Data.deviceHandle = USB_DEVICE_HANDLE_INVALID ;
/* Device configured status */
app2Data.isConfigured = false;
/* Initial get line coding state */
app2Data.getLineCodingData.dwDTERate = 115200;
app2Data.getLineCodingData.bParityType = 0;
app2Data.getLineCodingData.bDataBits = 8;
/* Read Transfer Handle */
app2Data.readTransferHandle = USB_DEVICE_CDC_TRANSFER_HANDLE_INVALID;
/* Write Transfer Handle */
app2Data.writeTransferHandle = USB_DEVICE_CDC_TRANSFER_HANDLE_INVALID;
/* Intialize the read complete flag */
app2Data.isReadComplete = true;
/*Initialize the write complete flag*/
app2Data.isWriteComplete = true;
/* Reset the switch debounce counter */
app2Data.switchDebounceTimer = 0;
/* Reset other flags */
app2Data.sofEventHasOccurred = false;
/* Set up the read buffer */
app2Data.readBuffer = &readBuffer[0];
/* Set up the write buffer */
app2Data.writeBuffer = &writeBuffer[0];
/* Initialize the timers */
app2Data.messageTimeout = 0;
/* Setup debug message queue */
app2Data.debugQueue = xQueueCreate( 20, sizeof(int) );
if(app2Data.debugQueue == NULL) {
; // Handle this
}
xQueueReset(app2Data.debugQueue);
}
/*
Deallocates a mailbox. If there are messages still present in the
mailbox when the mailbox is deallocated, it is an indication of a
programming error in lwIP and the developer should be notified.
*/
void
sys_mbox_free(sys_mbox_t *mbox)
{
#define MAX_POLL_CNT 100
#define PER_POLL_DELAY 20
uint16_t count = 0;
bool post_null = true;
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free: set alive false\n"));
(*mbox)->alive = false;
while ( count++ < MAX_POLL_CNT ){ //ESP32_WORKAROUND
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free:try lock=%d\n", count));
if (!sys_mutex_trylock( &(*mbox)->lock )){
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free:get lock ok %d\n", count));
sys_mutex_unlock( &(*mbox)->lock );
break;
}
if (post_null){
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free: post null to mbox\n"));
if (sys_mbox_trypost( mbox, NULL) != ERR_OK){
ESP_STATS_DROP_INC(esp.free_mbox_post_fail);
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free: post null mbox fail\n"));
} else {
post_null = false;
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free: post null mbox ok\n"));
}
}
if (count == (MAX_POLL_CNT-1)){
ESP_LOGW(TAG, "WARNING: mbox %p had a consumer who never unblocked. Leaking!\n", (*mbox)->os_mbox);
}
sys_delay_ms(PER_POLL_DELAY);
}
LWIP_DEBUGF(ESP_THREAD_SAFE_DEBUG, ("sys_mbox_free:free mbox\n"));
if (uxQueueMessagesWaiting((*mbox)->os_mbox)) {
xQueueReset((*mbox)->os_mbox);
/* Line for breakpoint. Should never break here! */
__asm__ volatile ("nop");
}
void MoveDistance(int d, int speed) // 1/17th of an inch
{
xQueueReset(MsgQueue_MapEncoder_Interrupt);
int rightEncoderCount = 0;
int leftEncoderCount = 0;
if(d < 0)
{
moveRobot(-1*speed, -1*speed);
d = -1*d;
}
else
{
moveRobot(speed, speed);
}
while( rightEncoderCount < d && leftEncoderCount < d )
{
IDT_UpdateDistance(&rightEncoderCount, &leftEncoderCount);
}
moveRobot(0,0);
}
static int xfsSpeakLowLevel(const char *p, int len, char type) {
int ret;
portBASE_TYPE rc;
xQueueReset(uartQueue);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, 0xFD);
ret = len + 2;
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, ret >> 8);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, ret & 0xFF);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, 0x01);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, type);
for (ret = 0; ret < len; ret++) {
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, *p++);
}
rc = xQueueReceive(uartQueue, &ret, configTICK_RATE_HZ);
if (rc != pdTRUE) {
return 0;
}
if (ret != 0x41) {
return 0;
}
ret = 0;
while (ret <= len) {
if (xfsQueryState() == 0x4F) {
return 1;
}
vTaskDelay(configTICK_RATE_HZ / 2);
++ret;
}
return 0;
}
void xUartQueueFlush(u08 nUart)
{
/* Vacio la cola de recepcion */
if (nUart == 0) {
if (bufferType == RINGBUFFER ) {
if ( xSemaphoreTake ( sem_GprsUart, 10 ) ) {
buffer_flush ( &xUart0RxedCharsBuffer, TRUE);
xSemaphoreGive( sem_GprsUart);
}
} else {
buffer_flush ( &xUart0RxedCharsBuffer, TRUE);
}
return;
} else {
xQueueReset(xUart1RxedCharsQueue);
//while (xUartGetChar( 1, &cChar, ( portTickType ) 10 ) );
return;
}
}
static int xfsSendCommand(const char *dat, int size, int timeoutTick) {
char ret;
portBASE_TYPE rc;
xQueueReset(uartQueue);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, 0xFD);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, size >> 8);
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, size & 0xFF);
while (size-- > 0) {
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
USART_SendData(USART3, *dat++);
}
rc = xQueueReceive(uartQueue, &ret, timeoutTick);
if (rc != pdTRUE) {
return -1;
}
return ret;
}
static void prvDemoQueueSpaceFunctions( void *pvParameters )
{
QueueHandle_t xQueue = NULL;
const unsigned portBASE_TYPE uxQueueLength = 10;
unsigned portBASE_TYPE uxReturn, x;
/* Remove compiler warnings. */
( void ) pvParameters;
/* Create the queue that will be used. Nothing is actually going to be
sent or received so the queue item size is set to 0. */
xQueue = xQueueCreate( uxQueueLength, 0 );
configASSERT( xQueue );
for( ;; )
{
for( x = 0; x < uxQueueLength; x++ )
{
/* Ask how many messages are available... */
uxReturn = uxQueueMessagesWaiting( xQueue );
/* Check the number of messages being reported as being available
is as expected, and force an assert if not. */
if( uxReturn != x )
{
/* xQueue cannot be NULL so this is deliberately causing an
assert to be triggered as there is an error. */
configASSERT( xQueue == NULL );
}
/* Ask how many spaces remain in the queue... */
uxReturn = uxQueueSpacesAvailable( xQueue );
/* Check the number of spaces being reported as being available
is as expected, and force an assert if not. */
if( uxReturn != ( uxQueueLength - x ) )
{
/* xQueue cannot be NULL so this is deliberately causing an
assert to be triggered as there is an error. */
configASSERT( xQueue == NULL );
}
/* Fill one more space in the queue. */
xQueueSendToBack( xQueue, NULL, 0 );
}
/* Perform the same check while the queue is full. */
uxReturn = uxQueueMessagesWaiting( xQueue );
if( uxReturn != uxQueueLength )
{
configASSERT( xQueue == NULL );
}
uxReturn = uxQueueSpacesAvailable( xQueue );
if( uxReturn != 0 )
{
configASSERT( xQueue == NULL );
}
/* The queue is full, start again. */
xQueueReset( xQueue );
#if( configUSE_PREEMPTION == 0 )
taskYIELD();
#endif
}
}
void SoundTask::clear() {
xQueueReset(_tones);
}
void prvTCPCpmIOTask( void *ram )
{
BaseType_t iosize;
char cRxedChar, cInputIndex = 0;
struct freertos_sockaddr xClient;
Socket_t xListeningSocket;
socklen_t xSize = sizeof( xClient );
cpminq = xQueueCreate(81, sizeof( CHAR));
while(FreeRTOS_IsNetworkUp() == pdFALSE)
vTaskDelay(3000);
/* Create the socket. */
xSocketRDisk = FreeRTOS_socket( FREERTOS_AF_INET,
FREERTOS_SOCK_DGRAM,
FREERTOS_IPPROTO_UDP );
/* Check the socket was created. */
configASSERT( xSocketRDisk != FREERTOS_INVALID_SOCKET );
for( ;; )
{
/* Attempt to open the socket. The port number is passed in the task
parameter. The strange casting is to remove compiler warnings on 32-bit
machines. NOTE: The FREERTOS_SO_REUSE_LISTEN_SOCKET option is used,
so the listening and connecting socket are the same - meaning only one
connection will be accepted at a time, and that xListeningSocket must
be created on each iteration. */
xListeningSocket = prvOpenTCPServerSocket( RDSK_PORT);
/* Nothing for this task to do if the socket cannot be created. */
if( xListeningSocket == FREERTOS_INVALID_SOCKET )
{
vTaskDelete( NULL );
}
/* Wait for an incoming connection. */
xConnectedSocket = FreeRTOS_accept( xListeningSocket, &xClient, &xSize );
/* The FREERTOS_SO_REUSE_LISTEN_SOCKET option is set, so the
connected and listening socket should be the same socket. */
configASSERT( xConnectedSocket == xListeningSocket );
xRDiskAddress.sin_addr = xClient.sin_addr;
xRDiskAddress.sin_port = FreeRTOS_htons( RDSK_PORT );
iosize = xTaskCreate( CPM22Task, "CPM22Task", configMINIMAL_STACK_SIZE*5, ram, PRIO_CPM22,&thcpm);
if(iosize != pdPASS)
{
prvGracefulShutdown( xListeningSocket );
vTaskDelete( NULL );
}
/* Send the welcome message. */
iosize = FreeRTOS_send( xConnectedSocket, ( void * ) pcWelcomeMessage, strlen( pcWelcomeMessage ), 0 );
xQueueReset(cpminq);
/* Process the socket as long as it remains connected. */
while( iosize >= 0 )
{
char c;
/* Receive data on the socket. */
iosize = FreeRTOS_recv( xConnectedSocket, &c, 1, 0 );
if( iosize >= 0 )
{
xQueueSend(cpminq,&c,0);
}
else
{
/* Socket closed? */
break;
}
}
/* Close the socket correctly. */
prvGracefulShutdown( xListeningSocket );
}
}
static void prvTakeAndGiveInTheOppositeOrder( void )
{
/* Ensure the slave is suspended, and that this task is running at the
lower priority as expected as the start conditions. */
#if( INCLUDE_eTaskGetState == 1 )
{
configASSERT( eTaskGetState( xSlaveHandle ) == eSuspended );
}
#endif /* INCLUDE_eTaskGetState */
if( uxTaskPriorityGet( NULL ) != intsemMASTER_PRIORITY ) {
xErrorDetected = pdTRUE;
}
/* Take the semaphore that is shared with the slave. */
if( xSemaphoreTake( xMasterSlaveMutex, intsemNO_BLOCK ) != pdPASS ) {
xErrorDetected = pdTRUE;
}
/* This task now has the mutex. Unsuspend the slave so it too
attempts to take the mutex. */
vTaskResume( xSlaveHandle );
/* The slave has the higher priority so should now have executed and
blocked on the semaphore. */
#if( INCLUDE_eTaskGetState == 1 )
{
configASSERT( eTaskGetState( xSlaveHandle ) == eBlocked );
}
#endif /* INCLUDE_eTaskGetState */
/* This task should now have inherited the priority of the slave
task. */
if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
xErrorDetected = pdTRUE;
}
/* Now wait a little longer than the time between ISR gives to also
obtain the ISR mutex. */
xOkToGiveMutex = pdTRUE;
if( xSemaphoreTake( xISRMutex, ( xInterruptGivePeriod * 2 ) ) != pdPASS ) {
xErrorDetected = pdTRUE;
}
xOkToGiveMutex = pdFALSE;
/* Attempting to take again immediately should fail as the mutex is
already held. */
if( xSemaphoreTake( xISRMutex, intsemNO_BLOCK ) != pdFAIL ) {
xErrorDetected = pdTRUE;
}
/* Should still be at the priority of the slave task. */
if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
xErrorDetected = pdTRUE;
}
/* Give back the shared semaphore to ensure the priority is not disinherited
as the ISR mutex is still held. The higher priority slave task should run
before this task runs again. */
if( xSemaphoreGive( xMasterSlaveMutex ) != pdPASS ) {
xErrorDetected = pdTRUE;
}
/* Should still be at the priority of the slave task as this task still
holds one semaphore (this is a simplification in the priority inheritance
mechanism. */
if( uxTaskPriorityGet( NULL ) != intsemSLAVE_PRIORITY ) {
xErrorDetected = pdTRUE;
}
/* Give back the ISR semaphore, which should result in the priority being
disinherited as it was the last mutex held. */
if( xSemaphoreGive( xISRMutex ) != pdPASS ) {
xErrorDetected = pdTRUE;
}
if( uxTaskPriorityGet( NULL ) != intsemMASTER_PRIORITY ) {
xErrorDetected = pdTRUE;
}
/* Reset the mutex ready for the next round. */
xQueueReset( xISRMutex );
}
static void mqtt_task(void *pvParameters)
{
int ret = 0;
struct Network network;
MQTTClient client = DefaultClient;
char mqtt_client_id[20];
uint8_t mqtt_buf[100];
uint8_t mqtt_readbuf[100];
MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
NewNetwork( &network );
memset(mqtt_client_id, 0, sizeof(mqtt_client_id));
strcpy(mqtt_client_id, "ESP-");
strcat(mqtt_client_id, get_my_id());
while(1) {
xSemaphoreTake(wifi_alive, portMAX_DELAY);
printf("%s: started\n\r", __func__);
printf("%s: (Re)connecting to MQTT server %s ... ",__func__,
MQTT_HOST);
ret = ConnectNetwork(&network, MQTT_HOST, MQTT_PORT);
if( ret ){
printf("error: %d\n\r", ret);
taskYIELD();
continue;
}
printf("done\n\r");
NewMQTTClient(&client, &network, 5000, mqtt_buf, 100,
mqtt_readbuf, 100);
data.willFlag = 0;
data.MQTTVersion = 3;
data.clientID.cstring = mqtt_client_id;
data.username.cstring = MQTT_USER;
data.password.cstring = MQTT_PASS;
data.keepAliveInterval = 10;
data.cleansession = 0;
printf("Send MQTT connect ... ");
ret = MQTTConnect(&client, &data);
if(ret){
printf("error: %d\n\r", ret);
DisconnectNetwork(&network);
taskYIELD();
continue;
}
printf("done\r\n");
MQTTSubscribe(&client, "/esptopic", QOS1, topic_received);
xQueueReset(publish_queue);
while(1){
char msg[PUB_MSG_LEN - 1] = "\0";
while(xQueueReceive(publish_queue, (void *)msg, 0) ==
pdTRUE){
printf("got message to publish\r\n");
MQTTMessage message;
message.payload = msg;
message.payloadlen = PUB_MSG_LEN;
message.dup = 0;
message.qos = QOS1;
message.retained = 0;
ret = MQTTPublish(&client, "/beat", &message);
if (ret != SUCCESS ){
printf("error while publishing message: %d\n", ret );
break;
}
}
ret = MQTTYield(&client, 1000);
if (ret == DISCONNECTED)
break;
}
printf("Connection dropped, request restart\n\r");
DisconnectNetwork(&network);
taskYIELD();
}
}
void prvArmController( void *prvParameters ) {
uint8_t cCommand, cServoPosition[5];
int8_t sParameters[4];
( void ) prvParameters;
/* At startup, initialize the pins on the LPC1769 for PWM. */
prvInitPWM();
for(;;) {
/* For for the Op-Code and the PWM position. */
xQueueReceive( xOpCodeQueue, &cCommand, portMAX_DELAY );
xQueueReceive( xPWMQueue, sParameters, portMAX_DELAY );
if ( 'r' == cCommand || 'a' == cCommand ) {
/* The PWM positions received via the UART receiver allow for control of the servos with a
* maximum resolution of 128 per 1000 pulses (the servos bounded by more restricted pulse
* width will have, obviously, less). PWM positions are calculated by multiplying the byte
* value by 8. For the relative (0x72) command, this number is simply added to the current
* value in the match register; for absolute, the new position is calculated as this number
* plus 1000 (the minimum pulse width).
*
* Current position of servo 1: 1500
* sParameters[0] = 0x2a
* The new position calculated from the 'a' (0x61) command is (8 * 0x2a) + 1000 = 1336.
* The new position calcualted form the 'r' (0x72) command is 1500 + (8 * 0x2a) = 1836.
*
* Note: the parameters bytes can also be negative (e.g., 0xa4), and this would cause a
* subtraction from the relative position (no effect on absolute).
*/
/* An 'r' command (byte 0x72) move the servo to a new position relative to its current. */
if ( 'r' == cCommand ) {
LPC_PWM1->MR1 += 8 * sParameters[0];
LPC_PWM1->MR2 += 8 * sParameters[1];
LPC_PWM1->MR3 += 8 * sParameters[2];
LPC_PWM1->MR4 += 8 * sParameters[3];
/* An 'a' command (byte 0x61) sets the servo to an absolute position between 1000 and 2000. */
} else if ( 'a' == cCommand ) {
LPC_PWM1->MR1 = 1000 + ( 4 * ( uint8_t )sParameters[0] );
LPC_PWM1->MR2 = 1000 + ( 4 * ( uint8_t )sParameters[1] );
LPC_PWM1->MR3 = 1000 + ( 4 * ( uint8_t )sParameters[2] );
LPC_PWM1->MR4 = 1000 + ( 4 * ( uint8_t )sParameters[2] );
}
/* The following lines check to make sure the servo positions are not going over or under their
* safe working positions, and sets them to the maximum or minimum value if this is the case.
*/
LPC_PWM1->MR1 = ( 2000 < LPC_PWM1->MR1 ) ? 2000 :
( 1000 > LPC_PWM1->MR1 ) ? 1000 :
LPC_PWM1->MR1;
LPC_PWM1->MR2 = ( 1650 < LPC_PWM1->MR2 ) ? 1650 :
( 1350 > LPC_PWM1->MR2 ) ? 1350 :
LPC_PWM1->MR2;
LPC_PWM1->MR3 = ( 1500 < LPC_PWM1->MR3 ) ? 1500 :
( 1050 > LPC_PWM1->MR3 ) ? 1050 :
LPC_PWM1->MR3;
LPC_PWM1->MR4 = ( 1500 < LPC_PWM1->MR4 ) ? 1500 :
( 1000 > LPC_PWM1->MR4 ) ? 1000 :
LPC_PWM1->MR4;
/* Set the latch enable register to latch MRs 1, 2, and 4 */
LPC_PWM1->LER = ( 1 << 1 ) | ( 1 << 2 ) | ( 1 << 3) | ( 1 << 4 );
/* The 'p' command return the position of the selected servo, in hex. */
} else if ('p' == cCommand ) {
if ( sParameters[0] )
itoa( LPC_PWM1->MR1, cServoPosition, 16);
else if ( sParameters[1] )
itoa( LPC_PWM1->MR2, cServoPosition, 16);
else if ( sParameters[2] )
itoa( LPC_PWM1->MR3, cServoPosition, 16);
else
itoa( 0, cServoPosition, 16);
if ( FreeRTOS_ioctl( xUART3, ioctlOBTAIN_WRITE_MUTEX, pwmctrl50ms ) == pdPASS ) {
FreeRTOS_write( xUART3, cServoPosition, strlen( cServoPosition ) );
}
}
/* Finally, clear the queues and wait for the next command */
xQueueReset( xOpCodeQueue );
xQueueReset( xPWMQueue );
}
}
void ButtonSubscription::clear() {
xQueueReset(mQueue);
}
void IDT_CorrectDirection(char c)
{
switch(c)
{
//NORMAL
case 12: //00110011
{
moveRobot(600,600);
break;
}
//WOBBLES
//Off left
case 8: //00111001
{
moveRobot(400,0);
break;
}
case 40: //00111100
{
moveRobot(400,0);
break;
}
case 32: //00111001
{
moveRobot(400,0);
break;
}
case 48: //00111100
{
moveRobot(400,0);
break;
}
case 16: //00111001
{
moveRobot(400,0);
break;
}
//Off right
case 2: //00100111
{
moveRobot(0,400);
break;
}
case 6: //00001111
{
moveRobot(0,400);
break;
}
case 4: //00001111
{
moveRobot(0,400);
break;
}
case 3: //00001111
{
moveRobot(0,400);
break;
}
case 1: //00001111
{
moveRobot(0,400);
break;
}
xQueueReset(MsgQueue_MapSensor_Interrupt);
}
}
