kj::Array <capnp::word> Fcore::messageWorker(
void* segmentsPtrsQ,
long long int segmentsSizesQ)
{
// to the capnp query
capnp::word* queryWords = static_cast <capnp::word*>(segmentsPtrsQ);
size_t querySize = segmentsSizesQ / sizeof(capnp::word);
kj::DestructorOnlyArrayDisposer adp;
kj::Array <capnp::word> msgArray(queryWords, querySize, adp);
kj::ArrayPtr <capnp::word> arrayPtrQ = msgArray.asPtr();
capnp::FlatArrayMessageReader cpnQuery(arrayPtrQ);
// read the capnproto struct
FcMsg::Message::Reader msgQ = cpnQuery.getRoot <FcMsg::Message>();
auto msgPtrQ = boost::make_shared <FcMsg::Message::Reader>(msgQ);
// the new capnproto reply
boost::shared_ptr <capnp::MallocMessageBuilder> cpnReply;
// work the query by the query type
switch (msgPtrQ->getMsgType()) {
case FcConst::MSG_TYPE_GET_FRAGMENT_TEXT: {
GetFragmentTextMsg msg(msgPtrQ);
cpnReply = msg.msgWorker();
break;
}
case FcConst::MSG_TYPE_CREATE_TEST_MODULE: {
CreateTestModuleMsg msg(msgPtrQ);
cpnReply = msg.msgWorker();
break;
}
case FcConst::MSG_TYPE_GET_FILE_TEXT: {
SendFileTextMsg msg(msgPtrQ);
cpnReply = msg.msgWorker();
break;
}
case FcConst::MSG_TYPE_GET_TEST_TEXT: {
GetTestTextMsg msg(msgPtrQ);
cpnReply = msg.msgWorker();
break;
}
default: {
SendErrorMsg msg(msgPtrQ);
cpnReply = msg.msgWorker();
break;
}
} // switch
// send the reply
if (nullptr != cpnReply) {
// capnproto message to the serialization
kj::Array <capnp::word> replyWords = messageToFlatArray(*cpnReply);
return (kj::mv(replyWords));
}
kj::Array <capnp::word> empty = kj::heapArray <capnp::word>(0);
return (kj::mv(empty));
} // Fcore::messageWorker
// This is the actual task that is run
static portTASK_FUNCTION( vi2cTempUpdateTask, pvParameters )
{
int byte1 = 0;
int byte2 = 0;
//float countPerC = 100.0, countRemain=0.0;
// Get the parameters
vtTempStruct *param = (vtTempStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->dev;
// Get the LCD information pointer
lcdOScopeStruct *lcdData = param->lcdData;
// String buffer for printing
char lcdBuffer[vtOScopeMaxLen+1];
// Buffer for receiving messages
vtTempMsg msgBuffer;
uint8_t currentState;
// Assumes that the I2C device (and thread) have already been initialized
// This task is implemented as a Finite State Machine. The incoming messages are examined to see
// whether or not the state should change.
//
// Temperature sensor configuration sequence (DS1621) Address 0x4F
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempInit,0x4F,sizeof(i2cCmdInit),i2cCmdInit,0) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
currentState = vtI2CMsgTypeTempRead1;
// Like all good tasks, this should never exit
for(;;)
{
// Wait for a message from either a timer or from an I2C operation
if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
// Now, based on the type of the message and the state, we decide on the new state and action to take
switch(getMsgType(&msgBuffer)) {
case vtI2CMsgTypeTempInit: {
if (currentState == fsmStateInit1Sent) {
currentState = fsmStateInit2Sent;
// Must wait 10ms after writing to the temperature sensor's configuration registers(per sensor data sheet)
vTaskDelay(10/portTICK_RATE_MS);
// Tell it to start converting
if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempInit,0x4F,sizeof(i2cCmdStartConvert),i2cCmdStartConvert,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
} else if (currentState == fsmStateInit2Sent) {
currentState = fsmStateTempRead1;
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case TempMsgTypeTimer: {
// Timer messages never change the state, they just cause an action (or not)
if ((currentState != fsmStateInit1Sent) && (currentState != fsmStateInit2Sent)) {
printf("got a timer call yo");
// Read in the values from the temperature sensor
// We have three transactions on i2c to read the full temperature
// we send all three requests to the I2C thread (via a Queue) -- responses come back through the conductor thread
// Temperature read -- use a convenient routine defined above
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead1,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,2) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
printf("TIMER GET\n");
if (vtI2CEnQ(devPtr,voidMsg,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,4) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
// Read in the read counter
/*if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead2,0x4F,sizeof(i2cCmdReadCnt),i2cCmdReadCnt,2) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}*/
// Read in the slope;
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead3,0x4F,sizeof(i2cCmdReadSlope),i2cCmdReadSlope,1) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
} else {
// just ignore timer messages until initialization is complete
}
break;
}
case vtI2CMsgTypeTempRead1: {
if (currentState == fsmStateTempRead1) {
currentState = fsmStateTempRead1;
byte1 = msgBuffer.buf[0];
byte2 = msgBuffer.buf[1];
uint16_t data = (byte2<<8)|byte1;
SendLCDOScopeMsg(lcdData,data,portMAX_DELAY);
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case vtI2CMsgTypeTempRead2: {
if (currentState == fsmStateTempRead2) {
currentState = fsmStateTempRead1;
byte2 = getValue(&msgBuffer);
uint16_t data = (byte2<<8)|byte1;
SendLCDOScopeMsg(lcdData,data,portMAX_DELAY);
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
default: {
VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
break;
}
}
}
}
static portTASK_FUNCTION(vmotorTask, pvParameters) {
motorStruct *param = (motorStruct *) pvParameters;
motorMsg msg;
const uint8_t bc_half_forward[] = {0xBC, 0xA0, 0x20, 0xFF, 0xFF, 0x00};
const uint8_t bc_fast_forward[] = {0xBC, 0x9B, 0x1B, 0xFF, 0xFF, 0x00};
const uint8_t bd_half_forward[] = {0xBD, 0xA0, 0x20, 0xFF, 0xFF, 0x00};
// const uint8_t ba_15cm_forward[] = {0xBA, 0xA0, 0x20, 0x0F, 0x0F, 0x00}; // 0.5ft
// const uint8_t ba_45cm_forward[] = {0xBA, 0xA0, 0x20, 0x2D, 0x2D, 0x00}; // 1.5ft
const uint8_t ba_90_left[] = {0xBA, 0xE0, 0x20, 0x12, 0x12, 0x00};
const uint8_t ba_90_right[] = {0xBA, 0xA0, 0x60, 0x12, 0x12, 0x00};
uint8_t ba_custom_forward[] = {0xBA, 0xA0, 0x20, 0x2D, 0x2D, 0x00};
const uint8_t *motorCommand = bc_half_forward;
SendLCDPrintMsg(param->lcdData,20,"motorTask Init",portMAX_DELAY);
for( ;; ) {
//wait forever or until queue has something
if (xQueueReceive(param->inQ,(void *) &msg,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
switch(getMsgType(&msg)) {
//only one type of message so far
case SENSORTASK_MSG: {
switch(getMoveType(&msg)) {
case ROVERMOVE_FORWARD_ABSOLUTE:
motorCommand = ba_custom_forward;
uint8_t distance = getDistance(&msg);
ba_custom_forward[3] = distance;
ba_custom_forward[4] = distance;
break;
case ROVERMOVE_FORWARD_CORRECTED:
if(msg.macroState == MACROSTATE_RUN_TWO)
motorCommand = bc_fast_forward;
else
motorCommand = bc_half_forward;
break;
case ROVERMOVE_TURN_LEFT:
motorCommand = ba_90_left;
break;
case ROVERMOVE_TURN_RIGHT:
motorCommand = ba_90_right;
break;
case ROVERMOVE_FORWARD_SPECIALD:
motorCommand = bd_half_forward;
break;
}
//current slave address is 0x4F, take note
if (vtI2CEnQ(param->dev, vtRoverMovementCommand, 0x4F, 6, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
SendLCDPrintMsg(param->lcdData,20,"SND: Move Command",portMAX_DELAY);
break;
}
case ROVERACK_ERROR: {
//this is where the arm will re-request the movement ack from the rover
if (vtI2CEnQ(param->dev, vtRoverMovementCommand, 0x4F, 6, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
SendLCDPrintMsg(param->lcdData,20,"RSND: Move Command",portMAX_DELAY);
break;
}
}
}
}
bool CmdJobsWeight::v_processArguments( int argc, char** argv, af::Msg &msg)
{
msg.set( getMsgType());
return true;
}
// This is the actual task that is run
static portTASK_FUNCTION( vMotorControlTask, pvParameters )
{
// Get the parameters
param = (motorControlStruct *) pvParameters;
// Get the I2C task pointer
i2cData = param->i2cData;
// Get the Navigation task pointer
webData = param->webData;
// Get the LCD task pointer
lcdData = param->lcdData;
prevSpeedVal = MOTOR_FORWARD_SPEED;
curSpeedVal = MOTOR_FORWARD_SPEED;
currentOp = NONE;
lastOp = NONE;
rightEncoderCount = 0;
leftEncoderCount = 0;
currentTime = 0;
speedDiff = 0;
forward = 0;
reverse = 0;
right = 0;
left = 0;
msg[0] = 'f';
msg[1] = ' ';
msg[4] = 'b';
msg[5] = ' ';
msg[8] = 'r';
msg[9] = ' ';
msg[13] = 'l';
msg[14] = ' ';
msg[18] = ' ';
msg[19] = 0;
// Like all good tasks, this should never exit
for(;;)
{
// Wait for a message from the I2C (Encoder data) or from the Navigation Task (motor command)
if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(Q_RECV_ERROR);
}
//sendi2cMotorMsg(i2cData, '!', 1, portMAX_DELAY);
switch(getMsgType(&msgBuffer)){
case motorTimerMsgType:
{
currentTime++;
if(currentOp != NONE)
{
if(currentTime >= targetVal)
{
sendi2cMotorMsg(i2cData,MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET,MOTOR_STOP_SPEED, portMAX_DELAY);
currentOp = NONE;
currentTime = 0;
}
}
if(currentTime%10 == 0){
switch(lastOp){
case FORWARD:
{
forward = forward + getCentimeters(rightEncoderCount, leftEncoderCount);
//updateMoveForwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
break;
}
case REVERSE:
{
reverse = reverse + getCentimeters(rightEncoderCount, leftEncoderCount);
//updateMoveBackwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
break;
}
case RIGHT:
{
right = right + getDegrees(rightEncoderCount, leftEncoderCount);
//updateRotateClockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
break;
}
case LEFT:
{
left = left + getDegrees(rightEncoderCount, leftEncoderCount); //This is exactly right... Because difference will be negative... Think this out later.
//updateRotateCounterclockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
break;
}
default:
{
break;
}
}
rightEncoderCount = 0;
leftEncoderCount = 0;
#ifdef SEND_COUNTS_TO_LCD
msg[2] = (forward / 10) % 10 + 48;
msg[3] = forward % 10 + 48;
msg[6] = (reverse / 10) % 10 + 48;
msg[7] = reverse % 10 + 48;
msg[10] = (right / 100) % 10 + 48;
msg[11] = (right / 10) % 10 + 48;
msg[12] = right % 10 + 48;
msg[15] = (left / 100) % 10 + 48;
msg[16] = (left / 10) % 10 + 48;
msg[17] = left % 10 + 48;
//SendLCDPrintMsg(lcdData, 20, msg);
#endif
}
break;
}
case setDirForwardMsgType:
{
currentOp = FORWARD;
lastOp = FORWARD;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case setDirReverseMsgType:
{
currentOp = REVERSE;
lastOp = REVERSE;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case setMotorSpeedMsgType:
{
speedDiff = curSpeedVal - getNewSpeed(&msgBuffer);
prevSpeedVal = curSpeedVal;
curSpeedVal = curSpeedVal + speedDiff;
break;
}
case turnRightMsgType:
{
currentOp = RIGHT;
lastOp = RIGHT;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case turnLeftMsgType:
{
currentOp = LEFT;
lastOp = LEFT;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case motorStopMsgType:
{
currentOp = NONE;
//prevSpeedVal = MOTOR_STOP_SPEED; // This is undoubtedly incorrect so look at this Matt!!
//curSpeedVal = MOTOR_STOP_SPEED; // This is undoubtedly incorrect so look at this Matt!!
sendi2cMotorMsg(i2cData, MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET, MOTOR_STOP_SPEED, portMAX_DELAY);
currentTime = 0;
break;
}
case encoderDataMsgType:
{
rightEncoderCount += getRightCount(&msgBuffer);
leftEncoderCount += getLeftCount(&msgBuffer);
// char msg[12];
// msg[0] = 'L';
// msg[1] = ':';
// msg[2] = ' ';
// msg[3] = (getLeftCount(&msgBuffer) / 10) % 10 + 48;
// msg[4] = getLeftCount(&msgBuffer) % 10 + 48;
// msg[5] = ' ';
// msg[6] = 'R';
// msg[7] = ':';
// msg[8] = ' ';
// msg[9] = (getRightCount(&msgBuffer) / 10) % 10 + 48;
// msg[10] = getRightCount(&msgBuffer) % 10 + 48;
// msg[11] = 0;
// SendLCDPrintMsg(lcdData, 11, msg);
break;
}
default:
{
VT_HANDLE_FATAL_ERROR(UNKNOWN_MOTOR_CONTROL_MSG_TYPE);
break;
}
}
}
}
