MCP Navigation Program
The purpose of the navigation program is move the MCP from a starting position through a series of waypoits and then back to the starting position. Along the way, the program must adjust the MCP's course in order to avoid obstacles.
2. Algorithm Overview
The navigation program is designed so that going to each waypoint is a subtask. The program reads in a text file called sequence.txt. This file contains the order of the waypoints that the MCP should travel to. The program creates a stack of waypoints. The MCP takes the top waypoint of the stack and travels to that waypoint. When the MCP reaches a waypoint, the top of the stack is popped. The MCP proceeds to read the next waypoint on the stack and then travels to that one. This process is repeated until there are no more waypoints.
During each subtask, the MCP may encounter obstacles that are between itself and a waypoint. When an obstacle is detected, the MCP will calculate new coordinates that are 5 meters to the left or the right of the obstacle. A new waypoint will be created from those new coordinates and will be pushed onto the stack. The MCP then works on the new subtask. Waypoints generated during the running of the program are called fake waypoints. The MCP then has a new subtask of travelling to the fake waypoint. By travelling to the fake waypoint before a real waypoint, the MCP can avoid obstacles.
When the program is initialized, a new thread is created to poll the sonar subsystem. Information about each sonar is stored in shared memory. When the thread detects that an obstacle is in between the MCP and a waypoint directly in front of it, it signals to stop the current navigation command and calculates a fake waypoint that gets pushed onto the stack.
4. Position and Direction
In order to decide where to go in each subtask, the MCP must first know it's current direction and position in relation to the competition course. To obtain this information, the MCP has a thread that continuously receives data from a gpsd process. The gpsd gives the latitude and longitude values of the current position. Direction is calculated by plotting a line that goes from the previous position of the MCP to the new position of the MCP. The direction is expressed as the number of degrees counter-clockwise from due east. The current direction is called theta.
5. Motor and Steer Command Algorithm
The navigation program issues commands to the steering subsystem and the motor subsystem for each subtask. To decide how much the MCP needs to turn, a line from the MCP's current position is plotted to the current waypoint in the stack. A second line is plotted from the MCP's current position towards the east. The angle of that these two lines create is known as theta-prime. The diffference of theta-prime and theta is called delta-theta. Multiplying delta-theta by a angle-to-time ratio (to be determined by calibration), gives the MCP the amount of time needed to turn and in which direction. Dividing the distance between the current position and the current waypoint by the cruising speed of the MCP (to be determined by calibration) gives the MCP the amount of time the motors need to be on. The MCP then issues the calculated commands to the steering and to the motor subsystems. After each command is finished, the MCP determines if a new waypoint has been pushed on the stack. If a new waypoint has not been pushed onto the stack, it the MCP checks if it reached the waypoint it was currently going towards. If it has not reached that waypoint, it will attempt again to calculate new steering and motor commands to be issued. The MCP continues to calculate and issue commands for the current waypoint until the subtask is satisfied. If a new waypoint has been pushed onto the stack, though, the MCP stops calculating commands for the current waypoint and starts working on the new waypoint.
6. Motor and Steer Communication
Communication to the steering and motor subsystem is done by sending commands to the steeringServer process via a UDP socket.
7. Code Overview
The source code of the navigation program is written in C++ and C. It uses the Qt libraries for string manipulation and file handling, and it uses libgps to communicate to the GPS.