This lab is extremely similar to the previous one. This lab also uses a PID controller to control an aspect of the robot which this time around is the orienatation of the robot. Since this lab is so simialr to the previous one I used a lot of the same code as in the previous one to handle the robot's reaction to the yaw readings from the gyroscope.
To begin this report I orginally tried to direclty take in the yaw readings then throw that into my PID controller but after VARIOUS attempts of trying to make that work I gave up on that plan. This approach caused my robot to spin violently and broke the wires of the battery that connects to the Artemis. This lead me to search other's webpages within my own year and prvious years to see if there was a way to fix my current appoarch or to use an entirely new one altogether. I was able to find that using the IMU's built-in DMP was a popular and meaningful appoarch so I headed down that route. You can see my first attemot with my new PID controller in the video below:
That discussion lead me to follow an example in the Arudino IDE that used the DMP and gather the setup needed to use it. Below is the code needed for the setup of the DMP:
// Initialize the DMP
success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok);
// Enable the DMP Game Rotation Vector sensor
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);
// Set the DMP output data rate (ODR): value = (DMP running rate / ODR ) - 1
// E.g. for a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to the maximum
// Enable the FIFO queue
success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
// Enable the DMP
success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
// Reset DMP
success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
// Reset FIFO
success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
// Check success
if (!success) {
Serial.println("Enabling DMP failed!");
while (1) {
// Freeze
}
}
NOTE: Before placeing this into your code please pay attention to the note used in the DMP example about uncoomenting out line 29 in the header file. That line allows you to use the DMP and without following that instruction it will never work and allow the code block above to work.
After placing that code into my setup() fucntion I placed all of the code needed to convert the DMP measurements in quaternions into Euler angles so that I can gather the measured angle of the yaw for the robot. Below is the code used for that conversion:
if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) {
if ((data.header & DMP_header_bitmap_Quat6) > 0) {
double qy = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
double qx = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
double qz = -((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
double qw = sqrt(1.0 - ((qy * qy) + (qx * qx) + (qz* qz)));
double t3 = +2.0 * (qw * qz + qx * qy);
double t4 = +1.0 - 2.0 * (qy * qy + qz * qz);
yaw_data = (float)atan2(t3, t4) * 180.0 / PI;
int error = yaw_setpoint - yaw_data;
From there I used the plogic from lab 5 to create my PID controller but the main difference was that the error was calulated from the measured yaw not distance.
unsigned long current_time = millis();
float dt = (float)(current_time - last_pid_time) / 1000.0; // Convert ms to
last_pid_time = current_time;
if (dt > 0) {
accumulated_error += error * dt;
}
// Calculate P and I terms
float p_term = Kp * error;
float error_rate = 0.0;
if (dt > 0) {
error_rate = (error - prev_error) / dt;
}
float d_term = Kd * error_rate;
float i_term = Ki * accumulated_error;
if (i_term > 200) {
i_term = 200;
} else if (i_term < -200) {
i_term = -200;
}
float pwm = p_term + d_term + i_term;
prev_error = error;
if (pwm > 0 && pwm > 255) {
pwm = 255;
} else if (pwm < -255) {
pwm = -255;
}
// time_data[i] = (int) millis();
int e = yaw_setpoint-yaw_data;
spin_control(pwm);
I added in a new function spin_control and new variable pwm from Kotey's and Akin's webpages since they're code was easier to read and was generally more effiecnt than what I was making.
After many trials I was able ot gather these values for each term in the PID controller:
Unfornately graphs were not able to be gathered in time but from the data I gathered from multiple runs and the behavior of the robot you can see clear differences in the way the robot reacted to needing to turn to the apporiate yaw. The first version of the robot was clearly had oscillations in turning left and right and when the robot moved completely out of the range it caused fast spins. When taking the dmp into account more accurate and reliable yaw data so that the robot could better react to the changing direction to the correct angle and orienatation.
The PID controller successfully stabilized orientation with minimal overshoot and steady-state error. Future improvements could include real-time setpoint adjustments during movement.