Waypoint Navigation for the Pathfinder
By Jose Alcantar, Electronics and Controls Engineer
Introduction:
This blog post covers the proposed waypoint navigation algorithm along with some issues regarding how the GPS data was transmitted and decoded by the Arduino. Sample code and navigational calculations are included.
Navigation Algorithm:
When the pathfinder travel mode is to Autopilot, the Arxterra App begins transmitting heading information, current coordinate information of the smartphone and waypoint coordinates, if any. The basic control logic is as follows:
- If Autopilot mode is engaged, begin Waypoint Navigation
- Process any new GPS information and update the course and distance to the target waypoint.
- Read heading information to get current bearing and decide the desired direction to turn the pathfinder.
- Begin moving the pathfinder and check for any obstacles to avoid.
- If waypoint is reached, switch to manual control.
The code to handle each of these was written in separate functions.
Autopilot Mode:
When the autopilot mode is engaged the WAYPOINT_ON command is called, which begins the Navigation algorithm. The first function called retrieves the heading information from the smartphone which is sent as a data packet containing the command id and the heading information in hex values. For more detail regarding formatting the data see link below,
http://arxterra.com/heading-and-gps-coordinates-formatting/
After the heading is retrieved, a function is called to calculate the difference between the current heading and the heading to the desired waypoint.
The following code demonstrates how to calculate the turn:
// returns distance in meters between two positions, both specified
// as signed decimal-degrees latitude and longitude. Uses great-circle
// distance computation for hypothetical sphere of radius 6372795 meters.
// Because Earth is no exact sphere, rounding errors may be up to 0.5
void calcDesiredTurn(void)
{
//calculate where we need to turn to head to destination
headingError = targetHeading – currentHeading;
//adjust for compass wrap
if (headingError < -180)
headingError += 360;
if (headingError > 180)
headingError -= 360;
// calculate which way to turn to intercept the targetHeading
if (abs(headingError) <= HEADING_TOLERANCE) // if within tolerance dont turn
turnDirection = straight; // GO FORWARD
else if (headingError < 0)
turnDirection = left; // TURN LEFT
else if (headingError > 0)
turnDirection = right; // TURN RIGHT
else
turnDirection = straight; // GO FORWARD
} // calcDesiredTurn()
After the target heading is calculated, the distance to the target waypoint is calculated using the following code:
int distanceToWaypoint()
{
float delta = radians(currentLon – waypointLon);
float sdlong = sin(delta);
float cdlong = cos(delta);
float lat1 = radians(currentLat);
float lat2 = radians(waypointLat);
float slat1 = sin(lat1);
float clat1 = cos(lat1);
float slat2 = sin(lat2);
float clat2 = cos(lat2);
delta = (clat1 * slat2) – (slat1 * clat2 * cdlong);
delta = sq(delta);
delta += sq(clat2 * sdlong);
delta = sqrt(delta);
float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
delta = atan2(delta, denom);
distanceToTarget = delta * 6372795;
//————-Edit to turn off navigation———-//
// check to see if we have reached the current waypoint
if (distanceToTarget <= WAYPOINT_DIST_TOLERANCE)
waypoint_on = 0;
//—————————————————//
return distanceToTarget;
} // distanceToWaypoint()
The previous functions return the new heading in degrees and the distance in meters
the next function, checkSonar() is called to read the ultrasonic sensor distance values.
void checkSonar(void)
{
distR = 0; distL = 0;
digitalWrite(11,LOW);
delayMicroseconds(2);
digitalWrite(11,HIGH);
delayMicroseconds(10);
digitalWrite(11, LOW);
pulse_width = pulseIn(8, HIGH);
distR = (pulse_width/2)/29.1;
delay(60);
digitalWrite(10,LOW);
delayMicroseconds(2);
digitalWrite(10,HIGH);
delayMicroseconds(10);
digitalWrite(10, LOW);
pulse_width = pulseIn(4, HIGH);
distL = (pulse_width/2)/29.1;
delay(60);
if( distR > MAX_DISTANCE_IN && distL > MAX_DISTANCE_IN)
{
distR = MAX_DISTANCE_IN;
distL = MAX_DISTANCE_IN;
}
} //checkSonar()
After calculating the distance, target heading and reading the sensor values the moveAndAvoid() function is called to move the pathfinder to its destination while avoiding obstacles.
void moveAndAvoid(void)
{
if(distL >= SAFE_DISTANCE && distR >= SAFE_DISTANCE) //no close objects in front
{
if(turnDirection == straight)
{
speedLeft = FAST_SPEED; //PWM SIGNAL FOR BOTH MOTORS
speedRight = FAST_SPEED;
left_forward();
right_forward();
}
else if(turnDirection == left)
{
speedLeft = TURN_SPEED * .85; //PWM RATIO FOR BOTH MOTORS
speedRight = TURN_SPEED;
left_forward();
right_forward();
}
else if(turnDirection == right)
{
speedLeft = TURN_SPEED;
speedRight = TURN_SPEED * .85;
left_forward();
right_forward();
//driveMotor->setSpeed(speed);
//driveMotor->run(FORWARD); // turn direction
//turnMotor->run(turnDirection);
//else(turnDirection == right)
//speed = TURN_SPEED;
}
return;
}
if ((distL > TURN_DISTANCE || distR > TURN_DISTANCE) && (distL < SAFE_DISTANCE || distR < SAFE_DISTANCE)) //not yet time to turn, but slow down
{
if(turnDirection == straight)
{
speedLeft = NORMAL_SPEED; // PWM FOR MOTORS
speedRight = NORMAL_SPEED;
left_forward();
right_forward();
}
else if(turnDirection == left)
{
speedLeft = TURN_SPEED * .85; //SPEED RATIO FOR MOTORS
speedRight = TURN_SPEED;
left_forward();
right_forward();
// already turning to navigate —-> Direction to turn
}
else if(turnDirection == right)
{
speedLeft = TURN_SPEED;
speedRight = TURN_SPEED * .85;
left_forward();
right_forward();
}
return;
}
if ((distL < TURN_DISTANCE || distR < TURN_DISTANCE) && (distL > STOP_DISTANCE || distR > STOP_DISTANCE)) // getting close, time to turn to avoid object
{
speedLeft = SLOW_SPEED; //PWM FOR MOTORS
speedRight = SLOW_SPEED;
left_forward();
right_forward();
switch(turnDirection) // decides whether to turn left or right
{
case straight: // going straight
{
if (headingError <= 0)
turnDirection = left;
else
turnDirection = right;
speedLeft = SLOW_SPEED;
speedRight = SLOW_SPEED * .85;
left_forward();
right_forward();
//turnMotor->run(turnDirection); // turn in the new direction
break;
}
case left: // if already turning left, try right
{
speedLeft = SLOW_SPEED;
speedRight = SLOW_SPEED * .85;
left_forward();
right_forward();
//turnMotor ->run(TURN_RIGHT);
break;
}
case right:
{
speedLeft = SLOW_SPEED * .85;
speedRight = SLOW_SPEED;
left_forward();
right_forward();
//turnMotor ->run(TURN_LEFT);
break;
}
} //end SWITCH
return;
}
if (distL < STOP_DISTANCE || distR < STOP_DISTANCE) // too close, stop and back up
{
left_release();
right_release();
//driveMotor->run(RELEASE); // stop
//turnMotor->run(RELEASE); // straighten up
turnDirection = straight;
speedLeft = NORMAL_SPEED;
speedRight = NORMAL_SPEED;
left_reverse();
right_reverse();
//driveMotor->setSpeed(NORMAL_SPEED); // go back at higher speet
//driveMotor->run(BACKWARD);
while (distL < TURN_DISTANCE || distR < TURN_DISTANCE) // backup until we get safe clearance
{
currentHeading = readCompass(); // get our current heading
calcDesiredTurn(); // calculate how we would optimatally turn, without regard to obstacles
checkSonar();
delay(100);
} // while (sonarDistance < TURN_DISTANCE)
left_release();
right_release();
//driveMotor->run(RELEASE); // stop backing up
return;
} // end of IF TOO CLOSE
} //moveAndAvoid
NOTES: One of the main issues with the waypoint navigation is that the heading data received does not give the full 0 – 360 degree range. When formatting the data, the heading is given from 0 to 180 degrees (North = 0 and South = 180) going counter clockwise, but any heading past 180 degrees does not transmit correctly. When formatting, the values given from the data packet, the degree range from (180 to 360) loops back, (going counter clockwise) the degree range is North = 180 and South = 360 degrees this causes an issue with calculating the correct heading values. It is recommended that Jeff is contacted to resolve this issue.
Conclusion:
With the following algorithm the pathfinder should be able to reach its destination with no issues.
