Пример #1
0
void navigateToWaypoint(float xtarget, float ytarget)
{
    while(!atTarget(xtarget, x) || (!atTarget(ytarget, y)))
    {
        float distance = calcDistance(xtarget, ytarget);
        float angle = calcAngle(xtarget, ytarget);

        float driveDistance = 0; //initialise
        if (distance < stepDistance)
        {
            driveDistance = distance;
        }
        else
        {
            driveDistance = stepDistance;
        }

        // co-ordinate system is different so cos and sin are swapped - take from current location not origin
        float tempWaypointX = x + (driveDistance * cos(angle));
        float tempWaypointY = y + (driveDistance * sin(angle));


        driveToWaypoint(tempWaypointX, tempWaypointY);
        monteCarlo();

        eraseDisplay();
        drawMap();
        drawParticles();
        wait1Msec(100);

    }

    PlayTone(784, 15);
}
void RosbeeProtocolHandler::run(){
	//Mavlink command packet that will be filled.
	mavlink_command_long_t packet;
	//Start he handshake with server.
	init();
	//Run forever, there is no need for this thread to ever end.
	while(true){
		//Wait for a new incoming packet.
		//This function will block unit something arrives.
		rosClient->waitReceiveMessage(packet);
		//Check which command needs to be executed.
		//For now we only have one command.
		switch(packet.command){
		case ROSBEE_COMMAND_FUNCTIONS::SENDWAYPOINT:
			//Drive to waypoint.
			driveToWaypoint(packet);
			break;
		}
	}
}