ME 461 Final Project - Real-time Waypoint Navigation

Overview

For the final project, our team modified the Segbot (which is controlled via the TI F28379D red board) so that it can complete real-time waypoint navigation. The user is able to type in and send four waypoints into a modified version of the LabVIEW interface. The robot receives these four values and sequentially goes to each determined waypoint through the use of a state machine and navigation code. Progress can be tracked through songs and LEDs on the Segbot.

Demo video

Sensors

We utilized the MPU-9250/6500 and two motor optical encoders. The MPU-9250 is connected via Serial Peripheral Interface (SPI), and provides three-axis of acceleration and gyroscopic data, which were used to keep the Segbot balanced.

Each motor has an attached optical encoder, which gives the angular position of the shaft of the motor. These encoders have a small wheel which is attached to the axle of the motor. As the motor rotates, optical sensors detect the passing of 100 slits on this disk. In conjunction with the encoders, we used the enhanced Quadrature Encoder Pulse (eQEP) peripheral, which is a part of the F28379D processor (Red board). With eQEP, and a gear ratio of 30:1, we have the granularity of 12000 counts per one revolution of the wheel. This resolution allows us to balance the Segbot more effectively, as well as enhances our dead reckoning capabilities.

Sensor video

LabVIEW

The user interface was built as a LabVIEW VI. The VI is used to send the coordinates of the waypoint and draw a birds-eye view of the room the Segbot is driving in which contains the path the Segbot will follow and the real-time position of the Segbot. The Segbot is allowed to travel anywhere within the boundaries of a 12 by 12-foot square foam wall. Upon starting the LabVIEW VI, there is a blank 2D picture in the front panel.

To set up the LabVIEW program, the user specifies the IP address of the Segbot it will be communicating within the block diagram of the VI.

The user runs a command in a PuTTY terminal to accept communications from the VI to the Segbot via a WiFi chip on the Segbot.

In the front panel of the VI, the user specifies X and Y coordinates of four waypoints.

The Segbot is placed in the center of the 12 by 12-foot foam wall facing north, with the motors running and the Segbot balancing in place. These first four steps can be performed in any order.

After the program is set up, the user can then run the program and press “SEND” and it sends the eight coordinates to the Segbot’s WiFi chip. When the Segbot receives the coordinates, the state machine begins to process the waypoints immediately. A map is separately drawn in the 2D picture on the front panel. The map populates with five important elements upon pressing the send button:

A 20 by 20-foot grid of dotted lines is drawn on the empty picture, corresponding to the tiles on the floor in the room where the Segbot is driven.

1. A 20 by 20-foot grid of dotted lines is drawn on the empty picture, corresponding to the tiles on the floor in the room where the Segbot is driven.

2. Text is displayed in the center of the grid that shows the default starting position of the Segbot. The starting position can be adjusted in the block diagram.

3. Four dots are drawn corresponding to the four waypoints specified in the front panel. A line is drawn connecting the four waypoints. This is the path that the Segbot will follow.

4. A 12 by 12-foot border is drawn to represent the foam walls the Segbot can drive in. The user can make sure the four waypoints are inside the border to prevent collision between the Segbot and the wall.

5. A square with a line coming out of it represents the position of the Segbot in the room. The square moves with the Segbot in real time because its position is based on the data received from the Segbot. The square turns to represent the orientation of the Segbot and the line coming out of the square points towards the direction the Segbot is heading. As the Segbot moves between the four waypoints, the square moves along the path and stops at each waypoint.

After successfully following the path, the Segbot returns to the initial position. At this point, the user can either press “STOP” or “SEND.” If the user presses “STOP, ” the program stops running and the Segbot idles at the starting position. If the user presses “SEND, ” the Segbot runs the course one more time.

LabVIEW Video

State Machine

In order to control the waypoint navigation and system communication with LabVIEW, a state machine was implemented. The state machine was created using a case structure in the Final_Project_main.c file. The case structure consists of five states to help tell the system when to complete certain tasks.

The first state checks if new waypoints have been sent to the system by using the waypointsReceived flag which is set to true when data is received from LabVIEW. If new waypoints have been received, the system sets the waypointsReceived flag to false and updates the state to state two once state one completes. Next, the waypoint X and Y values are populated, and the current waypoint—the next destination—is set to the first waypoint that was sent. To achieve this, an iterating variable waypointIterate is used and initialized at zero.

The second state commands the Segbot to go to the current waypoint using the navigation code and controller. When the current waypoint is reached, the songSelect flag is sent to play a small tune, and the state is updated to state three.

The third state lets the Segbot take a short pause to play the waypoint reached song and update the current waypoint counter on the LED display. After the short delay, the waypointIterate value is checked to determine if another waypoint still needs to be gone to. If there is another waypoint, the current waypoint is updated and the state set back to state two. If there are no additional waypoints, the Segbot updates its state to state four and plays the completion song.

The fourth state simply sends the Segbot back to its starting position. Once the starting position is reached, the Segbot updates its state to the fifth and final state.

The fifth state tells the Segbot to hold its position in the same spot that it started. If new waypoints are received, the state is set back to one and the process happens again.

In building the state machine with this structure, a new set of waypoints can be sent to the Segbot while it completes the initial set of waypoints sent, without interfering with its current course. By using the waypointIterate flag, it can also be used to signal which LED number to display throughout the navigation process. An image of the state machine’s flow chart is shown below.

Navigation Control

The navigation control functionality creates a linkage between the PID control that was created in earlier labs, and the waypoint requests received from LabVIEW. We previously made the Segbot to balance using a Kalman Filter and PID controllers. This balance control effort was multiplexed with two other control efforts, fwdback and turn; fwdback causes the SegBot to lean, creating an intentional drift forward or backward, while turn causes a bias to one wheel, making the Segbot turn.

This navigation control takes the current location of the Segbot and the desired location from the waypoint, and returns a velocity and turn command, which is then passed to the PID controllers mentioned prior. The velocity command is scaled based on the distance between the Segbot and it’s target location. The turn value, similarly, is scaled by the angular difference between the Segbot and the direction it needs to face in order to move towards its target location.

Both command variables are constantly updated as the Segbot operates. As the Segbot reaches its target, it will automatically slow down to avoid overshooting it. Also, to help avoid overshoot, a radius of 0.25 ft, or a quarter of a tile, is extended around the target point. Once the Segbot arrives within that radius it will indicate that it has reached the target and stop.

Extras

Extra features for the project include LED’s that keep track of the current waypoint being traveled to and a song that plays once the robot reaches a waypoint. The song plays with the use of a buzzer on the robot’s greenboard. EPWM signals are sent from the TI F28379D red board corresponding to different notes. The notes are then played by the buzzer. A different song is played once the robot has reached all four of the waypoints.