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;
}
