Lab 6: Orientation PID

PRE-LAB

IMPROVING LAB 5: To start this lab, I ran my Lab 5 code multiple times and perfected it. During this debugging process, because I had changed my code from what I had initially, my calibration factor for the wheels ended up decreasing from 1.8 to 1.6. And when I ran my PID, the final control values that work best were: Kp = 0.08, Ki = 0.00001, Kd = 0.8. My final Lab 5 code is:

Lab 5 code

BLUETOOTH: To improve the way data is collected and sent to the computer, I created a class called MotorControls, which, as the name suggests, is responsible for controlling the motors. This class essentially defines the pins and, depending on the input speed and time, operates the motors accordingly. The code for MotorControl.h is:

MotorsControl.h Part 1

MotorsControl.h Part 2

Besides this I added a SEND_ORIENTATION command case to the sketch, to be able to send the information to the computer:

My notification handler remained the same as last labs with just a few change in the names, and the way I call the commands is very similar too

TASKS

PID:

Since in Lab 5, my PI controller was the most inaccurate and the most prone to act unexpectedly I decided to use a PID controller. I created a similar function to pos_control for Lab 5, but in this case it is called turn_yaw() which calculates the P, I and D terms according to a desired turn angle and the actual yaw of the robot at the moment. This function is implemented here:

bool turn_yaw(float goal_yaw){
  float yaw_error = goal_yaw - z_angle;
  float i_control = 0;
  bool status = false;
  float dt = millis() - last_time; 

  float p_control = K_p * yaw_error;
  
  if (yaw_error > 0) {
    sumError = sumError + (yaw_error * dt); 
    i_control = K_i * sumError;
    if (i_control > 100) i_control = 100;
    else if (i_control < -100) i_control = -100;
  }
  else i_control = 0;

  float d_control = K_d * (yaw_error - last_error) / dt;
  last_error = yaw_error; 
  last_time = millis();

  float speed = p_control + i_control + d_control;

  if (speed > 0){
    motors.turn_right(speed,1000);
    status = true;
  }
  else {
    motors.turn_left(abs(speed),1000);
    status = true;
  }
  return status; //Commands were completed or not
}
                    

I then included my turn_yaw() function in a command case I made to see if it could go straight for a second, do a 180 turn and come back to where it started:

This code was succesful (with Kp = 0.08, Ki = 0.01, Kd = 0.9), however I did notice that when my minimum speed was set at 40 pwm, the car barely moved, which led me to think that there is too much traction and should consider taping the wheels. I also took a look at my wiring, just in case, and the ground pin to one of the motor drivers is loose from the Arduino which would affect the overall performance of the car.

I then moved on to implementing the code that would re-orient the car when disoriented. I changed my command case to:

While I was fixing my data for this new code, the yellow light on my Artemis turned on and I tried to manually reboot it multiple times but the light wouldn' even blink and it was no longer working the Bluetooth and my computer would not recognize it even when I restarted my computer. This is what it looked like:

SAMPLING TIME:

Before my Arduino crashed I was able to get the sampling frequency of my code, I got a value of around 52 Hz. This means the system updates every 20 ms give or take.

5000+ Wind Up:

I did however add the wind-up in my code when the Arduino was still fully functional. From experience doing Lab 5, if I did not implement the wind up protection, the oscillations would just start getting larger and larger without stopping unless I shut it down via code. After, a few minor mishaps I started implementing wind up protection everytime I run the I-controller. This is where my wind up is currently at:

REFERENCES:

For this lab I referenced, Stephan Wagner's and Krithik Ranjan's labs from previous years. I also used ChatGPT to help with the grammar.