Mikayla Lahr

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. The results from the first test are below:

Looking at the results from the first test it can be seen that the maximum velocity reached is just over -2000mm/s. This velocity was only reached once. To confirm the steady state speed, the test was run again and the following results were plotted:

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.

Below are two different runs of the robot using PID control.

Graph 1: Kp = 0.07, Ki = 0.000008, Kd = 8

Initially running the Kalman Filter using the previously described code results in the above graphs. The calculated Delta_T for the PID controller is 0.0066 seconds, a frequency of 151.52Hz. The Kalman Filter is running at a faster frequency than the ToF sensors. The deadband for the motor controller input is 40. This deadband is consistent to the deadband used in Lab 5 for linear PID control. This was the fastest run of the robot with a maximum PWM value of 150. Sigma_1 and Sigma_2 are 122.86 and Sigma_3 is 20. Overall, the model works fairly well, but at around 2 seconds the predictions no longer extend to the next data point compared to the earlier predictions. In the next step I modify the parameters to adjust the prediction values.

I adjusted the parameters for Sigma_1. Decreasing Sigma_1 (model uncertainty for location) indicates that the model is more accurate than what was initially predicted from the calculation for Sigma_1. Sigma_2 was set to 120, which is close to the initial calculated value found previously and Sigma_3 remains at 20. 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. 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. I initially did this to better see the individually plotted prediction points, but then realized that changing how often the while loop runs the prediction step of the Kalman Filter results in the predictions to be slightly off from the ToF distance measurements and the previously plotted prediction values.

The above graphs show the Kalman Filter plotted without the prediction steps, only the updated values. The calculated Delta_T for the ToF sensor readings is 0.1082 seconds, a frequency of 9.24Hz. The Kalman Filter is running at the same frequency as the ToF sensors. Sigma_1 and Sigma_2 are 30.41 and Sigma_3 is 20. The Kalman Filter follows the general trend of the ToF readings, but is slightly off from the plotted line for the ToF distances. I tried modifying the parameters as I did in the previous step, but there was no significant change in the Kalman Filter updates.

Graph 2: 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.

The above graphs show the Kalman Filter plotted without the prediction steps, only the updated values. The calculated Delta_T for the ToF sensor readings is 0.1019 seconds, a frequency of 9.81Hz. The Kalman Filter is running at the same frequency as the ToF sensors. Besides Delta_T, all other parameters were kept the same as in the previous step. The Kalman Filter follows the general trend of the ToF readings, but is slightly off from the plotted line for the ToF distances, although the plotted line is still closer to the ToF readings compared to Graph 1.

Extra Graphs From Prior Trials

Prior to collecting the data to create the previous graphs, I tested the robot multiple times and collected distance readings. The distance readings were not as smooth as I would have expected and the robot moved slowly. I realized that my distance sensor was set to short mode, rather than long mode. The below data was collected before changing the sensor mode. I decided to run this data through the Kalman Filter for fun, to see how well the model would respond to non-ideal data.

Initially, the Kalman Filter follows the ToF distance measurements, but later the predictions become more horizontal rather than continuing along the same slope as the distance sensor readings. I tried to address this issue in the next step.

I adjusted the parameters and determined that decreasing the drag (d) constant resulted in a more downward trend of the calculated predictions. This trend was most significant towards the beginning of the graph.

Above is the Kalman Filter plotted without the prediction steps.

Extra Credit: Implement Kalman Filter On Robot

I will include implementing the Kalman Filter on the robot for extra credit in the next lab report for Lab 8: Stunts.

Thank you!

Lab 7 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 6: Orientation PID Control

The objective of the lab to to gain experience working with PID controllers for orientation. I am a 5000 level student and I choose to work on a PID controller. 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.

Prelab

I replaced one of my motor drivers that was overheating due to some solder touching between pins after I had to reconnect a loose wire. The new motor driver and connections solved the issue.

I considered using tape around the wheels to reduce friction with the ground, but decided that I did not want to change the surface of the wheels. During the lab I found the lower limit PWM value for turning and implemented the found value as a lower limit for the motor control functions. The surface of the wheels is consistent with all past labs as I have previously found lower limit PWM values for forwards/backwards motion using both wheels and one wheel, starting from stationary and moving at a constant speed before lowering the PWM. All of my found lower limit PWM values are for the same wheel surface.

The lab materials include:

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.

Send and Receive Data Over Bluetooth

For Lab 6 data was sent over Bluetooth in the same manner as the previous Lab 5.

Two Arduino IDE cases were initallized. The case ORIENTATION_CASE starts and stops the PID controller and the case SEND_ORIENTATION_DATA sends the collected PID data to Jupyter Notebook. The PID controller starts when a command from Jupyter Notebook is run. The command inputs include a start(1)/stop(0) value; the desired setAngle value to determine the desired angle; and the Kp, Ki, and Kd constant values. In Jupyter Notebook a notification handler similar to the notification handler from Lab 1 is used to create the data array. The PID controller stops when the first input value is set to zero.

I also determined what data I wanted to store and send to Jupyter Notebook. I stored the time, yaw, error, proportional value, integral value, derivative value, PID value, right motor input, left motor input, and the setAngle value.

Below is the Jupyter Notebook Command and corresponding Arduino case to start/stop the PID controller. The run_ORIENTATION_CASE, setAngle, Kp, Ki, and Kd values sent from Jupyter Notebook are printed in the Arduino serial monitor each time a command is sent. These values were initalized as global variables and will be used in the PID controller that is written in the main void loop(). If a command to stop the robot is sent from Jupyter Notebook "Stopped Robot: " will also be printed in the Arduino serial monitor.

Below is the Jupyter Notebook Command and corresponding Arduino case to send the collected data. When data is sent the first time value is printed to the Arduino serial monitor to confirm that data was sent to Jupyter Notebook correctly. The time value is compared to the first time value in Jupyter Notebook.

The same hard stop used in Lab 5 is used in Lab 6 as well to stop the robot if Bluetooth connection is lost.

Lab Tasks

Final Code For While Loop and Added Functions

Final Code

The above link has the full final version of the code used during the lab. I attached the link for my own reference in the future. The below steps show how and why I wrote each piece of the code.

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.

Potential Problems With Digital Integration

A potential problem with digital integration includes not having available data to properly integrate. The if statement to read IMU data only runs when data is ready. If data is not ready then the PID controller will not be using the most current angle available. If there is no ready gyroscope data over time this might cause an issue. I observed the angle readings throughout the lab procedure and did not notice any significant gaps in data for the yaw calculations. Another potential problem is gyroscope drift. If the IMU is continuously used the gyroscope readings will drift away from the actual angle of the robots location. To address this issue, I set the initial yaw reading to zero every time I started using the PID controller.

IMU Sensor Bias

Potential IMU sensor bias includes the placement of the IMU on the robot. If one of the IMU axis is not aligned with the center of the robot or if the IMU is not centered on the middle axis, then the calculated angle from the gyroscope sensor reading may not align with the physical location of the robot.

I also checked to see if there was any numerical initial bias in the yaw calculations from the gyroscope by printing yaw readings to the Serial Monitor. I did not observe any bias when starting the PID controller.

Sensor Limitations and Maximum Rotational Velocity

The gyroscope sensor has four selections for rotational velocity. The selections are (+/-)250, (+/-)500, (+/-)1000, and (+/-)2000 degrees per second. This information is from this datasheet. Of the four potential configurations the settings of (+/-)500, (+/-)1000, and (+/-)2000 degrees per second will work for the purposes of this lab. The maximum angle that could potentially be used for robot motion control is 360 degrees, while smaller angles of 180 degress or less are preferred. The gyroscope configuration of (+/-)500 will be used to ensure that all potential gyroscope sensor data is collected.

Other limitations of the sensor include being compatible with 3.3V or 5V, having 9 degrees of freedom, and the accelerometer has four configurations including (+/-)2g, (+/-)4g, (+/-)8g, and (+/-)16g. The accelerometer will not be used for this lab.

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.

Range and Sampling Time Discussion

The calculated yaw error from the gyroscope sensor can be at most 360 degrees. To determine the sampling time of the gyroscope readings I found the time between each new IMU measurement by temporarily adding a Serial.print(dt_G); line to the if statement inside of the while loop. The sampling period was found to be 0.01s, resulting in a sampling frequency of (1/0.01s) = 100Hz for IMU measurements.

To find how fast the PID control loop is running I temporarily adding a Serial.print(dt_O); line inside of the while loop for the PID controller. The PID loop runs at around 0.01s (1/0.01s = 100Hz). The PID loop is running at around the same speed as the gyroscope sensor readings.

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. The code is below:

After testing various PWM values, I found that a lower limit value of 120 resulted in the robot turning. A video of the robot at a PWM value of 120 is below:

PID Control: Arduino Code

PID Control

The Arduino code for PID control is below:

Within the while loop if run_ORIENTATION_CASE is equal to 1, then the PID controller will run. First the time is recorded and then data for the gyroscope is collected and yaw is found. The error is the desired angle minus the yaw angle. The proportional control component is the error multiplied by Kp (within line 660). The integral control component is the error integrated over time multiplied by Ki (lines 649 and 650). Wind up protection is used in lines 651-656. If the value of the integral term multiplied by the set Ki value is greater than 200 then there will be no further increase in the KiIntegral term used to find the value for the integral controller. Without wind up protection the integral term will continuously increase causing the robot to not stop when the final yaw angle is reached. The derivative control componenet is the rate of change of the error multiplied by Kd (lines 658 and 659). The derivative controller uses a low pass filter and the gyroscope reading (gyrZ) as an input to reduce derivative kick when changing the setpoint.

The PID controller works for both positive and negative angles. Inputs of positive angles turn the robot counter clockwise. Inputs of negative angles turn the robot clockwise.

Motor Control and Stopping

The robot is set to stop when the calculated error is less than 1 degree. All pins are then set to zero. If the error is not within the stopping range than the functions RIGHT_MOTOR and LEFT_MOTOR are called. The if statements and functions can be found below.

Stop when error is 1 degree or less or continue motion:

Right Motor:

Left Motor:

For PID values greater than or equal to 120 and less than or equal to 255, the PID value is used as the input PWM value for the motors. For values greater than 0 and less than 120 the input PWM value is set to be 120, because PWM values less than 120 result in no motion of the robot. For values greater than 255, the PWM value is set to 255 as that is the maximum possible value for the motors.

The two other ways to stop the robot are the same as in Lab 5. The first way is intentionally stopping the robot by sending a "0" value for run_ORIENTATION_CASE in Jupyter Notebook. Sending a "0" causes the following if statement in the while loop to set all pins to zero.

The second way that the robot stops is if the maximum value for the collected arrays is reached, causing all pins to be set to zero and stopping the robot. This is to ensure that each time the robot runs I know if the maximum array size is reached.

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 = 1:

Graphs for Kp = 1.5:

Graphs for Kp = 1.75:

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.01:

Graphs for Kp = 2 and Ki = 0.05:

Graphs for Kp = 2 and Ki = 0.1:

Graphs for Kp = 2 and Ki = 0.2:

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)

The next step involved finding the value for the derivative constant Kd to remove overshoot at 180 degrees. I initially calculated the derivative using the following code (line 15): This resulted in a large spike for the first derivative control calculation as well as some noise as can be seen in the plotted graph for Kd = 0.05 below:

Derivative Kick and Changing Setpoint

After observing the previous graphs and to prepare for changing the setpoint in real time, I changed how the derivative control component was calculated. I used the information on this website. Instead of calculating the derivative of a signal using the integral of another signal, I calculated the derivative using the Z output of the gyroscope using the following code: This resulted in the large spike being removed, but there was still some noise in the derivative controller as can be seen in the plotted graph for Kd = 0.08 below:

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.03:

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

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

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

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:

Full PID Control and Programming Implementation

Sending Bluetooth Commands And Updating The Setpoint In Real Time

After tuning the PID controller I tested changing the setpoint (desired angle) while the controller was running and recording data for multiple changed angles. During the first few attempts the data was not entirely recorded correctly as can be seen in the below graphs.

I realized that whenever I changed the angle the array indices changed for recording yaw. To address this issue I added another if statement in the Arduino case ORIENTATION_CASE that allowed me to control when to set yaw to zero and when to set the initial indicies for the array's to one. The changed and added code can be found below.

The example below shows a change of 60 degrees to 90 degrees:

I was able to record data for multiple angle changes.

For future navigation and stunts, I might need to change the setpoint in real time depending on the desired task. For this lab changing the setpoint was done through Jupyter Notebook, but changing the setpoint could also be done through a series of if statements in Arduino IDE. Changing the setpoint may be beneficial during navigation along with using data from the ToF sensor readings to navigate through the course.

5000 Level Task: Wind Up Protection

The full PID controller with wind up protection, as described earlier, can be observed in the videos below.

Robot turning +90 degrees on carpet:

Robot at 0 degrees on carpet:

The PID controller works on multiple surfaces when maintaining an angle. When turning towards an angle there is slightly more delay compared to on a flat surface, but the robot is still able to reach the desired angle. Below is a video showing the serial monitor outputs for the wind up protection. In the video the KiIntegral term increases over time until reaching 200 and then does not further increase.

Controlling Orientation While Driving Forward And Backward

The robots orientation can be controlled while driving by altering the Arduino IDE code from the previous Lab 5 for the functions FORWARD and BACKWARD. As the robot drives either forward or backward the desired yaw angle will be set to zero. If the yaw angle changes from zero the PID controller will be used to increase the PWM of one wheel side and decrease the PWM of the other wheel side in order to keep the robot moving straight. For longer runs the gyroscope drift may be needed to accounted for. In the below video the yaw drifts from 0.01 to -8.72 degrees over (27.299 - 04.300) = 22.999 seconds,which is around -0.3796 degrees per second.

5000 Level Task: Wind Up Protection

See above: Completed previously.

Lab 6 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 5: Linear PID Control and Linear Interpolation

The objective of the lab to to gain experience working with PID controllers. I am a 5000 level student and I choose to work on a PID controller. Labs 5-8 detail PID, sensor fusion, and stunts. This lab is focused on using a PID controller for position control of the robot. Lectures 6, 7, and 8 were consulted during the lab.

Prelab

The used lab materials include:

Pololu TOF Distance Sensor ; TOF Manual ; TOF Datasheet

QWIIC Breakout Board

QWIIC Connector

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.

Send and Receive Data Over Bluetooth

During the Prelab, I determined how to send and receive data over Bluetooth. Two Arduino IDE cases were initallized. The case PID_CASE starts and stops the PID controller and the case SEND_PID_DATA sends the collected PID data to Jupyter Notebook. The PID controller starts when a command from Jupyter Notebook is run. The command inputs include a start(1)/stop(0) value; the desired setDistance value to determine the desired end distance from the wall; and the Kp, Ki, and Kd constant values. In Jupyter Notebook a notification handler similar to the notification handler from Lab 1 is used to create the data array. The PID controller stops when the first input value is set to zero.

I also determined what data I wanted to store and send to Jupyter Notebook. Initially, I stored time, two ToF distance values (one in millimeters and one in feet), the error value, the integral value, the derivative value, and the PID value. Later in the lab I added and removed append statements as necessary depending on the task I was working on. The SEND_PID_DATA case sends time, distance in millimeters, the error value, P value, I value, D value, PID value, and the motor input value.

Below is the Jupyter Notebook Command and corresponding Arduino case to start/stop the PID controller. The run_PID_CASE, setDistance, Kp, Ki, and Kd values sent from Jupyter Notebook are printed in the Arduino serial monitor each time a command is sent. These values were initalized as global variables and will be used in the PID controller that is written in the main void loop(). If a command to stop the robot is sent from Jupyter Notebook "Stopped Robot: " will also be printed in the Arduino serial monitor.

Below is the Jupyter Notebook Command and corresponding Arduino case to send the collected data. When data is sent the first time value is printed to the Arduino serial monitor to confirm that data was sent to Jupyter Notebook correctly. The time value is compared to the first time value in Jupyter Notebook.

In addition, a hard stop was implemented on the Artemis to stop the robot if the Bluetooth connection fails. The pin output values are all set to zero outside of the while (central.connected()) loop. The code for this is inside the larger void loop() and is seen below (lines 465-469):

Lab Tasks

Final Code For While Loop And Added Functions

Lab 5 Full Code

The above link has the full final version of the code used during the lab to collect distance measurements, extrapolate distance measurements, and move/stop the robot. I attached the link for my own reference in the future. The below steps show how and why I wrote each piece of the code.

Position Control

For the position control task the robot drives as fast as possible towards a wall and stops when it is 1ft away from the wall. The ToF sensor on the front of the robot will be used to determine the distance the robot is from the wall. The implemented PID control is also shown to be robust to changing conditions including starting distance and floor surface.

Range and Sampling Time Discussion

For this task I choose to use the long distance mode for the ToF sensor as I was testing the robot for longer distances ranging from 6ft to 13ft and the range for the ToF sensor when using the long mode is 4m (13.12ft). The ToF sensor error can be at most 4000mm. To determine the sampling time of the ToF sensor I found the time between each new distance measurement by temporarily adding the lines below to the code that runs when the case to start the robot is called:

The sampling period was found to be 100ms, resulting in a sampling frequency of (1/0.1s) = 10Hz for distance measurements. To find how fast the PID control loop is running I printed the value for dt that is calculated each time the PID loop runs:

The PID loop runs at around 8ms (1/0.008s = 125Hz). The PID loop is running faster than the ToF sensor produces new distance readings. This will be explored later in the lab as the ToF sensor data will be extrapolated to use estimated data when there is no new ToF sensor distance measurement.

Proportional Control (Kp)

The first control input I worked on was the Kp input for proportional control. I estimated the value to be around 0.06899 by using the below equation to solve for the PWM values. I wanted to minimize overshoot, so to find the value for the error I subtracted the final desired distance from the maximum error based on the ToF sensor readings. The equations used can be found below:

The initial loop used to find the PID value is:

First the time difference (dt) is found and then the error is calculated based on the current distance and desired stopping distance. After the error is calculated the integral and derivative terms are solved for (both are 0 in this step). These PID equations were also used during the next step to find the Kd value for derivative control.

I used the calculated Kp value (0.06899) as an estimate. During the first physical testing of the system I tested a range of Kp values to determine at what point the robot oscillated around 1ft in front of the wall. Two of these Kp values are found graphed below. I determined that the Kp value should be set to 0.03 to minimize the error and oscillation present. The graphs showing Time vs ToF Distance, Time vs P Control, and Time vs Error for Kp = 0.05 and Kp = 0.03 can be found below:

Plots for Kp = 0.05:

Video for Kp = 0.05:

Plots for Kp = 0.03:

Video for Kp = 0.03:

In the above videos it can be seen that for a value of Kp = 0.05 the robot moves faster but overshoots and oscillates more, resulting in a greater error, compared to when Kp = 0.03. Larger Kp values resulted in the robot moving faster and having more error in the final position. Kp = 0.03 was the smallest value for Kp that resulted in the robot completing the full motion. For this first task I wanted to reduce oscillation, rather than achieve a faster speed and moved to the next step using Kp = 0.03. I focused on achieving a faster speed when tuning the integral Ki value, and prevented overshoot when tuning the derivative Kd value.

Stopping the Robot or Continuing Motion:

The robot is set to stop when the calculated error is less than 5mm. All pins are then set to zero. If the error is not within the stopping range than the function FORWARD is called if the PID value is greater than zero, or the function BACKWARD is called if the PID value is less than zero. The if statements and functions can be found below.

Stop when 1ft is reached or continue motion:

Forward motion:

Backward motion:

For PID values greater than or equal to 40 and less than or equal to 255, the PID value is used as the input PWM value for the motors. For values greater than 0.5 and less than 40 the input PWM value is set to be 40, because assigning values less than 40 would result in no motion of the robot. For values greater than 255, the PWM value is set to 255 as that is the maximum possible value for the motors.

Prior to writing the FORWARD and BACKWARD functions for scaling, I tried using map() to scale the PID values to PWM motor input values. The written code can be found in this folder This approach was not used because when running the robot, I repeatedly entered into a Fault Handler. In a first attempt to fix this problem I removed all code in the Arduino script that was not being used for this lab to reduce the size of the code, but that did not solve the Fault Handler issue. I then wrote the previously discussed functions to move the robot.

Another way to stop the robot are either intentionally stopping the robot by sending a "0" value for run_PID_CASE in Jupyter Notebook. Sending a "0" causes the following if statement in the while loop to set all pins to zero.

The final way that the robot stops is if the maximum value for the collected arrays is reached, causing all pins to be set to zero and stopping the robot. This is to ensure that each time the robot runs I know if the maximum array size is reached.

Derivative Control (Kd)

The next task I worked on was tuning the Kd value using proportional control and derivative control at the same time. I wanted to prevent overshoot and any oscillation of the robot. I increased the Kd value until the overshoot was reduced and the robot stopped at the 1ft desired distance from the wall. This Kd value was found to be Kd = 5. A video showing the robot at Kp = 0.03 and Kd = 5 is below:

Derivative Kick and Low Pass Filter:

For this task the ending setpoint distance does not change, resulting in the error derivative to be continuous. I did not need to implement anything to get rid of derivative kick because there is no changing setpoint. I did consult the reference provided in the lab instructions and read about derivative kick on this website, for future reference.

In addition a low pass filter for the derivative was not implemented as all of the calculated derivative error values were small.

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

For the next task I added the integral controller and inputed a Ki value. To start I set Kd to zero and tested a value of Ki = 0.00001 and Kp = 0.03. This resulted in the robot moving very fast into the wall. I then decided to decrease Ki until I found a value where the robot would stop in front of the wall, but for each value tested the robot ran into the wall. I then implemented wind up protection for the integral control to prevent the robot from overshooting and running into the wall. The Arduino code that was added can be found below:

First a distance measurement is taken at the start of the robots motion to determine the total distance the robot needs to move and an integralStop value is set to be half of the total distance travelled. Each loop an if statement checks to see if the error is greater than the integralStop value. If the error is greater then integralStop a new integral value will be calculated. If the value of the integral term multiplied by the set Ki value is greater than 100 then there will be no further increase in the KiIntegral term used to find the PID control. In additon, if the error is smaller then the integralStop value then the new integral value will be set to zero. The integral term is the Ki value multiplied by the time integrated error. Without wind up protection the integral term will continuously increase causing the robot to not stop when the final distance is reached. Below is a video showing the KiIntegral (I) terms continuously increase until stopping at 100. The error term is not changing significantly, but the KiIntegral (I) term continuously increases.

After implementing wind up protection I found a Ki value that allows the robot to lightly run into the wall before pulling back. I started with a Kp value of 0.00001 which resulted in the robot driving too fast into the wall. I decreased this value until Ki = 0.000008 resulted in the following performance when Kp = 0.03 and Kd = 0:

Without wind up protection the robot accelerates quickly and the following behavior is observed:

I then added the Kd term found in the previous step. A value of Kd = 5 resulted in more overshoot than I wanted the robot to have. I increased the Kd value until the overshoot was again minimized. This resulted from a value of Kd = 8. For values above Kd = 8 there was no longer a noticable difference in minimizing overshoot. The behavior for Kp = 0.03, Ki = 0.000008, and Kd = 8 can be seen in the three trials below. The robot was started from a distance of 6.5ft.

The data collected from the last trial can be found below. I included the printed array of time, distance, PID values, and motor input to confirm that the data was collected properly.

The three below graphs show the data for Time vs ToF Distance, Time vs Motor Input, and Time vs Error.

Graph Analysis and Deadband:

The above graphs for the distance and error reading show that there is minimal over shoot and the robot stops at the desired distance of 1ft away from the wall. The graph for Time vs Motor Input shows that the initial motor input is 58 and decreases until a motor input of 40 is reached. The deadband for the motors is 40. Any value between 0.5 and 40 (forward) or -0.5 and -40 (backward) will be set to 40 in order to produce a PWM value that causes the motors to move. I choose a lower bound of 0.5 and -0.5 to prevent the robot from overly moving after reaching the final distance. The functions used to produce forward and backward motion were discussed previously in the section for Proportional Control. When the robot stops the motor input changes to zero.

Maximum Linear Speed:

The maximum linear speed of the robot can be found using the Time vs ToF Distance graph. Using two points on the graph it can be found that (1506mm - 707mm)/(36105ms - 38258ms) = 0.37m/s

Extrapolation

Frequency of Returned ToF Data

As stated previously in the lab in the Range and Sampling Time Discussion section, the sampling period was found to be 100ms, resulting in a sampling frequency of (1/0.1s) = 10Hz for distance measurements.

Speed of PID Control Loop and ToF Sensor Rate

As stated previously in the lab in the Range and Sampling Time Discussion section, The PID loop runs at around 8ms, which is faster than the ToF sensor produces new distance readings. Extrapolation will be used to estimate the distance when new data is not available using past ToF distance readings.

Calculate PID Control Every Loop and Extapolate Robot Distance From Past Data Readings

Due to the ToF sensor running slower than the PID loop, extrapolation is used to estimate the distance using past data points. Below is the extrapolation equation:

This equation was implemented in the Arduino code. An if statement checks if the ToF distance sensor has a new distance reading. If the sensor has a new distance reading then that point is added to the distance_data array and the current time and distance are stored in time1 and dis1 variables. The previous time and distance from the last time there was a new distance measurement are stored in the time2 and dis2 variables. If there is no new distance reading the extrapolation equation is used as can be seen in line 395. The Arduino code for extrapolation is below:

I outputted the raw data and extrapolated data to the serial monitor shown in the video below (units are mm). The first printed value is the raw data and the second printed value is the extrapolated data. The video shows that the extrapolated data readjusts whenever a new raw distance value is received.

Below is a graph for the raw and extrapolated distances. The graph is zoomed in to show that the extrapolated data (orange) readjusts to the raw data reading (blue) every time there is a new distance measurement. In the next section I ran tests using the extrapolated data.

Testing Different Distances and Surfaces

In addition, I tested the extrapolated distance PID control using different starting distances. The videos for each distance can be found below:

6.5 ft: Kp = 0.03, Ki = 0.000008, and Kd = 8

9.5 ft: Kp = 0.03, Ki = 0.000008, and Kd = 8

13 ft: Kp = 0.03, Ki = 0.000008, and Kd = 8

In the above videos it can be seen that the robot stops 1ft in front of the door, however for the largest tested distance of 13ft there were some runs where the robot did not go straight and instead curved towards the wall. This behavior can be seen in the second 13ft video. To correct this behavior orientation control can be implemented to keep the robot moving straight.

As a note, the PID control for the robot was tested on two different types of flooring as can be seen throughout the lab videos. The behavior of the robot on both surfaces was the same using the PID control. One surface was tile the other was laminate wood flooring.

5000 Level Task: Wind-Up Protection

See above: Completed in the 5000 Level Task: Integral Control (Ki) and Wind Up Protection Section.

Lab 5 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 4: Motors and Open Loop Control

The objective of the lab is to control the car using open loop control, rather than the manual controller. Two dual motor drivers will be connected to the car's motors and the Artemis board. The dual motor drivers will be used to control the movement of the car.

Prelab

The prelab involved reading about how to use an oscilloscope and determining how to connect the dual motor drivers to the motors and Artemis board. The lab materials include:

IMU Sensor

Pololu TOF Distance Sensor ; TOF Manual ; TOF Datasheet

QWIIC Breakout Board

QWIIC Connector

Sketch Of Wiring Diagram

Below is a wiring diagram for the two dual motor drivers connected to the Artemis board, DC Motors, and 850mAh battery. The two VIN pins and the two GND pins on both dual motor drivers are connected together and then connected to the 850mAh battery. The GND pin of one of the dual motor drivers is connected to the GND of the Artemis board. There is a bridge between the BOUT1 and AOUT1 pins of the dual motor driver that is then connected to the DC motor. There is also a bridge between the BOUT2 and AOUT2 pins of the dual motor driver that is then connected to the DC motor. There is a bridge between the BIN1 and AIN1 pins on the dual motor driver and there is also a bridge between the BIN2 and AIN2 pins. On the dual motor driver that connects the drivers to the ground of the Artemis board (top dual motar driver in sketch below), the bridge between BIN1 and AIN1 is connected to pin 11, and the bridge between BIN2 and AIN2 is connected to pin 12. On the other dual motor driver (bottom dual motor driver in sketch below) the bridge between BIN1 and AIN1 is connected to pin 7, and the bridge between BIN2 and AIN2 is connected to pin 9. The pins used in this lab were chosen from the remaining pins on the Artemis board (after the previous lab). The parallel-couple of the two inputs and two outputs on each dual motor driver allows for twice the average current to be delivered without overheating.

Battery Discussion

The Artemis board and the motor drivers/motors are powered from separate batteries. The Artemis board is powered using a 650mAh battery and the motors are powered using a 850mAh battery. Having the Artemis board and motors powered by different power sources avoids any potential transient effects and allows each component to function independently. Any potential current changes in the motors will not affect the Artemis board. In addition, the motors require a higher current battery, which is the reason the 850mAh battery is used to power the motors.

Lab Tasks

Testing The Circuit With The Oscilloscope and Power Supply

Picture Of Setup With Power Supply and Oscilloscope Hookup

The below image is a picture of the power supply and oscilloscope connections. The positive power supply wire (red, towards the top of the picture) is connected to the positive wire of the battery. The ground wire of the power supply (black) and oscilloscope (black) is connected to the same ground wire of the battery (near the middle of the picture). The input for the oscilloscope (near the bottom of the picture) is connected to the wire that will be attached to one of the motors. Each of the four wires that will be attached to the motors was tested to determine if the circuit was assembled correctly and a PWM signal was shown on the oscilloscope.

Picture of oscilloscope (left) and power supply (right) connected to the circuit:

Power Supply Setting And Oscilloscope Discussion

The voltage of the 850mAh battery is 3.7V. In place of using the battery, a power supply was used. The output of the power supply was set to 3.7V to represent the battery power that will be used later on during the lab. Each pin was tested independently of each other. In the Arduino IDE code, one pin was set to have an output of 200, while the other pins all had an output of 0. This was repeated for all four pins. The positive output voltage to the motor was displayed on the oscilloscope and indicated a constant voltage output at each time. The PWM values show that power can be regulated on the motor driver output.

Channel 3 of the power supply was used. 3.7V was used to test the motor driver circuit.

Picture of the oscilloscope output:

The below four videos show the output for each pin that was tested:

Pin 11, Output 200:

Pin 12, Output 200:

Pin 7, Output 200:

Pin 9, Output 200:

Motor Driver Code Snippet Using analogWrite

The below Arduino code shows how each of the wires to the motors were tested. Pins 7, 9, 11, and 12 were defined and analogWrite() was used to assign each pin number the tested output value. In the below example pin 11 is set to have an output of 200, and all of the other pins have an output of 0.

Testing The Robot Wheels

Video Of Wheels Spinning (Each Side Of Robot)

The wheels on each side of the robot were tested individually at two different output values. The power supply used at the beginning of the lab (3.7V) was used to power the motors. Pins 7, 9, 11, and 12 were tested at output values of 50 and 200. While testing each side of the robot I observed the direction and speed that each wheel turned. The wheels rotated faster for the higher output at 200 compared to the output at 50. For the higher output value of 200, the wheels reached the final set speed faster compared to the lower set speed. When rotating at 50 the wheels took longer to start rotating. From the perspective of the videos pin 7 rotates counter clockwise and pin 9 rotates clockwise. Pin 7 and pin 9 move the wheels on the same side of the robot in different directions. In addition, from the perspective of the videos pin 11 moves clockwise and pin 12 moves counter clockwise. Pin 11 and pin 12 move the wheels on the same side of the robot in different directions. To consolidate the report, all of the videos taken can be found in this folder: Folder Link Below are videos of pin 12 and pin 9 with an output value of 200.

Pin 12 with an output of 200:

Pin 9 with an output of 200:

The Arduino code to test the wheels is found below. One pin was set to have an output value of either 50 or 200 and all other pins were set to have an output value of 0. This is the same code that was used in the previous section to test if the circuit was working properly before sautering to the motors.

Video Of All Wheels Spinning

After testing each side of the robot, all of the wheels were tested at the same time. The 850mAh battery was used to power the motors instead of the power supply. The below video shows both sides rotating while the motors and Artemis board are both connected to their respective batteries.

The Arduino code to test the wheels on both sides of the robot is found below. Pins 9 and 12 were set to have an output of 200 and pins 7 and 11 were set to have an output value of 0. This is the same code that was used in the previous section to test both sides of the robot individually.

Picture Of All Components On Car

Below is a labeled picture showing all components on the robot. Including the 650mAh battery, Artemis board, IMU, two ToF sensors, two motor drivers, and a breakout board. The 850mAh battery to power the motors is on the other side of the robot.

After connecting all components to the car I decided to test how the robot moves. The video below shows my first test for having the robot drive on the ground. In the video, the robot initially moves towards the left when the pins used for the motors have the same output value. This trend was noticed in future steps of the lab when determining how to calibrate each of the motors.

The below code shows the loop used to run the robot on the ground.

Below is a diagram of the entire wiring that is used on the robot:

Lower Limit PWM And Open Loop Control

Lower Limit PWM Value Discussion

During this step of the lab, I used Bluetooth to send a command from Jupyter Notebook to run different output values more easily compared to uploading Arduino IDE code every time I wanted to change the output value of a pin. The below images show the Jupyter Notebook Command and the Arduino IDE case that uses analogWrite() to move the robot.

The lower limit PWM value is the lowest output value that a pin can be set to that will still result in continuous motion of the robot. The wheels on one side of the robot were tested, while the wheels on the other side remained stationary. The lower limit PWM for pin 11 (right side) was found to be 59. The lower limit PWM for pin 7 (left side) was found to be 50.

Pin 11 (Right Wheels) with an output of 59:

Pin 7 (Left Wheels) with an output of 50:

I also found the PWM value for both wheels when they are being used together, starting when the robot is stationary. The output value was found to be 35 for both wheel sides. The video showing the robot moving when both wheels (pins 11 and 7) have an output value of 35.

Note: During lab office hours I asked a TA (Larry Lu) about how the surface conditions affect the PWM values. Specifically I asked if testing the robot on the floors in my apartment (laminate wood flooring) would change the lower limit PWM value compared to the tile floors in the lab and hallways. I tested the robot's lower limit PWM value at my apartment and received the same PWM values. The PWM values may start to differ if the robot is travelling over a surface with a lot of friction such as a carpet or outside in the grass. This information is helpful to know as I can better understand the operating conditions of the robot.

Calibration Demonstration

The wheels of the robot do not spin at the same rate and a calibration factor needed to be implemented. The below video shows the robot moving in a straight line, starting at 7 feet and then continuing past the end of the tape measure. For the demonstration, pin 11 was set to an output of 60 and pin 7 was set to an output of 70. In addition to the below video, the robot was run multiple times at varying input values. My first tested value was pin 11 at an output of 60, which is close to the found PWM value from the previous step. When pin 7 was set to an output of 51, the robot continuously moved towards the left. Through trial and error I found that an output of 70 for pin 7 kept the robot moving in a straight line. I used these found values for reference during future tests: 70/60=1.166666666666667.

I increased the output of pin 11 by 20 for each test and multiplied the value for pin 7 by 1.166666666666667. This incrementation worked for all tested pin 11 and pin 7 values until I reached an output value of 220 for pin 11. The calculated output value for pin 7 should be 257, rounded down to 255 due to 255 being the maximum output value. For this test the robot continuously moved towards the right and the value for pin 7 needed to be lowered to 250 to keep the robot moving straight. Due to this discovery, I decided to also test an output value of 210 for pin 11, the calculated output value for pin 7 should be 245. In order to keep the robot moving straight, and not to the right I needed to reduce the output of pin 7 to 240. The trend shows that after an output of 200 for pin 11 the calibration factor to find the corresponding output value for pin 7 starts to differ.

In addition, as a final test I wanted to determine how low I could set the output values while still having the robot move in a straight line for at least 6 feet. I set the output value of pin 11 to 40 and calculated the output value of pin 7 to be 47. For these output values, the robot moved in a straight line for more than 6 feet. When I set the output value for pin 11 to be lower than 40 the wheels began to stall and hesitate before rotating compared to the other side of the robot. I also included a video at the end of the section showing the wheels spinning.

To produce the robots motion the same Arduino IDE case and Jupyter Notebook command were used from the previous section. The length of the delay was changed depending on how fast the robot moved (to prevent the robot from running into a wall). All of the recorded videos are in this folder.

Below is a graph showing all of the tested values as data points. In addition, I plotted the trendline for all data points, as well as the trendline for all of the data points except for the last two that began to differ from the previous measurements. The slope and y-intercept were also found for each trendline. The calculated slope is the calibration factor. The slope excluding the last two data points, 1.165 is close to the 1.166666666666667 value.

Video showing wheels hesitating when pin 11 (right) has an output of 35 and pin 7 (left) has an output of 41:

I also tested the calibration factor when the robot is going in "reverse" (with the robot moving in the direction away from the ToF sensor on one end). I initially tested the previously found output values with the corresponding pins on each wheel side, with pin 12 having an output of 60 and pin 9 having an output of 70. I found that by doing this the robot moved towards the left. I decided to switch the output values; pin 12 has an output of 70 and pin 9 has an output of 60). The robot moves straight using these output values. I find this interesting as it confirm my previous thoughts about one side of the wheels being slightly misaligned.

Open Loop Code And Video

Below is a video showing open loop control of the robot. In the video the robot moves forward, turn towards the right, moves backward, stops, spins counterclockwise, stops, spins clockwise, and then stops. I used output values found from the calibration graph.

Arduino code for robot motion:

5000 Level Tasks:

analogWrite Frequency Discussion

Below is the image of the oscilloscope when connected to the circuit. The wheel motors are powered using the battery. During lab office hours I asked a TA why the oscilloscope outputs were more noisy compared to the oscilloscope outputs from earlier in the lab and it is most likely because the circuit is now connected to the motors that result in more noise compared to when the circuit was not connected to the motors on the robot.

From the image its can be seen that one horizontal block along the x-axis is 2.50ms and one vertical block along the y-axis is 1.0V. The period of the PWM signal is about 2.6 horizontal blocks, which is equivalent to 6.50ms (0.0065s). The frequency analogWrite() generates is about 1/0.0065s ~= 154Hz. In lab 3 the sampling period of the ToF sensors was found to be 0.04s and the sampling frequency was 25Hz. The frequency analogWrite() generates is fast enough for the motors on the robot when running tests as well as when using data from the ToF sensors. Manually configuring the timers to generate a faster PWM signal will allow for more control over the movements of the robot and a more steady PWM signal.

The code used during the oscilloscope observation is below:

Lowest PWM Value Speed Discussion

The lowest PWM value for each side of the robot was found by running the robot with an initial output of 60 for pin 11 and 70 for pin 7 to correspond with the found calibration values. The robot ran with these outputs for 3 seconds and then all pin outputs were set to zero expect either pin 11 or pin 7. Either pin 11 or pin 7 remained turned on for another 3 seconds, but decreased to a lower output value. I ran the robot multiple times to determine at what output value the robot no longer moved. It was determined that for pin 11 the lowest PWM output is 35 and for pin 7 the lowest PWM output is 33. The videos for each pin can be found below.

Pin 11 with a lowest PWM value of 35:

Pin 7 with a lowest PWM value of 33:

In addition, the lowest combined wheel PWM value was found when both wheels were being used at the same time. The output value for both wheels (pin 11 and pin 7) is 25 when the robot is already moving. The robot was initially run with an initial output of 60 for pin 11 and 70 for pin 7 for 1.5 seconds and then both wheels decreased to an output of 25 for 5 seconds. Below are two videos. One video shows the final output at 25 (the robot moves until the total set time of 5 seconds is over). The other video shows the robot stop before the 5 seconds is over and the motors stall when set to an output of 24, confirming the lower limit PWM value of 25 to keep the robot moving.

Both Wheels: Lower Limit PWM of 25:

Both Wheels: Tested value of 24:

The command line in Jupyter Notebook and the Arduino IDE case can be found below:

Lab 4 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 3: TOF

The objective of the lab is to connect the time of flight (TOF) sensors to the Artemis board.

Prelab

The prelab involved reading about the ToF sensor used in the lab. The lab materials include:

Pololu TOF Distance Sensor ; TOF Manual ; TOF Datasheet

QWIIC Breakout Board

QWIIC Connector

Task 1: Note The I2C Sensor Address

From the datasheet it was found that the I2C sensor address for the ToF sensor is 0x52. The two ToF sensors have the same address and one of the addresses will need to be changed in order to receive data from both sensors at the same time.

Task 2: Approach To Using Two ToF Sensors

To use two ToF sensors, the XSHUT pin will be used to connect one of the ToF sensors to pin 8 on the Artemis board. The XSHUT pin will be set to LOW and then the I2C sensor address will be changed for the other ToF sensor that is on. After the I2C address has been changed the XSHUT pin will be set to HIGH, turning the other TOF sensor back on. This will allow for both ToF sensors to be used in parallel.

Task 3: Placement Of Sensors On Robot

The placement of the two ToF sensors on the robot will impact the field of vision. Two long QWIIC connectors will be used to attach the ToF sensors. Longer connectors will allow me to position the ToF sensors in more potential ways compared to shorter wires. The potential arrangements of the sensors include (1) a sensor on the front and left side, (2) a sensor on the front and right side, (3) two sensors positioned on either side of the front of the robot, and (4) a sensor on the front and the back. Depending on the task at hand any of these sensor positions may be effective. If there is no sensor on the back of the robot there might be an issue if the robot needs to back up or move in reverse. If there is no sensor on the side then the robot might miss an obstacle that is located in a blind spot.

Task 4: Sketch Of Wiring Diagram

Lab Tasks

Task 1: ToF Sensor Connected To QWIIC Breakout Board

The Artemis board was powered using a battery:

ToF sensors connected to the QWIIC breakout board:

Task 2: Artemis Scanning For I2C Device

The SparkFun VL53L1X 4m Laser Distance Sensor Library was installed on Arduino IDE. The code to find the I2C address: File->Examples->Apollo3->Wire->Example05_Wire_I2C.ino

One ToF sensor was connected during the scan. The sensor had an I2C address of 0x29. In the datasheet for the ToF sensor, the listed I2C address is 0x52. The Example05_Wire_I2C.ino code returned an address of 0x29. This is a result of the binary for address 0x52 (binary: 0b01010010) being shifted to the right, resulting in an address of 0x29 (binary: 0b00101001). The rightmost bit is ignored because the bit identifies if data is read/write.

Task 3: Sensor Data With Chosen Mode

Sensor Mode Discussion

The ToF sensor has two modes: (.setDistanceModeMedium(); //3m has been removed)

.setDistanceModeShort(); //1.3m

.setDistanceModeLong(); //4m

Information from the datasheet shows that the short mode can only reach up to 1.3m and is the most resistant to ambient light. The long mode can reach up to 4m but is very sensitive to ambient light, which decreases the distance to 0.73m. The timing budget range for the short mode is 20ms - 1000ms. The timing range for the long mode is 33ms - 1000ms. Short mode has greater accuracy and less sensor noise compared to the long mode. While it would be beneficial for a robot that needs to sense objects farther away to use the long mode, for this lab I am prioritizing accuracy and choose to test the short mode.

Testing .setDistanceModeShort();

The ToF sensor was first tested using the File->Examples->SparkFun_VL53L1X_4m_Laser_Distance_Sensor->Example1_ReadDistance.ino This code was modified to include the line distanceSensor.setDistanceModeShort(); in the setup to choose the short mode for the ToF sensor.

ToF Sensor Setup:

Arudino code to find the ToF distance:

Sensor Range:

In the datasheet it is stated that the short mode for the ToF sensor has a range up to 1.3m. To test the range of the sensor I took an initial measurement at 100mm and continued to take measurements up to 1900mm. The graph below shows the actual distance of the ToF sensor from the wall vs the ToF sensor distance. The plotted measured distance value is the mean value of the ten collected data points. From the graph below it can be seen that after 1.3m the measured values start to differ more from the actual values compared to measured values less than 1.3m.

Accuracy:

The difference in the measured and actual distances was plotted against the actual distances the ToF sensor was from the wall. When the ToF sensor is less than 1.3m from the wall, the difference is between 18mm and 40mm. After 1.3m the difference in the measured vs actual distance increases.

Repeatability:

The standard deviation vs actual distance graph is shown below. The standard deviation is below 3mm for measurements under 1.3m. After 1.3m the standard deviation starts to vary and the standard deviation at 1900mm is 8.75mm.

Below are the time and measured distance arrays:

100mm: 9 measurements at 75mm, 1 measurement at 74mm

300mm: 6 measurements at 268mm, 4 measurements at 267mm

500mm: 5 measurements at 478mm, 5 measurements at 479mm

700mm: 5 measurements at 679mm, 4 measurements at 681mm, 1 measurement at 677mm

900mm: 4 measurements at 882mm, 5 measurements at 879mm, 1 measurement at 883mm

1100mm: 5 measurements at 1076mm, 4 measurements at 1070mm, 1 measurement at 1071mm

1300mm: 4 measurements at 1258mm, 4 measurements at 1263mm, 2 measurements at 1265mm

1500mm: 3 measurements at 1448mm, 2 measurements at 1447mm, 3 measurements at 1440mm, 2 measurements at 1449mm

1700mm: 2 measurements at 1634mm, 3 measurements at 1631mm, 3 measurements at 1632mm, 2 measurements at 1637mm

1900mm: 3 measurements at 1782mm, 2 measurements at 1791mm, 4 measurements at 1802mm, 1 measurement at 1784mm

Ranging Time:

The ranging time is the time between each sensor measurement. I commented out all print statements, except the calculated value between the start and end times to allow for code to run as fast as possible. The ranging time was found to be around 55ms.

Task 4: Two ToF Sensors: Working In Parallel

Two ToF Sensors were connected to the Artemis board and Arduino code needed to be written for the two ToF sensors to work in parallel. The code below is written according to the prelab procedure.

The Serial Monitor prints the I2C addresses for both distance sensors to confirm that one of the addresses was changed to 0x30 and the other remains 0x52.

Task 5: ToF Sensor Speed: Speed And Limiting Factor

The code to find the ToF sensor speed can be found below. Time is continuously printed and distance measurements are only printed when a measurement is available.

When the sensors are not collecting data the loop takes around 2ms. When data is being collected the loop takes around 11ms. The longer loop time when collecting ToF sensor data is the limiting factor. The limiting factor is the time needed to record distance measurements.

Task 6: Time vs Distance: Data From Two ToF Sensors Over Bluetooth

The code from Lab 2 was modified to send time stamped ToF sensor data to Jupyter Notebook. The data for the two ToF sensors was plotted against the recorded time. The case I created in Arduino can be found below.

5000 Level Task 1: Discuss Different Distance Infrared Sensors

Active and Passive Infrared Sensors

The two main types of infrared sensors are active and passive infrared sensors. Passive infrared sensors sense nearby emitted infrared light, using a difference in signals between two pyroelectric sensors. These sensors are often used for security purposes to detect if there is a heat energy change in the environment, but cannot determine the distance to an object. Active infrared sensors use an emitter and receiver to determine the distance to an object. If there is an obstruction, the active infrared sensor knows that there is an object in between the emitter and receiver. Active infrared sensors are used in industry conveyor belts, as well as in garage door safety mechanisms. (Source: Arrow Electronics) Different types of infrared sensors include proximity sensors, ToF sensors, and triangulation sensors. (Source: NYU)

5000 Level Task 2: ToF Response To Different Colors And Textures

Four colors and three textures were tested at a distance of 500mm. Colors include pink, blue, yellow, and red. Textures include a hand towel, rug, and folder. The control surface was the dark grey wall that I used throughout the lab for distance readings. The change in color or texture did not affect the ToF sensor's distance output that remained around 478mm.

Control:

Color Pink:

Color Blue:

Color Yellow:

Color Red:

Texture Hand Towel:

Texture Rug:

Texture Folder:

Lab 3 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 2: IMU

The objective of the lab is to connect the IMU to the Artemis board and record the robot performing a stunt.

Prelab

The prelab involved reading about the IMU used in the lab. The lab materials include:

Li_Ion 3.7V 850mAh Battery

SparkFun 9DOF IMU Sensor and IMU Datasheet

Force 1 RC Car

Lab Tasks

Task 1: Set Up The IMU

Install SparkFun

The SparkFun 9DOF IMU Breakout - ICM 20948 - Arduino Library was installed.

Artemis IMU Connections

The setup for this lab is shown in the image below.

IMU Example Code and AD0_VAL

The example code found under SparkFun 9DOF IMU Breakout - ICM 20948 - Arduino Library->Arduino->Example1_Basics was run to test if the IMU was working correctly.

Upon opening the Example1_Basics code, the AD0_VAL default was set to one. When the ADR jumper is closed the value should be set to zero. AD0_VAL corresponds to the last bit of the I2C address. My ADR jumer on the IMU was closed. For this reason, I needed to change the AD0_VAL to zero to return the accelerometer, gyroscope, magnetometer, and temperature data.

While viewing the output data in the serial monitor, I moved the IMU to change the accelerometer and gyroscope data. I looked at the notated axis that was printed on the IMU and moved the IMU along the X, Y, and Z axis. I noticed that the respective X, Y, and Z serial monitor outputs for the accelerometer increased when I moved the IMU along the respective positive axis. The gyroscope data for pitch, roll, and yaw also changed when I rotated the IMU around the X, Y, and Z axis.

Adding A Slow Blink

In the Example1_Basics code, I added a slow blink at the beginning of void setup() to turn the Artemis LED on and off three times.

Task 2: Accelerometer

Accelerometer Equations

The following equations for pitch (theta) and roll (phi) were found in the FastRobots-4-IMU Lecture Presentation.

Accelerometer Pitch and Roll at -90, 0, and 90 degrees

These equations were used to compute pitch and roll for the accelerometer. I implemented the calculations in the Example1_Basics sketch (lines 247 - 255 below) that was used in the previous task to receive continuous data from the Artemis board and plot the results.

The below graphs show Pitch (data 0, green) and Roll (data 1, purple) at 0 degrees. The x-axis is time (sec) and the y-axis is degrees.

Pitch and Roll at 0 degrees:

The below graphs show Pitch (data 0, green) at -90 and 90 degrees and the corresponding Roll (data 1, purple) outputs. The x-axis is time (sec) and the y-axis is degrees. From the graphs it can be seen that when pitch is at -90 degrees there is noise in the roll outputs.

Pitch at -90 degrees:

Pitch at 90 degrees:

The below graphs show Roll (data 1, purple) at -90 and 90 degrees and the corresponding Pitch (data 0, green) outputs. The x-axis is time (sec) and the y-axis is degrees.

Roll at -90 degrees:

Roll at 90 degrees:

Accuracy Analysis

I plotted the data for pitch and roll at 0, 90, and -90 degrees in Jupyter Notebook and calculated the mean and standard deviation.

Below are graphs. I also included a black line at 0, 90, or -90 degrees for reference. Note: The data for the graphs was collected by holding the IMU against the table using my hand. This may have resulted in some error due to the surface level or any small hand movements. In the future the IMU should be calibrated when it is secured to the robot to achieve the most accurate pitch and roll data.

Fast Fourier Transform (FFT) Graphs

The noise in the accelerometer was examined using a Fast Fourier Transform. Data for the FFT was collected as I moved the IMU. The sinusoid and the FFT graphs can be found below. There is a large spike closer to 0Hz and less noise as the frequency increases. Note: I modified the code from Stephan Wagner's Ed Discussion post to plot the FFT graphs.

Low Pass Filter FFT Graphs

From observing the above graphs, I choose a cutoff frequency of 25Hz to use for a low pass filter. Using the determined cutoff frequency, I calculated the alpha used in the low pass filter. The equations from lecture are below:

The low pass filter alpha was calculated to be 0.3698

The low pass filter was implemented in Arduino. The code to find pitch and roll for the accelerometer can be found below.

The below FFT graphs show the original signal (red) and the reduction in noise after implementing a low pass filter (blue). Choosing a lower cut off frequency reduces more noise.

Prior to implementing a low pass filter, there was not a significant level of noise in the IMU data due to a pre-existing internal low pass filter. The information for this can be found on page 10 of the datasheet for the IMU.

Task 3: Gyroscope

Gyroscope Equations

The following equations for pitch, roll, and yaw were found in the FastRobots-4-IMU Lecture Presentation.

The gyroscope measures angular velocity. To find pitch, roll, and yaw the data collected from the IMU needs to be multiplied by a time step (dt in the below code).

Gyroscope vs. Accelerometer Outputs

The code above was added to Example1_Basics and plotted alongside the data for the accelerometer's pitch and roll. The gyroscope data drifts over time, but has less noise then the accelerometer data. In the below graphs the IMU was moved slightly to achieve different outputs.

Pitch for Gyroscope (Blue) and Accelerometer (Orange):

Roll for Gyroscope (Blue) and Accelerometer (Teal):

The drift for the gyroscope can be better seen in the below graphs. The gyroscope was held in the same position as the data was collected. As time increases the measurements drift away from the starting value.

Code to plot the above graphs:

Adjusting The Sampling Frequency

Decreasing the sampling frequency resulted in a slower update to the angle measurements and more variation in the outputs between each successive measurement. The sample rate should be high to achieve the greatest level of accuracy when recording data.

Complimentary Filter

Complimentary filter equation from lecture:

The complimentary filter shows resistance to small vibrations as the IMU is moved during data collection. The complimentary filter reduces noise compared to the accelerometer output. Alpha is a set constant that represents how well the accelerometer output measures the correct pitch and roll data. I examined how different alpha values affected the graphs. In the below graphs alpha = 0.5

Pitch for Gyroscope (Blue), Gyroscope Complimentary Filter (Orange), and Accelerometer (Teal):

Roll for Gyroscope (Blue), Gyroscope Complimentary Filter (Orange), and Accelerometer (Teal):

Pitch for Gyroscope, Gyroscope Complimentary Filter, and Accelerometer (alpha = 0.5):

Roll for Gyroscope, Gyroscope Complimentary Filter, and Accelerometer (alpha = 0.5):

Additional graphs when alpha = 0.2

Code to plot the above graphs:

Task 4: Sample Data

Speed Of Sampling

All of the accelerometer and gyroscope data print statements and any delays in the ble_arduino.ino file were removed in order to speed up the execution of the main loop. A case called "ACC_GYR" was created that checked if the data was ready in the main loop, and if there was data available stored it in arrays. After speeding up the loop I found that new values were sampled every 0.04s, which is a sampling frequency of 1/0.04s = 25Hz.

Collect And Store Time Stamped IMU Data In Arrays

The data is stored in arrays on the Artemis board before being sent to Jupyter Notebook. There are 7 arrays including time (1); accelerometer: x (2), y (3), z (4); gyroscope: x (5), y (6), z (7). I choose to use separate arrays because in the future I can decide what data I want to send to Jupyter Notebook. The data type used to store the data is a float. I choose to use a float as it is less bytes then a double, allowing for more data to be stored on the memory of the Artemis. A float is 4 bytes, resulting in one packet of data information for the 7 arrays to be 28 bytes. The Artemis has 384kB of RAM. Sampling at 0.04s (25Hz) will result in data being collected for 548 seconds.

Code to find X, Y, and Z for Accelerometer and Gyroscope.

Code to find pitch and roll for Accelerometer; and pitch, pitch compliment, roll, roll compliment, and yaw for Gyroscope.

At Least 5 Seconds of IMU Data Sent Over Bluetooth

I collected data for the X,Y,Z positions of the accelerometer and gyroscope as well as the pitch and roll for the accelerometer; and pitch, roll, and yaw for the gyroscope.

Task 5: Stunt Recording

The robot is moved using the controller. The robot can move very quickly, but it is hard to control the speed. If the robot changes directions too quickly there is a chance that it might flip over. It is also hard to slow down the robot to a stop. When slowing down the robot, the reverse needs to be applied. This is easy to over compensate and the robot might end up traveling in reverse rather than coming to a stop.

Lab 2 References

Thank you to all of the TAs that answered my questions. I referenced the past lab reports of Liam Kain, Rafael Gottlieb, Larry Lu, Julian Prieto, and Ignacio Romo.

Lab 1: The Artemis Board and Bluetooth

Part 1

Test the connection between the Artemis board and the computer. Arduino IDE was installed and a number of tasks were completed to show the functionality of the Artemis board.

Prelab

Arduino IDE 2.2.1 was installed onto my computer.

Materials include:

Arduino IDE