E80

Experimental Engineering

Lab 7: Autonomous Navigation

Team size: 4

In order to give everyone access to the tank, we will be running the lab as follows:

Group A - Version 1:  Run the lab as-is - Sections 1, 2, 3, 4, 5

Group B - Version 2:  Run the lab out of order - Sections 1, 5, 2, 3, 4

Until every team has completed the required portion of the lab, tank access for extra credit sections will be at the discretion of the Professor in charge.

Learning Objectives

After successful completion of this lab, you will be able to…


0. Introduction

The objective of this lab is to equip your robot with the ability to autonomously navigate between depth or X, Y waypoints. By doing so, you will learn how your robot's software fits together and about various navigation sensors.

This lab will be done in teams of 4 students. The lab will take place in multiple locations: students will start work in the E80 lab room, but some experiments will be conducted in the Parsons parking lot (where your GPS can receive signals) and in the tank room. Don't forget that you need to check out GPS's and other expensive equipment from professors or proctors. You will also need to fill out hardware failure reports if you need replacements for these pieces of equipment.

For pre-lab purposes, a sample submission sheet that contains all of the questions and requirements on the submission sheet is  here. 


1. Familiarize Yourself With the Software

Open E80_Lab_07.ino in the Arduino IDE. This sketch will be used as starter code for lab 7. Also make sure you have the up to date E80 libraries by performing a git fetch and checking that it's set as your current sketchbook. Confirm that you can compile and upload the starter code to your Teensy. 

In lab 1 you modified your main .ino file using the Arduino IDE. In this lab you will be modifying library files which are written in C++ (and thus have file extension .cpp). These files need to be opened in a text or code editor like Notepad or Sublime, not a Word Processor like Microsoft Word. The most popular source code editor these days is Visual Studio (VS) Code which is cross platform. Both support syntax coloring. The changed files will be compiled to your robot when you compile the E80_Lab_07.ino file from the Arduino IDE. You will also be asked to refer to header files (with the file extension .h) which are used to set up constants, initialize variables and do other house cleaning for the .cpp files of the same name.

The libraries are included in E80_Lab_07.ino using #include statements at the top of the file. Then commands from the libraries are used to create special variables, which we call objects, to represent your sensors like the GPS and the IMU. Finally, those objects are used to interact with the sensors. Read the E80_Lab_07.ino file and make note of where the libraries are included and used. Figure out how often your sensors are sampled by reading  E80_Lab_07.ino and the TimingOffsets.h header file located in libraries/main and record your answer on your submission sheet.


2. Position Measurements via IMU

Set up your PCB board stack that includes a teensy/dataloggerIMU/E80 PCB  with a battery. Confirm you can successfully log IMU data. Refer to lab 1 instructions if you need help. Information about the IMU can be found here.

With the data logger running, move your board stack along a linear path in the horizontal x-y plane, keeping the orientation fixed. That is, create your own global Cartesian coordinate frame on a table, and set the position of the board stack to be (0,0,0), and set the orientation of the board stack to be (0,0,0). Then, move the board stack through the following positions: (0,0,0), (0.5,0,0), (0,0,0). Assume coordinates are in units of meters. Don't move too fast. It may help to wait 5 seconds at the (0.5,0,0) way point.

Using MATLAB, load the IMU data and calculate the position of the board stack as a function of time. Plot the x,y coordinates of the board stack. Does the plotted path resemble the straight 0.5 meter path? If not, why? Think about how errors are accrued with double integration of IMU measurements. You can review the Integrals and Derivatives videos from the Data Analysis lectures if you need a refresher.


3. Magnetometer Calibration

A magnetometer measures the earth's magnetic field, so it can be used as a compass to sense your robot's heading. Read this article describing how it works. We have a magnetometer integrated into our IMU.

Every magnetometer has errors in its measurement, which can be byproducts of manufacturing or of local magnetic disturbances (eg: the magnetic declination is -10.52 degrees at Mudd). You need to calibrate those errors out before you can trust your magnetometer results. This article on magnetometer calibration is a good summary of the process.

Take your robot outside, far from buildings, and log your magnetometer data while your robot is held upright and you slowly (over ~10s) rotate 360 degrees while standing in place. Log your data. Next, program your Teensy with our magnetic calibration software -- Mag_Calibration.ino -- and use Mag_Calibration.m to extract the hard and soft iron correction coefficients. You will need MATLAB version 2018b or later to run this code. Save the figure generated by Mag_Calibration.m to your submission sheet. Update SensorIMU.h, which is included in the IMU reading library SensorIMU.cpp, with these coefficients. Turn in a circle again outside and save your magnetometer readings again.  Generate the following two plots: a scatter plot of mx vs. my where your calibrated and uncalibrated data are superimposed (what units are the axes?), and a plot of heading vs. time for both your calibrated and uncalibrated data.   

In this class we are using east-north-up local tangent plane coordinates. This means that you need to convert the heading from the IMU, which should have 0 degrees due North and positive angle in the CW direction, to yaw angles where 0 degrees points in the positive X direction (due East), and positive angles are in the CCW direction. If 0 degrees doesn't face perfectly East, (which is location dependent due to abnormalities in the Earth's magnetic field), you can add an offset angle to ensure it does. Put code to make this conversion in XYStateEstimator.cpp. Include the code you use to convert from heading to yaw in your submission sheet.

NOTE: the most common programming mistakes that we see are messing up units (is this angle in degrees or radians?) and failing to re-map angles between -pi and pi after they are calculated (3*pi/2 will crash a lot of your code unless you're very careful).


4. Position Measurements via GPS

Integrate the Adafruit Ultimate GPS into the board stack so that the Teensy has access to GPS measurements: First, solder a 9x1 female header pin row onto the motherboard at the position labelled GPS (see Lab 1 for details). Second, solder male header pins to the BOTTOM of the GPS breakout board. Be sure to get the orientation correct, (see image here). Note that screws, nuts, and standoffs may help secure the GPS breakout board to the motherboard.

Test your GPS Sensor integration by opening the serial monitor while E80_Lab_07.ino is running. Make sure your battery is plugged into the stack (your GPS is powered off the battery and not off the same USB connection that powers your Teensy), and that you are outside the building so that you can get GPS lock and measurements. 


Modify the library file named XYStateEstimator.cpp so that it calculates the x,y position (in meters) of the board stack with respect to a Cartesian coordinate frame whose origin is at ( 34.106465°, -117.712488°). Consult the XYStateEstimator.h file to see what variables you have available. Design your coordinate frame so the X axis points due East and the Y axis points due North. The code to make this transformation should be placed in the function named XYStateEstimator::updateState(), which is already in the file. Note, the x,y positions will be stored in the variables state.x and state.y

It may help to first make sure you are aware of how longitude and latitude work. This link is helpful and has a nice image to help visualize the coordinate system. There are several algorithms that have been used to convert longitude-latitude coordinates to x-y coordinates. There is the Vincenty formula, the Haversine equation, and linear transformations that simply scale the coordinates. The first two methods incorporate the curvature of the earth which is important when the vehicle is traveling many kilometers. Scaling methods are easier, but can only be used if distances are short. A good reference can be found here.

We will use a modified version of the Forward Equirectangular Projection, where we multiply by the radius of curvature R_earth. We note that the conversion of latitude to y is easier and if we assume the earth is a sphere (which it is to within ±0.25%), so we can use the  relationship between the distance along an arc's edge, the radius of the arc, and the angle, (e.g. state.y = R_earth*latitudeChange). To calculate x, note that it is a function of latitude, (e.g. state.x=R_earth*longituteChange*cos(latitude at origin)

To test that your sketch is functioning well, log the x,y positions (they are logged by default in the sample code) as you walk from the coordinate system origin to Foothill Blvd. Walk along the sidewalk East towards Shanahan. Then walk South between Shanahan and Parsons. Cut West back to Parson's building and end at the origin. The path is illustrated here.  It takes time (sometimes a few minutes) before the GPS receiver gets a lock on the signals from the satellites.  The GPS lock LED can help you identify if your board has achieved lock. You may want to have the serial monitor open (and print x,y to the screen) when walking the course: this will inform you if your conversion makes sense.  Using MATLAB, plot the logged x,y positions from the path. Only plot the x,y points for which the GPS has a lock on satellites.

NOTE: See above note on common programming errors.  They really crop up here.


5. P Control on Depth

In order to implement closed loop depth control with your AUV, it first needs to be able to sense its current depth. To measure depth, you will be using a pressure sensor adapter PCB (picture, schematic, layout, KiCAD) to connect a pressure sensor to pin A0 of your Teensy.  This adapter can be found on the central table.

In the library file named ZStateEstimator.cpp, in the function called ZStateEstimator::updateState(), you can see that the z position (depth) of the robot is calculated using the function’s input pressure_signal. The main loop() of the Arduino code is where z_state_estimator.updateState() is called. By reviewing this loop(), you can see that this pressure_signal input is receiving the output of analogRead(PRESSURE_PIN). analogRead() is an Arduino function that reads the value of a specified pin, and PRESSURE_PIN is a variable set in Pinouts.h, currently set to pin A0 (pin 14) of the Teensy. The pressure_signal input provides the pressure sensor signal in Teensy units. The ZStateEstimator::updateState() function then converts from Teensy units to Volts, then finally converts from Volts to depth [m] using the variables depthCal_slope and depthCal_intercept. This depth measurement is stored in state.z. 

The depthCal_slope and depthCal_intercept are not calibrated to your pressure sensor, so you will have to update them. Find where these variables are defined in the library file called ZStateEstimator.h (which is included in ZStateEstimator.cpp), and write down the default values and line numbers for these variables. After finding the calibration slope and intercept for your pressure sensor, this is where you will update them. 

NOTE: Navigating the code like this is helpful in understanding the interaction between the .h files, the .cpp files, and the Arduino sketch. The .h files are the interfaces, they hold information on variables that are used in other files and the functions of the .cpp files of the same name; the .cpp files are the implementations of these functions where the variables held in .h files are used; and the sketch is the top level instructions where some of these functions and variables are used to implement the desired behavior of the robot.

To calibrate your pressure sensor, get a pressure sensor adapter PCB and a pressure sensor, a graduated cylinder and a meter stick with pressure tubing. Edit ZStateEstmiator::updateState() by uncommenting the two lines of code below the calculation of the z position that initialize a string and print a string with the function printer.printMessage(). The pressure sensor voltage should now appear in the serial monitor, and you will use that display to calibrate your pressure sensor.

Attach the pressure tube to the pressure sensor and attach the adapter PCB to the motherboard, then plug the pressure sensor into the adapter. Open the serial monitor while E80_Lab_07.ino is running and record both depth and voltage readings. Measure at least seven evenly spaced depths (measured in meters) between the water level and the bottom of the cylinder. Plot the data in order to make a calibration curve and determine the equation of the line of best fit, then update the calibration slope and intercept in ZStateEstimator.h.

Add a Proportional depth control system to your robot by modifying the DepthControl.cpp file. Add the following steps:

Test your depth P control using the meter stick with tubing and the graduated cylinder. Attach the pressure tubing to your pressure sensor (careful not to bend the pressure sensor - it’ll be easier if you take the pressure sensor out of the adapter, attach the tubing, then plug it back in). Then, while E80_Lab_07.ino is running with the serial monitor open, insert the meter stick into the cylinder, keeping an eye on the DepthControl:, Depth_Des:, Depth:, and uV: values printed to the serial monitor. Make sure that the Depth is reporting a similar value to the current depth of the meter stick to confirm that your calibration works. Then watch the control effort uV change as the difference between Depth and Depth_Des changes, making sure it behaves as expected for proportional control.

Finally, get an E79 umbilical (found in the lab room) and attach it to your robot. Put the motor in the vertical position, and use a ziptie to secure the tube to the frame. Connect the tubing to your pressure sensor and the motor wires to the Motor C port on your motherboard. Test if the umbilical works by running E80_Lab_07.ino while the serial monitor is open. The motor should start spinning to get to the first waypoint at 0.5 meters. Check if it’s rotating in the correct direction; if not, switch the wires on your motherboard (or change the order you subtracted the depth and depth_des to get the error in DepthControl.cpp, or change the sign of Kp). Blow in the pressure tubing and confirm that the depth changes, and that the motor responds to the change in depth.

Once everything is tested and working, bring your robot to the tank room. Put it into the tank, run the sketch, and let the robot go to the two depth waypoints at 0.5m and 1m. Keep an eye on the values and messages printed to the serial monitor. Be sure to log your data, then plot the logged uV, depth and depth_des as a function of time.

NOTE: Be careful with the umbilical, it may not have enough slack for the robot to go down to 1 meter; if that’s the case, you can change the depth waypoints in the setup() function of E80_Lab_07.ino. If your robot’s motor is too strong or too weak, you may need to change your Kp in DepthControl.h.

NOTE: You can instruct the robot to stay at each depth waypoint for a specified amount of time by changing the diveDelay variable defined in the setup() function of E80_Lab_07.ino. e.g.: diveDelay = 3000 will tell the robot to wait at each depth waypoint for 3 seconds before continuing.


6. Clean up

Be sure to dry and return your robot and board stack to your designated Tupperware container in the cupboard.  Because this is the last lab of the semester, your team may keep the battery and GPS used for this lab.  Make sure the battery is not stored fully charged.


7. Deliverables

All labs require two submissions per group. The first submission is a submission sheet in which specific data must be shown. The submission sheet is due at the end of the 4 hour lab period. The submission sheet must be uploaded before the end of your lab session at 5:15 pm. Note that only ONE member of each team should access and submit the submission sheet. It is the responsibility of that team member to add the rest of the team’s names to the submission sheet. 

 For pre-lab purposes, a sample submission sheet that contains all of the questions and requirements on the submission sheet is here. 

The second submission is a writing assignment. In a deviation from normal patterns, we will do the whole writing assignment in class, and it will be due after the first 30 minutes of the writing and reflection section. The rest of the 3/10/2023 Friday section will be our Project Kickoff meeting.  


Extra Credit 1. Go Autonomous

In this section you will once again have your robot dive in the tank room using Proportional depth control, but this time you will use your waterproof box to hold your battery, motherboard and pressure sensor on your robot instead of using the umbilical, going completely autonomous.

You will need to use a pressure tube penetrator bolt (found in the lab room) so that the pressure sensor inside the box will be able to read the pressure outside the box. You will also need to drill another hole for this pressure penetrator bolt into your waterproof box. Be sure to use a 1/2” step drill bit (which we have in our cupboards) to drill out the hole. Standard drill bits will result in ragged holes that are not watertight. Make sure that the placement of your hole isn’t too close to any protrusions on the surface of the box; You want the rubber washer to be completely flat against the surface.

Next, you will want to test if your box is waterproofed with the new hole, then add ballast to your robot in order to make it close to neutrally buoyant. Refer to the section on AUV Assembly in Lab 1 for directions on doing this.

When your robot is ready, bring it to the tank room. Upload the sketch, make sure you’re logging your data, close your box and place your AUV in the water. Let your robot dive to the two depth waypoints (0.5) and (1), then include a plot of the uV, depth and depth_des as a function of time in the submission sheet (it should look similar to the submission for section 3). Then change the waypoints and the diveDelay, keeping the values within reason. Run the updated sketch and let your robot dive to the new waypoints, confirming that it’s waiting at each waypoint. Include a plot of the uV, depth and depth_des as a function of time for this run, which should show the depth staying still at each waypoint and the depth_des changing only after the delay is over.


Extra Credit 2: Proportional Surface Control

In this section you will implement a Proportional surface control system with your AUV using the GPS on your motherboard. Use E80_Lab_07_EC2.ino as the starter code for this portion.  You will also interact with two new library files: SurfaceControl.cpp and SurfaceControl.h. Confirm that you can compile and upload the starter code to your Teensy.

Add a Proportional surface control system to your robot by modifying the SurfaceControl.cpp file. Add the following steps:

Test your P control by walking the campus in the direction your P control guides you when the path is defined in the code to be the three x,y coordinates (125,-40), (150,-40), (125,-40). This should walk you 25 meters along the centerline of campus (see picture here). Walk at a slow speed , turning in response to the u_R and u_L values printed to the Serial monitor. E.g. if the the values are u_R=45 and u_L = 55, then veer right. Be sure to log your data.  Plot the logged path of x, y points, (bonus points if plotted on top of an image of campus). Also plot the angle error and control effort u as a function of time.

Finally, attach motors to the Motor A and Motor B port and repeat the above experiment, this time obeying your observations of the motor's rotation rates rather than printed u_R and u_L values.  Modify Kr and Kl if necessary to balance your motors. Pay special attention to how motors affect your yaw measurement: they have powerful magnetic fields that are likely to disrupt your experiment if you don't handle them with care.  Include plots of your x,y points, angle and control error.