int32_t irobotSensorPollSensorGroup6( const irobotUARTPort_t port, irobotSensorGroup6_t * const sensorGroup6 ){ int32_t status = ERROR_SUCCESS; // initialize communications buffer uint8_t txBuffer[OP_SENSORS_SIZE]; uint8_t rxQueueBuffer[SENSOR_SIZE_UPPER_BOUND] = {0x00}; xqueue_t rxQueue; xqueue_init(&rxQueue, rxQueueBuffer, SENSOR_SIZE_UPPER_BOUND); // check for NULL pointer if(!sensorGroup6){ return ERROR_INVALID_PARAMETER; } // request packet txBuffer[0] = OP_SENSORS; txBuffer[1] = SENSOR_GROUP6; irobot_StatusMerge(&status, irobotUARTWriteRaw(port, txBuffer, OP_SENSORS_SIZE)); // receive response if(!irobot_IsError(status)){ irobot_StatusMerge(&status, irobotUARTRead(port, &rxQueue, SENSOR_GROUP6_SIZE)); } // read response if(!irobot_IsError(status)){ irobot_StatusMerge(&status, irobotReadSensorGroup6(&rxQueue, sensorGroup6)); } return status; }
int32_t LIBSTATECHARTEXAMPLE_EXP irobotNavigationStatechartSimulation( const int32_t netDistance, const int32_t netAngle, const uint8_t * const sensorStream, const int32_t sensorStreamSize, const double * const accelAxes, const int32_t accelAxesSize, int16_t * const pRightWheelSpeed, int16_t * const pLeftWheelSpeed ){ // construct sensors from a simulated sensor stream. Copy into xqueue structure then parse xqueue_t sensorStreamQueue; uint8_t sensorStreamQueueBuffer[SENSOR_SIZE_UPPER_BOUND]; irobotSensorGroup6_t sensors; accelerometer_t accel; bool packetFound = false; if (!sensorStream || !pRightWheelSpeed || !pLeftWheelSpeed || accelAxesSize != 3) return 1; //mgArgErr // Verify correct sensor stream packet size if(sensorStreamSize == SENSOR_GROUP6_SIZE + 4){ xqueue_init(&sensorStreamQueue, sensorStreamQueueBuffer, SENSOR_SIZE_UPPER_BOUND); xqueue_push_buffer(&sensorStreamQueue, sensorStream, sensorStreamSize); if(irobotSensorStreamProcessAll(&sensorStreamQueue, &sensors, &packetFound) >= 0){ if(packetFound){ accel.x = accelAxes[0]; accel.y = accelAxes[1]; accel.z = accelAxes[2]; // Execute statechart irobotNavigationStatechart(netDistance, netAngle, sensors, accel, true, pRightWheelSpeed, pLeftWheelSpeed); return ERROR_SUCCESS; } } } else{ fprintf(stderr, "irobotNavigationSensorStream() expected sensor packet size %d, received size %d.\n", SENSOR_GROUP6_SIZE + 4, sensorStreamSize); } return ERROR_INVALID_PARAMETER; }