Getting the Code

One repository per team will be created for this project. Both teammates will have access to the repository and will be able to view it and make changes. Both teammates will share ownership of the code and receive credit for writing the code through the license file.

The invite link to accept the assignment on Github Classroom can be found on Slack.

The first teammate to accept will need to create a new team. The team must be named P1 UM Team # for UM students, and P1 Berea Team # for Berea students. Replace # with the team number assigned to you. The next teammate should join the team from the list of existing teams.

You will be cloning the repository on the robot's Raspberry Pi. See the robot tutorial for instructions on how to open a remote VSCode session connected to the robot. Once you are connected to the robot, in VSCode, open a terminal. This should be a terminal in the robot's Raspberry Pi. Then, clone the repository in the home directory:

git clone <ADDRESS>

Substitute the address to your repository. Use the SSH address (see instructions here). Open the folder of the repository you just cloned in VSCode using the instructions in the tutorial.

Submitting the Assignment

Your submission for this assignment should include your code and a video demonstrating your robot completing each of the three parts of this assignment. You should make one submission for your team. Teammates will be graded together.

Modify the LICENSE.txt file to include the names of all teammates. Make sure the change is committed to your repository.

• P1.0 (1 point): In the file LICENSE.txt, replace <COPYRIGHT HOLDER> with the names of all teammates, separated by commas. Replace <YEAR> with the current year.

Submitting the code: Tag the verion of the code you wish to submit, just like you did for Project 0.

Submitting the video: Modify the file README.md to include a link to a video demonstrating all three parts of the project. You can submit one video for all the parts, or one video for each part. Commit the changes to README.md.

• P1.1 (1 point): Add a link to a video demonstration of Parts 1, 2 and 3 to the README.md file in your repository.

Note: Consider uploading your video to YouTube. You can make it public or unlisted if you prefer. Alternatively, you can upload your video to Google Drive (you have unlimited storage through your UM account). If you do this, make sure the link is accessible to the instructors.

In Project 0, we compiled our C++ code using the command g++. This called the C++ compiler and generated an executable that we could run. This time, we have a bit more code to deal with. The wall follower code will have dependencies on external libraries to drive the robot and read data from the Lidar. We could still use g++ for this, but the command would be much longer and more complicated. Instead, we'll use a tool called CMake. CMake finds all the code and external libraries we need and generates instructions to build the executable. To build the code, in a terminal, type:

cd ~/wall-follower/build
cmake ..
make

Remember that the code should be cloned and compiled on the Raspberry Pi. This will fail on your computer! Let's look at these instructions line by line:

1. cd ~/wall-follower/build: Changes the directory to the build folder, where all the files CMake needs will be generated. The executables will also be in this folder. Keeping all the files related to compilation in build keeps our code organized.
2. cmake ..: Calls CMake, and tells it to use the instructions in the file CMakeLists.txt which is located in wall-follower to generate instructions to compile the code. Generally, cmake .. only needs to be called once, not every time you change your code.
3. make: Compiles the code, using the instructions generated by CMake (called Makefiles). You need to rerun make every time you change the code.

Repository structure

The repository includes the following dirctories and files:

• build: Build files and executables should be generated here. All commands to compile code should be executed inside this directory. The contents are not pushed to GitHub.
• include: Header files are stored in this directory. These allow us to include code written in separate files in our code.
• lib: Precompiled library files.
• omnibot_msgs: These are message types which are needed for communicating with the Beaglebone.
• src: Source code and executables are stored here. All your changes should be in this folder.
• CMakeLists.txt: Instructions for CMake to use to find dependencies and compile executables.

Provided functions & structs

To use provided functions, all you need to do is include the correct header file. The needed header files should already be included in the templates. The following functions are provided:

• void drive(float vx, float vy, float wz): Send a velocity command to the motors. The velocity command has x and y components in meters per second (vx and vy) and an angular component in radians per second (wz).
  Warning: Once drive is called, the robot will continue to drive at the given speed until you tell it to stop! Use drive(0, 0, 0) to stop the robot.
• LidarScan readLidarScan(RPlidarDriver* drv): Read the most recent Lidar scan and return it.
• void sleepFor(double secs): Sleep for a given number of seconds.
• double normalizeAngle(double angle): Normalize an angle in the range [-pi, pi]. This function returns the normalized angle.
• Vector3D crossProduct(Vector3D& v1, Vector3D& v2): Preform the cross product between the two vectors provided, and return the resulting vector.

The following structs are provided:

• Vector3D: For representing 3D vectors. This is the type that the cross product function requires.

  struct Vector3D
  {
  float x, y, z;
  };

  The values will default to zero if uninitialized.
• LaserScan: For storing Lidar data. This is the type returned by the function that reads the Lidar scan.

  struct LidarScan
  {
  bool good;  // Whether the scan is valid.
  
  int utime;       // Clock time of the scan, in microseconds.
  int num_ranges;  // Number of rays in the scan.
  
  std::vector ranges;       // Length of each ray in the scan, in meters.
  std::vector thetas;       // Angle of each ray in the scan, in radians.
  std::vector intensities;  // Intensity of each ray in the scan.
  std::vector times;        // Time each ray was emitted.
  };

  The vectors ranges, thetas, intensities, and times will all be of length num_ranges. The rays are listed in the same order, in the counterclockwise (positive) direction. For example, for a LaserScan instance named scan, the third ray, at index 2, will have length scan.ranges[2], angle scan.thetas[2], intensity scan.intensities[2], and time scan.times[2].
  Warning: Some rays in the scan never return (for example, if there are no obstacles, or the ray bounces off a material and goes in a different direction). If the ray does not return, its range and intensity will be zero. Make sure you check for rays with zero intensity and ignore them.

This project is separated into three parts. Most of the time you spend on this project should be spent on part 3.

1. Part 1: Intended to help you get used to programming the robot and sending velocity commands to drive it.
2. Part 2: Intended as an introduction to using the Lidar.
3. Part 3: The wall follower.

A number of functions have been provided for you along with the template code. Make sure you read the code overview to learn how to use them.

Part 1: Drive Square

We will start by driving the robot in a square without any sensor feedback. For this part, you will write your code in the file wall-follower/src/drive_square.cpp. In the main function, write code to make the robot drive in a 1 meter by 1 meter square three times.

• P1.1.1 (2 points): In the file drive_square.cpp, write code to drive the robot in a 1m by 1m square three times.

• Hint: You can use the drive() function together with the sleepFor() function to make the robot drive in one direction for a certain amount of time. To drive in a square, you might drive forward for one second, left for one second, backward for one second, then right for one second.
• Hint: Start by writing code to drive the robot in a square once. Then, you can add a for loop to repeat the pattern however many times you would like.
• Hint: It's good practice to create variables in the main function to store constants. For example, you might want to define variables to store the velocity you are driving at, the number of seconds to drive for, and the number of times to repeat the square.

Part 2: Safe Drive

Now we know how to drive the robot, but we aren't using any of the sensors. If there is an obstacle, our drive square code will just bump into it. In this part, we will use the Lidar to check whether there is an obstacle close by. If so, we will stop driving.

First, we need to write code to find the distance to the nearest obstacle. The ranges vector in the LaserScan struct contains the length, or range, of each ray in the scan in meters. The distance to the nearest obstacle is the range of the shortest ray in the scan.

Complete the provided function, findMinDist(), in wall-follower/src/common/utils.cpp so that it finds the index of the shortest ray. For example, if the smallest range in ranges is the tenth element at index 9, the function should return 9.

• P1.2.1 (2 points): In the file common/utils.cpp, complete the function findMinDist() so that it finds the minimum length ray in the given scan. Return the index of this ray.

• Hint: Remember to ignore rays with zero intensity when finding the minimum range value. Rays with zero intensity have length zero by default. If you forget to check the intensity, the minimum range will always be zero.

For this part, the robot should drive forward. If the robot gets within 35 cm from an obstacle, it should stop. If the obstacle moves away, it should continue to drive. If the robot stops when it gets too close to a wall, you should be able to pick it up and move it somewhere else, and it should keep moving forward again. The code should continue forever, until you quit with Ctrl-C. Write your code to accomplish this in the file wall-follower/src/drive_safe.cpp.

• P1.2.2 (1 point): In the file drive_safe.cpp, write code to make the robot drive forward and stop if an obstacle gets within 35 cm. You should use the Lidar scan and your function, findMinDist(), to find the distance to the nearest obstacle.

• Hint: You should create variables to store the velocity and obstacle threshold. You might have to tune these values to make it work well.
• Hint: This code stops the robot:

  drive(0, 0, 0);

Part 3: Wall Following (Bang-Bang Control)

Now that we have gotten used to controlling the robot and reading Lidar data, we're ready to code a wall follower! The wall follower should run forever, until the program is stopped with Ctrl-C. At each iteration, the wall follower should do the following:

1. Find the distance to the nearest wall and the angle where the wall is located (use findMinDist() for this part).
2. Use the cross product to find a vector pointing parallel to the wall, in the direction the robot should drive.
3. Apply a correction to the vector using bang-bang control to move closer to or farther from the wall, depending on the current distance to the wall.
4. Convert the vector to a velocity vector and send a velocity command to the robot.

You will need to write the cross product function first, in the provided function in wall-follower/src/common/utils.cpp. The wall follower code should go in wall-follower/src/wall_follower.cpp.

• P1.3.1 (2 points): In the file common/utils.cpp, complete the function crossProduct() to find the cross product between two vectors.
• P1.3.2 (6 points): In the file wall_follower.cpp, write a program that follows a wall using the procedure outlined above. You will need to use your functions findMinDist() and crossProduct().

• Hint: You should create variables to store the setpoint (how far your robot should stay from the wall) and the magnitude of the correction you will apply. You might also want to declare an acceptable margin where you won't apply a correction, and the velocity of the robot. These values will need to be tuned to make your wall follower work well!

Advanced Extension: Wall Following (P-Control)

We can make a smoother controller using proportional control, or P-control, instead of bang-bang control. Instead of applying a correction of a fixed size, we can apply a correction proportional to the magnitude of the error between the desired setpoint and the current distance to the wall.

P-control can be implemented for optional advanced extension credit.

• Advanced Extension P1.3.i (1 extension point): Implement a wall follower using P-control.

• Hint: Instead of using a fixed correction size, the magnitude of the correction will be kp * error. You will need to declare and tune variable kp, often called the proportional gain.

• P1.0 (1 point): In the file LICENSE.txt, replace <COPYRIGHT HOLDER> with the names of all teammates, separated by commas. Replace <YEAR> with the current year.
• P1.1.1 (2 points): In the file drive_square.cpp, write code to drive the robot in a square three times.
• P1.2.1 (2 points): In the file common/utils.cpp, complete the function findMinDist() so that it finds the minimum length ray in the given scan. Return the index of this ray.
• P1.2.2 (1 point): In the file drive_safe.cpp, write code to make the robot drive forward and stop if an obstacle gets too close. You should use the Lidar scan and your function, findMinDist(), to find the distance to the nearest obstacle.
• P1.3.1 (2 points): In the file common/utils.cpp, complete the function crossProduct() to find the cross product between two vectors.
• P1.3.2 (6 points): In the file wall_follower.cpp, write a program that follows a wall using the procedure outlined above. You will need to use your functions findMinDist() and crossProduct().
• P1.1 (1 point): Add a link to a video demonstration of Parts 1, 2 and 3 to the README.md file in your repository.

• Advanced Extension P1.3.i (1 extension point): Implement a wall follower using P-control.

Lectures and labs

• TuTh 3-4:30 PM @ FRB 1060
• F 10:30 AM-12:30 PM @ FRB 1060
  F 12:30-2:30 PM @ FRB 1060
  F 2:30-4:30 PM @ FRB 1060