Grade: A+

My full website for can be found at this link. Below are selected portions.

The collective objective of the labs is to collecte IMU and Time of Flight distance sensor data for localization. The below robot was assembled and used throughout the labs.

Objectives

Lab 1
• Bluetooth Setup
Lab 2
• Reading IMU Data
Lab 3
• Time of Flight (ToF) Distance Sensors
Lab 4
• Motor Driver Assembly
Lab 5
• Linear PID Control
Lab 6
• Orientation PID Control
Lab 7
• Kalman Filter Using Collected ToF Distance Sensor Data
Lab 8
• Implement the Kalman Filter on the Robot
Lab 9
• Mapping
• Orientation Control
• ToF Distance Sensor Readings
Lab 10
• Localization Simulation
• Write functions to compute the control, odometry motion, prediction step, sensor model, and update step.
Lab 11
• Localization on Robot
• Compare the Belief and Ground Truth
• Orientation Control
• ToF Distance Sensor Readings

Lab 11: Localization On The Robot

The objective of the lab is to implement the Bayes Filter on the real robot. Due to noise when the robot is turning, only the update step of the filter will be be used during a 360 degree scan.

Lab Tasks

Simulation Localization

The first lab task was to run the provided Bayes Filter simulation. The lab11_sim.ipynb file was downloaded and a screenshot of the final result is below as well as a link to the data from the simulation. The red line is odometry, the green line is the ground truth, and the blue line is the belief. The map is a three dimensional grid (x, y, theta). The dimension of x is 12, the dimension of y is 9, and the dimension of theta is 18.

Run Robot Localization

The results of rotating the robot at the four marked positions are below. The four marked positions include: (-3ft, -2ft, 0deg), (0ft, 3ft, 0deg), (5ft, -3ft, 0deg), and (5ft, 3ft, 0deg). The robot was run around 4-5 times at each point. Two runs for each point are shown below. The ground truth is plotted as the green dot and is the location where the robot rotated. The belief of the robots position is plotted as the blue dot. The ToF sensor used to collect distance readings was located on the front of the robot, between the left and right sides of the robot. When the robot was at zero degrees the ToF sensor was pointed towards the rightside wall. The robot turned counterclockwise in the same manner as in Lab 9. The Jupyter Notebook code to plot each Ground Truth value is below:

Bottom Left Point (-3ft, -2ft, 0deg):

Graphs for the Belief (blue) and the Ground Truth (green):

For the bottom left point, the Belief was at the same point as the Ground Truth. As can be seen in the below polar plot, the robot sensed the surrounding walls very well. I reran the rotation several (5) times and this point had the most rotations result in the Belief being the same as the Ground Truth. This is a result of the several walls surrounding the robot when rotating. The next run shows one of the runs where the Belief was different then the Ground Truth.

---

Polar Plot and recorded data below:

Top Middle Point (0ft, 3ft, 0deg):

Graphs for the Belief (blue) and the Ground Truth (green):

For the top middle point, the Belief was at the same point as the Ground Truth. I was surprised to achieve this result as there are not a lot of nearby walls to use for localization and I suspected that this would be the hardest point to localize. As can be seen in the below polar plot, the robot was able to sense the surrounding walls and obstacles.

---

Polar Plot and recorded data below:

Bottom Right Point (5ft, -3ft, 0deg):

Graphs for the Belief (blue) and the Ground Truth (green):

For the bottom right point, the Belief was at the same point as the Ground Truth. As can be seen in the below polar plot, the robot rotated well and was able to sense the surrounding walls and obstacles.

---

Polar Plot and recorded data below:

Lab 10: Grid Localization Using Bayes Filter

The objective of the lab is to use the Bayes Filter to implement grid localization.

Before starting the lab, I familiarized myself with how the Bayes Filter worked and how to implement the Bayes filter for the robot in simulation. Localization and of the robot determines where the robot is in the enviroment. There is a predition and update step of the Bayes filter. The prediction step uses the control input and accounts for the noise from the actuator to predict the new location of the robot. After the prediction step, the robot rotates 360 degrees gathering distance measurement data. The update step uses the gathered distance measurements to determine the real location of the robot. The Bayes Filter from Lecture 16 is below:

The dimensions of each grid cell in the x, y, and theta axes are 0.3048 meters, 0.3048 meters, and 20 degrees. The grid is (12,9,18) in three dimensions, totalling 1944 cells. The discritized grid map is:

(-1.6764,+1.9812) meters or (-5.5,6.5) feet in the x-axis

(-1.3716,+1.3716) meters or (-4.5,4.5) feet in the y-axis

(-180,+180) degrees in the theta-axis

Lab Tasks

Implementation

Five functions were written to run the Bayes Filter as outlined below.

Compute Control

The control information from the robot is used to find the first rotation, translation, and second rotation. The inputs include the current position (cur_pose) and the previous position (prev_pose). The return values include the first rotation (delta_rot_1), the translation (delta_trans), and the second rotation (delta_rot_2). The Lecture 17 slide with the equations to find the odometry model parameters is below:

The above equations were modeled in Jupyter Notebook. The code is found below. First the differences in the y and x positions are found (y_comp and x_comp). Then the normalized rotations are calculated as well as the translation.

Odometry Motion Model

The odometry motion model finds the probability that the robot performed the motion that was calculated in the previous step. The inputs include the current position (cur_pose), previous position (prev_pose), and odometry control values (u) for rotation 1, translation, and rotation 2. The return value is the probablility ( p(x'|x,u) ) that the robot achieved the current position (x'), knowing the previous position (x) and odometry control values (u). A Gaussian distribution is used. Each motion is assumed to be an independent event. The probability of each event is then multiplied together to find the probability of the entire motion. The Jupyter Notebook code is found below.

Prediction Step

The prediction step of the Bayes Filter as implemented in Jupyter Notebook uses six for loops to calculate the probability that the robot moved to the next grid cell. The inputs include the current odometry position (cur_odom) and the previous odometry position (prev_odom). The if statement in the pediction step code is used to speed up the filter. If the belief is less than 0.0001 than the three remaining for loops will not be entered as the robot is unlikely to be in the remaining grid cells. If the belief is greater than or equal to 0.0001 the probability will be calculated. The code for calculating the probability is below.

Sensor Model

The sensor model function finds the probability of the sensor measurements, modelled as a Gaussian distribution. The length of the array is 18 as the robot measures 18 different distance reading values. The input includes the 1D observation array (obs) for the robot position. The function returns a 1D array of the same length including the likelihood of each event (prob_array). The Jupyter Notebook code is below.

Update Step

The update step calculates the belief of the robot for each grid cell. The probability ( p(z|x) ) is multiplied by the predicted belief (loc.bel_bar) to find the belief (loc.bel). The belief is then normalized. The code can be found below.

Simulation Results

Two runs of the Bayes Filter simulation are found below. The green line is the ground truth, the blue line is the belief, and the red line is the odometry measurements. The lighter grid cells represent a higher belief. For both runs the ground truth and the belief are close together. Both runs were 15 iterations.

Run 1 Results

Run 2 Results

Lab 9: Mapping

The objective of the lab is to use yaw angle measurements from the IMU sensor and distance readings from the ToF sensor to map a room. The map will be used during localization and navigation tasks. Map quality depends on how many readings are obtained and the separation between each reading during each rotation.

Lab Tasks

Orientation Control

Orientation Control was chosen as the PID controller for the lab. The PID controller results in the robot performing on-axis turns in 10 degree increments. The ToF sensor used to collect distance readings is located on the front of the robot between the left and right motor sides.

Code For Orientation Control

First the distance is found using the readings from the ToF sensors, then the yaw angle is found using the IMU sensor. The PID controller value is used in the RIGHT_MOTOR and LEFT_MOTOR functions to determine the motor input value. If the error between the current yaw angle and the setAngle (10 degrees) is less than -0.5 degrees or greater than 0.5 degrees then the RIGHT_MOTOR and LEFT_MOTOR functions will run. The "stop" variable is set to be 360 divided by the "setAngle" variable, which is equal to 10. The results in stop = 36 increments. If variable "end" is less than "stop" the robot will stop for one second. I stopped the robot for one second to ensure that the ToF sensor is pointed towards a fixed point in space. The total yaw angle is recorded by adding the current total to the current yaw value (yaw_arr). The yaw value is then set to zero for the next iteration of the while loop. Resetting the yaw value for the IMU sensor reduces gyroscope drift. If the "end" value is greater than or equal to the "stop" value the robot will stop and no new data will be recorded into the yaw_arr array.

The Jupyter Notebook command and Arduino case to start the PID orientation controller are below. From testing I found that using a Kp value of 1, Ki value of 0 and Kd value of 0 worked to move the robot in 10 degree increments. Each time the PID orientation controller starts, yaw[0], yaw_arr[0], and the total are set to zero. In addition the stop variable is found. Graphs showing the changing yaw angles to confirm the PID controller works are found in the next section.

Video and PID Control Graphs

Two videos of the robot turning are below. The robot roughly turns on axis. In the second video, the robot overshoots at some increments but is able to correct itself to the correct angle. The robot stays inside of one floor tile for the duration of the turn.

Turn 1:

Turn 2:

I plotted the values of yaw for each 10 degree increment. As can be seen in the below graph the PID controller works as expected. After yaw increases to 10 degrees, the value of yaw is reset to zero and then again increases to 10 degrees for the next increment. The slight variation in the final yaw value is due to the -0.5 to 0.5 degree tolerance.

The total angle values were found by adding each final yaw value (between 9.5 and 10.5 degrees) to a summation of all previous yaw values. The values and graph for an example turn are found below.

In the above graph as the turn progresses the angles remain accurate as the summation calculation is correct to the yaw reading of the robot, but the angles slightly vary from the ideal 360 degree turn values. To address this I also performed half turns from 0 to 180 degrees, and 180 to 360 degrees. The angle values and corresponding graphs can be found below:

---

The half turns in the above graphs are more in line with the ideal angles. When testing the robot in the lab, I collected angle data for full turns and half turns.

Read Out Distances

Execute Turn At Each Marked Position

The robot was placed at each of the following points: (-3,-2), (0,0), (0,3), (5,3), and (5,-3). The image below shows each of the points in the lab.

The distance was measured using the front ToF sensor between the left and right motor sides. The data collected was sent over Bluetooth to Jupyter Notebook to be plotted. The robot was started in the same orietation, facing the right side wall of the lab, for each full turn and each half turn starting at zero degrees. For half turns starting at 180 degrees, the robot started facing the left side wall. The range of potential angle values for each ideal 10 degree rotation is 9.5 degrees to 10.5 degrees. There is a -/+ 0.5 degree range for the robot to stop and collect a distance measurement. Due to the range of degree values, it can not be assumed that the robot will stop at exactly 10 degrees every time it rotates. To address this issue, I did not assume that the robot stopped at exactly 10 degrees for every rotation and instead recorded a "total" variable value that added the angle that the robot stopped at to the previously recorded stopping angle. This allowed me to plot the distance measurements with the correct recorded angles.

Polar And Global Frame Plots

This section details the second attempt at collecting data at each point. The data collected for each point in the lab is plotted in the below graphs. I readjusted the ToF sensor to point more upwards before collecting the following data. This adjustement removed noise from inside the map region. The data collected for each point in the lab is plotted in the below graphs.

Bottom Left: Point (-3,-2)

Middle: Point (0,0)

Top Middle: Point (0,3)

Top Right: Point (5,3)

Bottom Right: Point (5,-3)

Merge And Plot Readings

I plotted the distances in the global frame using a transformation matrix to convert the frame of the robot used to collect the sensor measurements into the global frame.

Below are the distance readings in the global frame.

Convert To Line Based Map

After plotting the above graph I plotted lines representing the walls of the map. The plot showing the line based map is below as well as the code used to plot each line.

Lab 8 Extra Credit: Implement Kalman Filter On Robot

I implemented the Kalman Filter on the robot for extra credit. To complete this task I needed to install the Basic Linear Algebra Library on Arduino IDE. To implement the Kalman Filter I first created two new cases in Arduino IDE: KF_DATA and SEND_KF_DATA. These cases are similar to the cases used to start/stop the PID controller and send PID controller data, but I added array's for Kalman Filter time (KF_Time) and the Kalman filter distance (mu_Store). I added a delay in KF_CASE before running the while loop. When I was testing the filter I was repeatedly starting with distances of 0mm. The delay after the ToF distance sensor starts ranging reduces the amount of 0mm distance readings when starting the filter.

After creating the cases for the Kalman Filter, I then started working on writing a Kalman Filter function in Arduino IDE. The final initialization of the matrices is below:

Step 1: Kalman Filter Function

The first function that I wrote for the lab was the function Kalman_Filter() that is called from inside the larger while loop. To perform this task, I referenced the Jupyter Notebook code I wrote for Lab 7 and modified the code for the Arduino IDE syntax. The function can be seen below:

The Ad (discritized A) and Bd (discritized B) matrices are updated each time the Kalman Filter is called to use the most recent dt value. I printed the dt (time difference) values to the Serial Monitor and observed that the dt values slightly varied each time the while loop ran. The variable dt is how fast the while loop runs and ranged from around 10ms to 25ms. After finding Ad and Bd, mu_p and sigma_p are calculated. The if statement of the filter is the update step and runs when there is a new TOF sensor reading (newTOF == 1). The else statement is the prediction step and assigns the mu_p value to mu and the sigma_p value to sigma. During both steps the first value for mu is stored in the array mu_Store.

Step 2: Kalman Filter Update Step On Robot

Using ToF Readings For PID Loop

I started working on the calling the Kalman Filter from inside the while loop by modifying the script for my PID controller. After finding the ToF sensor readings and running the PID controller within the larger while loop, I added an if statement inside the loop to call the Kalman Filter to find the update values. The if statement runs if dStart == 1 and if the currect distance value is not equal to the previous distance value. The variable dStart is set to 1 if the new ToF sensor distance is not the same value as the previous sensor distance. If the current distance reading is the same as the previous distance reading dStart is set to 0. The if statement to call the update of the Kalman Filter is below:

The PID controller was run using values of Kp = 0.05, Ki = 0.0000008, and Kd = 0.5. A test run showing the plotted ToF data and the Kalman Filter updates is shown below. It can be seen that the Kalman Filter updates align with each new reading of the ToF sensor.

The video below shows the Serial Monitor outputs for the ToF distance readings and the update values:

After successfully implemeting the update step of the Kalman Filter, I started working on the prediction step.

Step 3: Kalman Filter Update and Prediction Steps On Robot

Using ToF Readings and Predicted Distances For PID Loop

I started the next step by removing the PID control from the larger while loop and creating a separate function called PID_Function(). A separate function for PID control allowed for me to more easily switch the order to performing the Kalman Filter before the PID controller. Previously the PID controller used the ToF sensor measurements every time the loop ran. In the new iteration of the code, if the update step of the filter runs, the distance measurement used will be the ToF sensor reading. If the prediction step runs the most current value of mu will be used in the PID controller. The if and else statements for the update and prediction steps are shown below. The variable dis is the distance measurement used in the PID controller. The u value for the Kalman Filter was calculated the same as in Jupyter Notebook using the PID control values.

The PID_Function is below:

Throughout testing, I determined that a good value to multuiply the B matrix with is 8. The Kalman Filter updates align with each new reading of the ToF sensor and the prediction values follow the trend of the data. Below are two sets of graphs showing data that was recorded after the B matrix was multiplied by 8. Needing to multiply the B matrix by 8 indicates that the value found for m during the Jupyter Lab simulation does not work as well when implementing the Kalman Filter on the robot.

---

The video below shows the Serial Monitor outputs for the ToF distance readings and the update values:

Lab 7: Kalman Filter

The objective of the lab is to gain experience implementing Kalman Filters. Labs 5-8 detail PID, sensor fusion, and stunts. This lab is focused on using a Kalman Filter to predict ToF sensor distance readings of the robot. The code from Lab 5: Linear PID Control will be used to gather data, having the robot speed towards a wall and stop 1ft in front of the wall. A Kalman Filter will be implemented in Jupyter Notebook using the collected data.

Kalman Filter Explaination

A Kalman Filter uses a state space model to predict the location (distance from the wall) of the robot. The location of the robot is predicted and then updated using the ToF sensor reading. The prediction and update steps from Lecture 13 are below:

The state space model is used to find the A, B, and C matrices. The state space model for the robot from Lecture 13 is below. The model uses drag (d) and momentum (m) to form the A and B matrices. The two states are the location of the robot and the robots velocity.

The first part of the lab is estimating the drag and momentum of the robot using steady state velocity. One difference from the lecture slides is that for this lab the C matrix will be C = [1 0]

Lab Tasks

Estimate Drag and Momentum

The drag and momentum for the A and B matrices are estimated using a step response. The robot drives towards a wall at a constant input motor speed. The ToF sensor distance measurements and motor input values are recorded. Velocity is calculated and plotted along with the 90% rise time. The data is used to determine the drag and momentum and build the state space model of the system.

The equations for drag and momentum are below:

The step response u(t) was chosen to be a PWM value of 165. The step response is 64.71% of the maximum u PWM value of 255. I choose a PWM value of 165 as it was the fastest the robot could move while still moving straight for a distance of around 6.5ft. I collected time, ToF distance readings, and used these readings to calculated the velocity. To ensure that the step time was long enough for the robot to reach steady state, I ran the test multiple times.

From the above graph it can be seen that the steady state velocity is just over -2000mm/s, or more specifically -2031.91mm/s. Using a 90% rise time a line on the above graph was plotted showing 90% of the settling value. This value is (-2031.91mm/s * 0.9) = -1828.72mm/s. The 90% rise time can be determined from the graph as 1.32s. The steady state velocity is used to find the drag. The drag and rise time are used to find the momentum. The calculations to find drag (d) and momentum (m) are found below:

Calculated: d = 0.0004921 and m = 0.0002821

Drag is inversly proportional to the speed of the robot and affects how much the system reacts to changes in u. Momentum relates to how much effort is needed to change the velocity. Higher momentum signifies that more effort needs to be exerted to change the velocity, whereas lower momentum signifies that less effort needs to be exerted to change the velocity.

The collected data array's from all trials were stored in Jupyter Notebook in case the data needed to be used in the future. The code to find the steady state velocity can be found here: Max Speed Code

Initialize Kalman Filter: Python

Compute the A and B Matrices

The A and B matrices were computed using the calculated values for d and m. From the state space equation the A and B matrices are:

The A and B matrices were calculated as seen below:

The discritized A and B matrices are calculated using the following method from Lecture 13:

Initially, the sampling time for the discritized Ad and Bd matrices was calculated by subtracting the first time stamp from the second time stamp. After looking closely at the data, the sampling time between each time step was slightly different. I changed the calculation for Delta_T to be an average of all of the sampling time differences. The Delta_T was recalculated for each new dataset that I graphed. Below is an example calculation of the discritized Ad and Bd matrices, using the found sampling time of 0.008s for the PID loop in Lab 5. This dt is how fast the PID control loop is running.

The provided code in the lab report as well as my method of finding Delta_T was used to implement the calculation in Python as seen below:

There are two different time array's. One method of finding Delta_T uses the time array of the PID controller (array: time). This method was for the 5000 level task, resulting in a faster frequecy than the frequency found for the ToF readings (array: timeF). In between ToF readings the prediction step of the Kalman Filter estimates the state of the robot.

The previously used code that does not calculate the average sampling time to find Ad and Bd can be found here: Previous Ad Bd Code

Identify the C Matrix and Initialize the State Vector

The C matrix for the state space is C = [1 0]. C is an m x n matrix where m is the number of states measured (1, ToF sensor distance) and n is the dimensions of the state space (2, ToF distance and velocity). The initial state is the first distance measurement and a velocity of 0 as the robot is not yet moving. The positive distance from the wall was measured.

Below is the Python code to establish the C matrix and initialize the state vector for the ToF distance measurements.

Specify Process Noise and Covariance Matrices

The process noise and the sensor noise covariance matrices need to be specified for the Kalman Filter to work well. Sigma_3 was initialized as 20. This value was determined from slide 35 in Lecture 13 as well as the ToF sensor datasheet. Sigma_Z represents the mistrust in the sensor readings. The ToF sensor has a ranging error of +/- 20mm in ambient and dark lighting.

Sigma_1 and sigma_2 were initialized using the calculation found on slide 35 of Lecture 13. Sigma_U represents the mistrust in the Kalman Filter model. Sigma_1 is the relationship between the ToF distance reading and the model predictions. Sigma_2 is the relationship between the velocity and the model predictions.

In Jupyter Notebook, I calculated the sigma values for each dataset as shown in the below code. The initiallized sigma had a value of 20 for sigma_1 and 5 for sigma_2 as the robot is not moving initially.

The sigma values were altered as needed for each dataset.

5000 Level Task: Faster Frequency Kalman Filter -- And -- Implement and Test Kalman Filter: Python

The below section includes the 5000 level task to use a faster frequency and use the Kalman Filter prediction step to estimate the state of the car, as well as the same data graphed without the prediction step for comparison. To write the below code I went to office hours to ask questions and looked at all of the lab reports for the TAs.

The below code for the Kalman Filter function was provided in the lab instructions and implemented in Jupyter Notebook.

The first two lines finding mu_p and sigma_p are the prediction step of the Kalman Filter. If there is no new ToF sensor reading, then the returned mu and sigma will be equal to what is found during the prediction step. Within the if statement there is the update step of the Kalman Filter that runs every time there is a new ToF reading. The current state is mu, the uncertainty is sigma, the motor input values are u, and the distance readings are y.

To loop through each time step the below code was used.

The while loop determines how many times the Kalman Filter runs. How many times the while loop runs depends on the the second to last time step for a ToF sensor reading and the value of Delta_T. If the value of the ToF time array is less than or equal to the value of the index, i, then the value of newTOF will be set to 1 and the update step of the Kalman Filter will run. If the value of the ToF time array is not less than or equal to the value of the index, i, then the value of newTOF will be set to 0 and only the prediction step of the Kalman Filter will run. The index i is updated by adding Delta_T each time the while loop runs and the value of i is appended to the array KF_time to plot the Kalman Filter. After i is updated, the Kalman Filter function is called and then the output distance (x) is appended to the KF_distance array to later be plotted. The u input was the negative value of 40 subtracted from the motor array storing PWM values, divided by 215 (the upper bound of 255 minus the lower limit of 40). The lower limit PWM value is 40. In this model if the motor input value is 40 then the value for u is zero (-(40-40)/215).

Filter Parameters

Throughout the lab report I discuss the parameters used to modify the Kalman Filter and in the section below I discuss how the parameters are modified to better fit the Kalman Filter to the ToF distance data. To summarize, the parameters include the sampling time (Delta_T), drag (d), momentum (m), and sigma values including Sigma_1, Sigma_2, and Sigma_3. The sigma values are used to find the covariance matrices: Sigma_U and Sigma_Z. Sigma_U represents the confidence in the model (distance and velocity) and Sigma_Z represents the confidence in the sensor measurement. Drag is inversely proportional to speed and momentum is related to how much effort is needed to change the speed of the robot. In extension, the values for drag and momentum affect the A and B matrices. Delta_T and the A and B matrices affect the values of the Ad and Bd matrices.

Kp = 0.05, Ki = 0.000008, Kd = 8

Initially running the Kalman Filter results in the above graphs. The calculated Delta_T for the PID controller is 0.0058 seconds, a frequency of 172.41Hz. The Kalman Filter is running at a faster frequency than the ToF sensors. The maximum PWM value is 115. Sigma_1 and Sigma_2 are 131.54 and Sigma_3 is 20. Overall, the model works well, but the predictions could be more inline with the ToF distances. In the next step I modify the parameters to adjust the prediction values.

---

I adjusted the parameters for Sigma_1, Sigma_2, and Sigma_3. Decreasing Sigma_1 to 10 (model uncertainty for location) and Sigma_2 to 120 (model uncertainty for velocity) indicates that the model is more accurate than what was initially predicted. Sigma_3 (sensor uncertainty) was changed to 10 as the sensor readings were reliable. All other parameters (d, m, and Delta_T) were kept the same as the previous step. As can be seen from the graphs the Kalman Filter more closely follows the trend of the distance measurements.

In the above graph I kept all previously found parameters the same but changed the value of the increment "i" in the while loop in the same manner as I did for Graph 1. The line changed from "i = i + Delta_T" to "i = i + 2*Delta_T". This change results in less Kalman Filter predictions to be plotted and the same offset behaviour as seen in Graph 1.

Lab 6: Orientation PID Control

The objective of the lab to to gain experience working with PID controllers for orientation. Labs 5-8 detail PID, sensor fusion, and stunts. This lab is focused on using a PID controller for orientation control of the robot. The IMU will be used to control the yaw of the robot.

PID Control Explanation

For this lab a PID controller will be used for orientation control of the robot. The below equation from the course Lecture 7 is used to compute the value for the PID controller:

This equation uses the error e(t) (difference between the final desired value and the current value) in proportional, integral, and derivative control to calculate the new PID input value u(t). For this lab the error is the difference between the desired angle (yaw) and the robots current angle (yaw). The calculated PID input value is related to the robots angular speed. Proportional control multiplies a constant Kp by the error. Integral control multiplies a constant Ki by by the integral of the error over time. Derivative control multiplies a constant Kd by the rate of change of the error.

Lab Tasks

PID Input Signal

Integrate Gyroscope To Estimate Orientation

The gyroscope reading was integrated using the following lines of code:

These lines of code are within the larger while loop and each time the while loop runs the gyroscope updates the sensor readings. Yaw is then found using the previous yaw reading, the gyroscope reading, and the calculated difference in time.

Orientation Control

For the orientation control task the robot uses a PID controller and the calculated yaw angle from the gyroscope readings to move towards and maintain the set angle. The implemented PID controller works for various angles and on different floor surfaces.

Rotational Lower Limit PWM

While I was initially testing the PID controller the lower limit PWM value was set to the same as the lower limit value of 40 from the previous lab. Using a lower limit of 40 resulted in the robot getting stuck and not fully turning. I created a new Arduino case and Jupyter Notebook command to find the lower limit PWM for turning. After testing various PWM values, I found that a lower limit value of 120 resulted in the robot turning.

Proportional Control (Kp)

I started with testing the proportional controller. I worked on finding a value for Kp, while keeping Ki and Kd zero. I choose an angle of 180 degrees to start testing the proportional controller. I estimated the Kp constant to be 1.4167 when testing 180 degrees. The equations are found below:

During physical testing I wanted the proportional control to slightly over shoot the desired angle. The over shoot will be addressed using derivative control and the speed will be addressed using integral control. A range of Kp values including 1, 1.5, 1.75, and 2 were tested as can be seen in the below graphs showing yaw, error, PID components, and motor input. I determined that the Kp value should be 2. The code for the proportional controller is below:

---

Graphs for Kp = 2:

---

Trends that can be observed include, as the value for Kp increases the amount of time the robot takes to reach the desired angle decreases, but there is also more overshoot observed. As the value of Kp increases the motor input spends less time at the lower limit value of 120. For example, it is noticeably observed that for Kp = 1 the motor quickly reach the input value of 120 and remains at that value until the desired angle is reached. For Kp = 2 the motors start at the upper bound of 255 and then decrease to the lower limit of 120 before reaching 180 degrees. I choose a value of Kp = 2 to then move to testing the proportional and integral controllers together.

5000 Level Task: Integral Control (Ki) and Wind Up Protection

The next step involved using the proportional controller (with Kp = 2) and the integral controller to increase the speed of rotation and to achieve a small amount of oscillation at the desired angle. The integral controller is the constant Ki multiplied by integrated error over time. As time increases the integrated error increases, resulting in a greater contribution of the integral controller. To prevent the integral controller component from increasing continuously, wind up protection was implemented. If the value of Ki mulitplied by the time integrated error is greater than 200 or less then -200, then the KiIntegral is set to be 200 or -200 and does not further increase/decrease. The code for the proportional and integral controller is below.

Below are graphs for the tested values for Kp and Ki.

---

Graphs for Kp = 2 and Ki = 0.05:

---

Trends that can be observed include, as the value for Ki increases the amount of oscillation and overshoot of the robot at the desired angle increases. The motor input for all tested values starts at the maximum of 255 before decreasing to a value of 120 before reaching the desired angle. For a value of Ki = 0.01 there is minimal overshoot and oscillation and a value of Ki = 0.05 also results in minimal overshoot but slightly more oscillation. For Ki = 0.1 there is more oscillation before reaching the desired angle and for Ki = 0.2 the oscillation continues for a longer amount of time. I choose a value of Ki = 0.05 to move on to testing the proportional, integral, and derivative controllers together.

Derivative Control (Kd)

Derivative Calculation: Lowpass Filter

The final derivative calculation used the previous gyroscope output combined with a low pass filter with an alpha of 0.3055 to remove some of the noise. The calculation for alpha is found below along with the final code to find the derivative control.

I tested a serives of Kd values to find where the overshoot was reduced. The final derivative calculation resulted in the spike and the noise being removed as can be seen in the below graphs.

---

Graphs for Kp = 2, Ki = 0.05, and Kd = 0.05:

---

I choose a final value of Kd = 0.05 as the robot did not overshoot the desired angle of 180 degrees.

Below is a video of the robot turning +180 degrees at Kp = 2, Ki = 0.05, and Kd = 0.05:

More Videos

Below is a video of the robot at 0 degrees:

I also tested negative angles. Below is a video of the robot turning -90 degrees:

Lab 5: Linear PID Control and Linear Interpolation

The objective of the lab to to gain experience working with PID controllers. Labs 5-8 detail PID, sensor fusion, and stunts. This lab is focused on using a PID controller for position control of the robot.

PID Control Explaination

For this lab a PID controller will be used for position control of the robot. The below equation from the course Lecture 7 is used to compute the value for the PID controller:

This equation uses the error e(t) (difference between the final desired value and the current value) in proportional, integral, and derivative control to calculate the new PID input value u(t). For this lab the error is the difference between the desired distance from the wall (1ft) and the robots current distance from the wall. The calculated PID input value is related to the robots speed. Proportional control multiplies a constant Kp by the error. Integral control multiplies a constant Ki by by the integral of the error over time. Derivative control multiplies a constant Kd by the rate of change of the error.

Lab Tasks