Prelab
DMP
In lab 2, we used complementary filters to get pitch and roll data. We cannot do the same with yaw as the gravity is parallel to the z-axis. If we change the yaw by rotating the robot along the z-axis, the z vector is constant. One option is to integrate the gyroscope in the z-direction to get yaw. However, by doing digital integration, there is a small nonzero error each time we integrate (even when not moving), which will unboundedly accumulate over time. Another approach is using the onboard magnetometer of the IMU as another reference point. However, we won't do this since it is susceptible to noise.
The limitation of my sensor is that the maximum rotational velocity read by the gyroscope is 2000 dps due to the line “success&=(myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR)==ICM_20948_Stat_Ok)." This is sufficient for us since, from testing, our car never gets to such speeds. We can configure the max velocity with the GYRO_FS_SEL parameter according to the data sheet.
I use the DMP onboard the IMU by following Stephan Wagner’s tutorial to minimize yaw drift. A DMP offloads all the math the MCU would do onto the breakout board of the IMU instead. It is very low power and uses sensor fusion to give clean data and can correct for noise and drift in the accelerometer and gyroscope, respectively. The drawback is that this requires 14kB of the MCU’s program memory. Below is a visual demonstration of the DMP working as it maps the quaternion angles to a 3D visualization.
My sensor has a bias. With the DMP we can use an offset to fix this (discussed in the calibration section). Looking at the data below when the car is still the slope of this line is around (2.04-1.92)/30=0.004 deg/sec which is the rate our error grows as a result of the bias.
Yaw calculation:
Orientation of my IMU sensor:
To avoid the FIFO overflowing, I need to slow down the rate of the DMP and make sure to continually flush the queue. From previous labs, my PID loop runs at 125Hz, and the DMP runs at a max of 55Hz, so it should be fine running at the max. To be safe, I run at a 27.5Hz ODR rate, which gives me (55/27.5)-1=1.
To continually flush the queue, I take data from the FIFO while “myICM.status==ICM_20948_Stat_FIFOMoreDataAvail." Since this is faster than TOF, we can poll the BLE more often to maintain connection.
Improving BLE
Calibration
Yaw angle still drifts with DMP. I collect yaw data for 15s, and then after 6s (when the angle equilibrates), I take the average of the rest of the data and store that as a global offset value. I subtract this offset from the measured angle so our controller is more accurate.
New Parameters
I added parameters to the START_PID command for the set point and a flag for linear/angular PID. I have a separate command to collect data after the control loop runs for a few seconds so as to not slow down the loop. I can also easily tune the PID gains like in the last lab.
Lab
I chose the P controller since it worked well enough. Since this is in-place orientation control, the output of the PID controller will be a PWM value to drive one motor forwards and the other backwards at the same speed provided from the control output. Like the last lab, we will have a calibration offset of 16 to add to the left motor. However, here it is used to try and keep the robot rotating in-place instead of pivoting.
I use my results from Lab 4 to set the minimum PWM value to 140 and the maximum PWM value to 200. I update the PID controller only when we are on an index that is a multiple of 3 to avoid starving the BLE. I also updated my motor commands to only analogWrite() when we have new data as discussed in my ed post. I clamp the control output with the min and max PWM values and then use the sign to determine which direction to turn to reach the set point. Lastly, if the absolute error<2, I just don’t move the motors to avoid jittering.
The video below is a set point of 45° and the max error we can have is 180° as atan2 goes from (−π,π] The max speed of 200 is what we want when the error is a max so our initial kp=200/180=1.111.
As you can see there is overshoot and steady state error the blue and red line do not completely overlap) as expected with a P controller. We would use the I term to remove the steady state error and the D term to dampen the P term to minimize overshoot.
Kp=1.9, target_angle=45°, and resistance to external perturbations.
Kp=1.9 and Target_angle=-95°
Kp=2.3 and Target_angle=120° to get values that were not just the minimum PWM.
Kp=2.1 with the first half of the time a target_angle of 60° and the second half a target_angle of -60°
Final Kp=2.1 for a balance of reaching the set point fast and minimizing overshoot. The example above does overshoot, and we can solve this with Kd as discussed earlier. By observing the data, the settling time seems to be around 1.25s. You can see how the motor output sometimes saturates at the max or min.
You may have predetermined waypoints in the navigation lab where you update the set points for linear and angular PID as you go. To move forward and turn, we would rotate the left and right motors at different speeds. We could have a general set motor command that takes in the control output as well as a parameter of the speed we go forwards, and we can add these two values to get different speeds.
Range/Sampling time discussion
Our data frequency is expected to be 27.5Hz but is really around 28.6Hz, and our control loop frequency is around 9.5Hz, as we only update the control output when the index%3==0 to avoid starving the BLE. I successfully tested from -120 to 120°. One problem I had was with -179° because of wraparound, which is something to consider in future labs.
Collaborations
Thank you Prof. Helbling. I referenced Sean Zhang’s, Aidan Derocher's, Stephan Wagner's webpages. This website template was inspired by Hunter.