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