Example #1
0
  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
Example #2
0
// 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;
		}
		}


	}
}
Example #3
0
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;
			}
		}

	}
}
Example #4
0
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;
           }
       }
    }
}