Prelab

Most of the infrastructure for this lab had already been implemented in the previous lab. However, I added a flags start_IMU and start_O_controller to start sampling the IMU and running the orientation pid controller. I also created accociated BLE commands START_IMU, STOP_IMU, START_O_CONTROLLER, and STOP_O_CONTROLLER to command these flags. Finally, I slightly modified my command_motors function so that I could also supply a minimum PWM to account for deadband as an aguement. This is because moving straight and turning have different minimum PWM times.

DMP Setup

One thing that I noticed in the previous labs was that the yaw angle has substantial drift on the IMU. I found that I could correct this using the digital motion processor (DMP) built into the IMU but disabled by default. The DMP does this by fusing the accelerometer, gyroscope, and magnetometer data so that a single quaternion can be constructed that encodes the orientation of the robot. After reading through the documentation and enabling the DMP, I added the following code to my setup to initialize DMP.

One important thing to note is that INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR sets the maximum sampling rate of the IMU to 1.1kHz and the maximum detectable rotational speed to 2000 degrees per second. Both of these should be more than enough for the dynamics profile of this car. In practice, I was able to sample the sensor at 384hz. This is around how fast the Artemis loop runs on its own. We can change the sample rate of the IMU but the current one seems more than adaquate.

The DMP outputs sensor measuments as a quaternion. Therefore, both polling the IMU and computing the yaw euler angle ([-180,180]) from the quaternion is handled by the IMU_DMP_YAW function shown below. Note that this code was heavily inspired by Example7_DMP_Quat6_EulerAngles.ino in the examples folder.

Orientation PID Control

The code for my orientation pid controller is shown below and very similar to the code used for the linear PID controller in the previous lab.

Note that I added the following logic to prevent the transition from -180 to 180 degrees from causing spikes and instability in my controller.

One thing to note is that similar to the previous lab, I took the derivative of the sensor measurement rather than the error itself. This prevents derivative kick in my controller. In addition, although the IMU outputs raw rotation rates, the DMP only gives angle measurements so I was forced to descretely differentiate those measurements. To ensure that the transition from -180 degrees to 180 degrees does not cause a massive spike in velocity, I added similar logic to that used on the error term.

Finally, just like the linear controller, I have a low pass filter to remove noise from my derivative term.

With IMU_DMP_YAW and pid_O_controller implemented, I integrated these functions into my main control loop as shown below.

Testing the Controller

After some experimentation, I found that kp = 0.03, ki = 0.006, and kd = 0.08 had nice preformance. Below are plots and video of commanding my car to move from around -90 degrees to 30 degrees. As shown by the plots and video, the car can quickly change orientation to a new setpoint with an error threshold of less that 0.3 degrees.

Here is an example of me trying to disturb my controller after it has reached an angle setpoint.

Improvements for the Future

I also added functionality so that I could dynamically change the setpoint of the orientation controller. The nature of my PID controller function made this easy to do as I simply had to add an extra command to allow this. In addition, my flag based implementation of my software makes it easy overall to enable/ disable different controllers and sensors which will hopefully make development easier in the future.

Another thing to think about is how to correct orientation while the car is moving forward. I think it might be possible to layer some differential power calculated by the the orientation controller onto to the standard linear PID controller keep the orientation constant while the car is moving forward or backward.

Windup Protection

I implemented windup protection in a very similar manner to the previous lab. I did not observe any horrible behavior from my orientation controller due to windup but it is still important to have just in case the car gets stuck somewhere. Without windup protection, the integral term could saturate the actuators and cause the car to spin out of control. By constraining the integrator error, we can prevent this.

Conclusion

Overall, I am very happy with the preformance of my orientation controller. I think that the DMP really helped enhance the utility of my orientation controller and I look forward to using it in the future.