E80 Project Mini-Lab: The Navigation Jump Start

Team size: 4

Submission sheet: html Word

Prelab help sheet: None, but you’re pros at this point.

Writing: None.

1 Introduction

This week we’re doing work in lab that is designed to accelerate you into the project. We are going to structure the work like a lab from the first half of the class – there are videos, a quiz and a submission sheet – but the goal is to provide you the tools you need to start right away on your project. Specifically, we want you to leave this Jump Start with an autonomously controlled robot and a confident attitude towards understanding and modifying the robot code.

Though this week’s activity looks like a lab. there are several crucial differences from a standard lab.

  • There will be no resubmissions allowed for lab 7 material. You’re done with this activity at the end of your lab session this week.
  • Consequently, grading standards are going to be a little gentler for lab 7 materials.
  • You don’t have to do all parts of this lab. See more discussion below.

In this lab, every team is going to do some activities to learn about your robot software, then test out using IMU data to dead reckon a course, which is an echo of the work that you did in lab 1. After that, your team will select one of the following activities to pursue during this lab, each of which are designed to help prepare you for different types of projects:

  • Diving Activity: In this activity you will build a pressure sensor and a P control loop for robot depth. You will test your loop in the tank. Pick this if you plan to have your robot dive or use a depth-controlled winch. It is possible that there will be a queue for the tank if many students in your section are doing the diving activity. Feel free to start on your robot build while you are waiting.
  • Surface Navigation Activity: In this activity you will use GPS and your magnetometer to control the bearing of your robot, and you will test that navigation system by walking around with your robot outside of Parsons. Pick this if you plan to do surface navigation, if you intend to have your robot keep station (stay still), or if you plan to do a hybrid diving/surface navigation scheme.

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

2 Software and IMU (All Teams Do this)

This section consists of some exercises to help you understand the E80 robot code followed by an exercise to explore the noise performance of the IMU.

2.1 Set up a Team Github Repository

Though E80 doesn’t require you to do a lot of software modification, it’s a good idea to make sure that your team is all able to work from the same code base. Modern best practices for doing so involve using Git for version control in conjunction with a web-based remote repository like Github. In this section (which can be completed entirely during prelab), your team will set up a shared repository so you can all work on the same code.

  1. All team members should make a Github account.
  2. Optional: Consider applying for Student access, which unlocks advanced Github features while you’re a student.
  3. One team member should
    1. Run a fetch and pull operation on the current E80 repository using Github Desktop (just to make sure you have the most up-to-date code).
    2. Make a new repository,
    3. Invite your team to the new repository.
    4. Clone your new repository using Github Desktop web integration or typical clone commands.
    5. Populate your new repository with all of your E80 code by copying and pasting it from your E80 repository folder to your new repository folder.
    6. Commit and push your copied and pasted code to the new repository.
    7. (Git savvy students will note that there are slicker ways to track both the E80 repository and your team’s work, for instance by forking the E80 repository. We’re doing this because it’s easy and we expect to push few software updates.)
  4. All other team members should clone the new repository (see links above).
  5. All team members should tell Arduino to look for sketches in the new repository by editing Settings -> Sketchbook Location.
Tip

We’ll be linking to specific files on the E80 Github repository below to talk about them, but remember that you downloaded all the Arduino and library files for this class in both your E80 repository and now in your team’s repository. So don’t download them again from the links, just edit the files in place on your machine.

2.2 Familiarize Yourself With the E80 Software

Read this section and make the requested modification to your code. We recommend reading this section with your E80 software open in a code editor (VScode, Sublime, Notepad++, etc., NOT MS Word) so that you can look for features of the code as we discuss them. We also think every member of your team should do this section even though you’ll only turn in one submission sheet.

The E80 robot code appears in three files: E80_Lab_07_dive.ino, E80_Lab_07_surface.ino and Default_Robot.ino. We intend for you to use Default_Robot.ino on your final project, but you should modify E80_Lab_07_dive.ino and E80_Lab_07_surface.ino this week. (The dive and surface programs are simplified versions of Default_Robot.ino, so they’re easier to learn from.) Look at E80_Lab_07_dive.ino in this section.

The E80 robot code all has the .ino suffix, which means it is encoded in Arduino files (Language tutorials and reference), and as such these files have two extremely important functions: setup() and loop(). The setup() function is run once when your robot powers on, and then the robot runs the loop() function over and over until power is removed.

The loop() function contains several if statements that dispatch to functions. We call these functions “services”, and each represents one thing that the robot is doing. The functions used in many services are defined in libraries, which are described below.

The E80 robot code uses #include statements at the top to include functionality that is written in C++ (Language tutorials and reference), and we call each such C++ file a “library”. Libraries can be found in our libraries/main/ folder, and they usually consist of a C++ file (e.g.: SensorIMU.cpp) and a header file that the C++ file #includes (e.g.: SensorIMU.h). However, some library files only provide constant values, and therefore only have header files (e.g.: TimingOffsets.h or Pinouts.h). Usually we invoke a library function to in at the top of the .ino file to create a special variable called an “object”, and then we ask that object to use its functions to perform a service.

We can put these ideas together with an example: at line 150 a service called adc.updateSample() is called, and this service samples and records the Voltage on all of the analog pins. The if statement at line 148 guarantees that this functions is called exactly once per LOOP_PERIOD, which is a constant you can look up (or change!) in TimingOffsets.h. The adc object is created asa global variable on line 40. The adc creation code (the init() function) and the adc.updateSample() function are defined in the ADCSampler.cpp library, which depends on ADCSampler.h.

To put our new understanding to the test, we are going to edit the serial message that the robot displays each cycle.

  1. Run the E80_Lab_07_dive.ino code and look at the printed display by opening the serial monitor.
  2. Modify SensorIMU.cpp so that the String returned by the printRollPitchHeading() function includes the X magnetometer value.
  3. Modify E80_Lab_07_dive.ino so that it displays the output of the printState() function of ButtonSampler.cpp on line 2 of the message instead of the printStates() function of ErrorFlagSampler.cpp.
  4. Run the E80_Lab_07_dive.ino code again and look at the new printed display to see your changes. Note that your changed libraries should be automatically recompiled when you recompile your .ino file. Press the user button on the E80 motherboard to see the ButtonSampler.cpp library at work.

Finally, look at TimingOffsets.h to figure out how often your main loop runs, which sets how quickly you sample data with the ADC, IMU, etc.

Tip

You generally should not include calls to Serial.println() in your code, even though it’s standard practice for Arduino, because it messes with our printer object. The printer takes great pains to keep all the text easy to read as the robot spits out updates, and random print statements can cause it to jitter around. Use printer.printMessage() for one time messages (see DepthControl.cpp for an example) and printer.printValue() to print recurring values (as seen in the loop() function of the E80 code).

Tip

Be sure to read the README in the libraries folder because it contains a lot of information about the features and adjustable knobs in each file. The adjustments described in the README file are sufficient to cover most standard student projects, so you might not even need to write code.

For instance, the README contains this important deep cut for the project: It is possible to increase the logging time of your robot by increasing the FILE_BLOCK_COUNT value in Logger.h. This can help you to log data during long deployments.

Exploring the code in this section highlights some of the interaction between the header (.h), library (.cpp), and Arduino (.ino) files. The header files are the interfaces, they hold information on variables that are used in other files and the functions of the library files of the same name. The library files are the implementations of the functions defined in the headers, and they use the variables held in the headers. The Arduino file sketch is the top level of the program, and it invokes library functions and header variables to implement the desired behavior of the robot.

2.3 Position Measurements via IMU

In this section we’ll use our board stack and our IMU to try to calculate position by integrating noisy acceleration data. As the lecture videos suggest, the noise is likely to make this position measurement fail. We’re hoping to see a concrete demonstration of that principle.

  1. Power your motherboard with a battery
  2. Run stock E80_Lab_07_dive.ino code without your modifications from last section, or run Default_Robot.ino if you don’t want to roll changes back. Make sure your IMU is producing sensible accelerations by turning it around in the air and, if need be, reviewing your lab 1 results.
  3. With the data logger running, move your motherboard along a linear path in the horizontal x-y plane, keeping the orientation fixed. In other words, create your own global Cartesian coordinate frame on a table, set the position of the board stack to be (0 m,0 m,0 m), set the orientation of the board stack to be (0°, 0°, 0°). Then, move the board stack through the following positions: (0 m, 0 m, 0 m), (0.5 m, 0 m, 0 m), (0 m, 0 m, 0 m). Don’t move too fast, and note that it may help to wait 5 seconds at the (0.5 m, 0 m, 0 m) coordinate.
  4. Using MATLAB, load the IMU data and calculate the position of the board stack as a function of time by integrating accelerations.
  5. Plot the x,y coordinates of the board stack and comment on whether the plotted path resembles the straight 0.5 meter path.
  6. Plot the y coordinates vs. time and include appropriate measures of uncertainty for integrated values.

3 Diving Activity

In this activity, you will design a P control loop that interacts with a pressure sensor to control the robot’s depth. You will need to modify E80_Lab_07_dive.ino and the E80 C++ libraries to do so.

3.1 Calibrate and Test a Pressure Sensor

In order to implement closed loop depth control with your AUV, it first needs to be able to sense its current depth, which we will do by interpreting a pressure sensor. You have two options to make your pressure sensor: build the pressure sensor from your project proposal on a breadboard, or use one of our pre-made pressure sensor adapter PCBs (picture, schematic, layout, KiCAD). We recommend building your own design to get a start on your project build, but feel free to use our PCB if you’re short on time. (Do note that our PCB uses a weird op-amp circuit that you’ll need to analyze on your own.)

In a deviation from previous practice, you will measure the operation of this sensor using your Teensy. By measuring your pressure directly in the Teensy, you can write code that interacts with your results in a direct way. We will take advantage of the serial data

  1. Attach your pressure sensor output to pin A0 on the Teensy.
  2. Add a line showing your current pressure measurement to your robot’s serial report. Edit updateState() in ZStateEstimator.cpp by uncommenting the two lines of code below the calculation of the z position.
  3. Run E80_Lab_07_dive.ino. The pressure sensor Voltage should now appear in the serial monitor, and you will use that display to calibrate your pressure sensor.
  4. Use a graduated cylinder and a meter stick with pressure tubing attached to make a calibration curve with at least seven points between depth and Teensy-reported Voltage.
  5. Modify depthCal_slope and depthCal_intercept in ZStateEstimator.h so your Teensy display reports the proper depth in meters.

3.2 Write a P Control Algorithm for Depth

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

  1. Assign the desired depth to a variable named depth_des. The desired depth can be found in the wayPoints array at the index currentWayPoint (i.e.: at wayPoints[currentWayPoint]).
  2. Assign the current depth to a variable depth. The current dept can be found in the state.z variable.
  3. Calculate the error, depth_error, as the difference between depth_des and depth. Pay attention to the order you subtract depth_des and depth, because a sign error here can make your P control push away from the desired waypoint.
  4. Set the vertical motor control effort, uV, to be the product of the control gain, Kp, and the error, depth_error. The default value of Kp is 0, so you need to change it in DepthControl.h. Kp=80 is usually pretty good, but tune it up or down if your motor is too weak or too strong.
  5. Bound the vertical control effort between -200 and 200. These are empirical values that work well for most situations. Depending on your motor, you may need to go beyond these limits. You can go up to a maximum of +/- 250.
Tip

You usually need to declare variables in C++, but all the variables in these instructions have already been declared in DepthControl.h (go look for yourself), so you don’t need to declare them here.

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.

3.3 Test the P Control Algorithm

Test your depth P control using the meter stick with tubing and the graduated cylinder.

  1. Attach the pressure tubing to your pressure sensor and run E80_Lab_07_dive.ino.
  2. While is running with the serial monitor open, insert the meter stick into the cylinder
  3. Keep an eye on the Depth_Des:, Depth:, and uV: values printed to the serial monitor.
    1. Make sure that the Depth is reporting a similar value to the current depth of the meter stick to confirm that your calibration works.
    2. Watch the control effort uV change as the difference between Depth and Depth_Des changes, making sure it behaves as expected for proportional control.

Improve your P control test by adding a motor to your robot.

  1. Get an E79 umbilical (found in the lab room) and attach it to your robot.
  2. Put the motor in the vertical position, and use a ziptie to secure the tube to the frame.
  3. Connect the tubing to your pressure sensor and the motor wires to the Motor C port on your motherboard.
  4. Run E80_Lab_07.ino while the serial monitor is open.
  5. Watch the motor and the reported control values:
    1. The motor should start spinning to get to the first waypoint at 0.5 meters. If the motor is spinning too fast for you to see what’s going on, you can reduce Kp.
    2. 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).
    3. Blow in the pressure tubing and confirm that the depth changes, and that the motor responds to the change in depth.

Finally, once everything is tested and working, deploy the robot in the tank.

  1. Bring your robot to the tank room, run E80_Lab_07_dive.ino, and put the robot into the tank. Watch out for splashes onto your computer or motherboard as you put the robot in.
  2. Let the robot go to the two depth waypoints at 0.5 m and 1 m while keeping an eye on the values and messages printed to the serial monitor. Be sure to log your data.
  3. Plot the logged uV, depth and depth_des as a function of time.
Warning

Be careful with the umbilical because 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.. For example, setting diveDelay = 3000 will tell the robot to wait at each depth waypoint for 3 seconds before continuing.

4 Surface Navigation Activity

In this activity, you will design a P control loop that interacts with a magnetometer and GPS sensor to control the robot’s angle. You will need to modify E80_Lab_07_surface.ino and the E80 C++ libraries to do so.

4.1 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° 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.

  1. Take your robot outside, far from buildings, run E80_Lab_07_surface.ino, and log your magnetometer data while your robot is held upright and you slowly (over ~10s) rotate 360 degrees while standing in place. Save the logged data from this exercise for later.
  2. 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.
  3. Update the mag_offsets and mag_comp variables in SensorIMU.h with the hard and soft iron coefficients you just measured.
  4. Run E80_Lab_07_surface.ino and turn in a circle outside again. Also save these magnetometer readings.
  5. Generate the following two plots:
    1. A scatter plot of mx vs. my where your calibrated and uncalibrated data are superimposed. Find appropriate units for the axes in Mag_Calibration.m.
    2. A plot of heading vs. time for both your calibrated and uncalibrated data.

4.2 GPS Integration

Integrate the Adafruit Ultimate GPS into the motherboard

  1. Solder a 9x1 female header pin row onto the motherboard at the position labelled GPS (see Lab 1 for details).
  2. Try to find an assembled GPS. If you can’t find one, then solder male header pins to the BOTTOM of the GPS breakout board. Be sure to get the orientation correct; Figure 1 shows the correct orientation.
  3. Use screws, nuts, and standoffs to help secure the GPS breakout board to the motherboard.
Figure 1: A motherboard with a GPS attached. The GPS here doesn’t have standoffs in the left two holes, but you should add those on your board.

Go outside the building and test your GPS Sensor integration by opening the serial monitor while E80_Lab_07_surface.ino is running. Make sure your battery is plugged into the motherboard because your GPS is powered off the battery (NOT off the USB connection that powers your Teensy). Make sure you achieve GPS lock and see >4 satellites.

4.3 Write Latitude and Longitude Software

Modify the library file XYStateEstimator.cpp so that it calculates the x,y position (in meters) of the board 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 updateState() in XYStateEstimator.cpp, and the calculated positions will be stored in the variables state.x and state.y. To do this, we will use a modified version of the Forward Equirectangular Projection, where we multiply use the radius of curvature R_earth to scale latitude and longitude angles in order to calculate arc lengths on the Earth’s surface. This conversion assumes that the earth is a sphere (which it is to within ±0.25%).

While we’re here, we have a little work to do to calculate yaw correctly. 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 in the updateState() function. Store the result in state.yaw.

  1. To calculate state.y, 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).
  2. To calculate state.x, note that it is a function of latitude, (e.g. state.x=R_earth*longitudeChange*cos(latitude at origin)).
  3. To calculate state.yaw, add an appropriate offset to state.heading and, if necessary, multiply some part of your expression by -1 to flip around a sign.

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.

Tip

You usually need to declare variables in C++, but all the variables in these instructions have already been declared in XYStateEstimator.h (go look for yourself), so you don’t need to declare them here.

Tip

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. Letting an angle be \(3\pi/2\) will crash a lot of your code unless you’re very careful.

4.4 Position Measurements via GPS

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.

  1. Run E80_Lab_07_surface.ino outside and ensure that you achieve GPS lock and are logging reasonable GPS data. 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.
  2. Walk the following path
    1. Start at the origin defined above
    2. Walk along the sidewalk East towards Shanahan.
    3. Walk South between Shanahan and Parsons.
    4. Cut West back to Parson’s building and end at the origin.
    5. The path is illustrated in Figure 2. 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.
  3. 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. Overlay these points on an image of campus using the imread and image Matlab commands in addition to normal plotting functions. The hold on command may help.
Figure 2: A path to walk to test the GPS.

4.5 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_surface.ino as the starter code for this portion. You will also interact with two new library files: SurfaceControl.cpp and SurfaceControl.h.

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

  1. Confirm that you can compile and upload the starter code to your Teensy before you make any changes.
  2. Calculate the desired yaw angle using the atan2 function. i.e. yaw_des = atan2(y_des - y, x_des - x). In this case, (x_des, y_des) is the desired position of the robot.
  3. Calculate the error, yaw_error, as the difference between yaw_des and yaw. (Which you just calculated based on state.heading above).
  4. Set the control effort, u, to be a product of the control gain, Kp, and the error, yaw_error.
  5. Set the right and left motor thruster values (uR and uL) to be the avgPower + u and avgPower - u respectively.
  6. Modify SurfaceControl.h to set avgPower to be 50 by default. This can be modified later if you want your robot to have faster or slower forward motion.
  7. Multiply uR and uL by Kr and Kl respectively, (e.g. uR = uR*Kr;) to give you a way to account for uneven motors. The default values for Kr and Kl are 1.0, but you can change them later if your motors don’t run at the same speed when given the same PWM signal.
  8. Bound the uR and uL control values to be between 0 and 127.
Tip

As above, the header file (in this case, SurfaceControl.h) takes care of variable declarations for you.

Test your P control by walking the campus while trying to obey the commands given by uR and uL.

  1. Make sure the path defined is 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 Figure 3).
  2. Go to the origin.
  3. Walk at a slow speed, turning in response to the u_R and u_L values printed to the Serial monitor. If the the values are uR=45 and uL = 55, then veer right to try to equalize uR and uL. Be sure to save your logged data.
  4. Plot the logged path of x, y points on top of an image of campus. Also plot the angle error and control effort u as a function of time.
Figure 3: A path to walk to test GPS-based P control of robot angle.

Finally, if you have time, 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 the 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 give off powerful magnetic fields that are likely to interfere with your experiment if you don’t handle them with care. No plots are required for this extra activity, but it can give you insight into how your robot will perform during the project deployments.