The purpose of this lab is to gain experience with PID control. We were given an option to choose between three tasks to be implemented over a series of labs (labs 6-8) on PIS control, sensor fusion and stunts. I chose to work on Task B - Drift Much? The final stunt for this task involves driving the robot fast forward towards a wall, then turn with a drift to do a 180 degree turn and return back in the same direction. For this lab, we had to implement PID control on the orientation of the robot by introducing a differece in motor speeds in order to control the 180 degree turn.

I collaborated with Krithik Ranjan (kr397) and Aparajito Saha (as2537) on this lab. I used my own robot and wrote my own code, but I wanted to mentioned them as we we discussed implementation details.

Adding Support for Debugging

The robot will implement the task completely on the Artemis, but it would be helpful to see the data (time stamp, yaw, error, motor output) it is using to implement the PID control in order to tune the PID variables. We can not print this data over serial as the robot can not be tethered during the run. We can get this data over Bluetooth. I setup system for debugging using a Jupyter Notebook. The Jupyter Notebook was used for sending commands to the Artemis, collect data and analyze the data collected.

I used the bluetooth communication code from Lab 2 and added new commands. I created a START_TASK command, when when sent, the robot implements the actual task. By using this I did not have keep reset the robot every time I wanted to implement the task; it also gave me buffer time for recording videos as I could run the task whenever needed instead having it run a certain time after starting the robot. I added the SET_PID_VAR command to set new PID gains (Kp, Ki, Kd) and the SET_SETPOINT command to change the set point. The data was sent to the Artemis as a string, using the "|" character to separate different values. These commands proved to be very helpful as I did not have to reflash the Artemis with the new PID gains and setpoint while tuning the robot.

To collect the PID data I added a GET_DATA command which returns the PID data as a series of strings (EString on the Artemis side). Each string is the PID data for a single iteration of the PID loop (explained below). The single string consists of timestamp, yaw, error, pidOutput and motor speed separated by the "|" variable. I chose to send data as an Estring instead of float, as we can send multiple values at the same time. On the python side, I used start notifications along with a call back function to store the data in a list. The bluetooth notifications are turned on, the command is sent to the Artemis, the notifications are turned stopped after waiting for 120 seconds. I consolidated all this logic inside the get_data function. I chose to turn on the notifications only for a certain time to ensure that if any data is sent by the Artemis by mistake, it is not stored as debug data. I also set up a new bluetooth uuid to send over PID data.

Closed Loop Control

For this lab, we had to setup basic pid control on the Artemis. I resued code from Artemis code from Lab 2 for bluetooth, Lab 3 for IMU setup & data collection and Lab 5 for motion control. I first set up the bluetooth on the Artemis side for the commands described above and tested it with hardcoded data to ensure that the commands were working. Next I setup the IMU and added functions for recording the yaw using the gyroscope intergal method. I defined a pidDataType data type to store PID data as defined below. I created a 1000 length C array pidData containing values of this data type to store the data used during PID control. As this array was reset during at the beginning of the task, the data never overflowed. Before starting the lab I was anticipating a lot of issues with data overflow in this array and null pointer errors, but I did not face any such difficulties.

PID

PID control stands for Proportional, Intergral Derivative Control and is commonly used in control system in robotics for closed loop control. In our case, we will use PID control on the orientation of the robot to turn the robot 180 degrees. Given a goal orientation (180), the PID controller will use the feedback from the IMU gryroscope (yaw angle) and output the PWM duty cycle for the motor inputs to turn the robot inorder to achieve the goal. The output speed given by the PID control is provided to the motors and the process is repeated till the goal is achieved. As the PID controller uses the current error as well as the integral and derivative of previous errors, we need to store the data from previous iterations of the PID controller. This is stored in the pidData array described above.

As seen in Lab 5, the robot has a deadband PWM duty cycle below which it does not move. The robot moves very fast at high PWM duty cycles and IMU will not be able to get yaw in time for the PID control to produce a new output to achieve the goal orientation. To avoid overshooting and to ensure that the robot rotates in a controlled manner, we need to put an upper bound on the duty cycle as well. I started with 120 and 160 as the limit for the robot motor input for the PID controller. However, as the robot battery discharged, I has to increase the bounds to 160, 200. The output provided by the pid controller is set as one input of the motor driver and the other input is set to 0. The motor input is calibrated to account for the fact that one set of wheels move faster than other (Lab 5). To turn in place, one set of wheels rotate clockwise and other turns anticlockwise. Sometimes the PID control will provide a negative output, when we have already passed the goal. When the pid output is negative, the wheels rotate in opposite directions so that the robot reverses its motion

When the command to start the task is provided, the robot resets the PID data array, calculates the measures yaw angle and sets that 0 degrees and then starts the pid control loop to turn to the setpoint. I started implementing the PID control with just the P term. The proportional term outputs the motor speed proportional to the current error. In the beginning the error is high, so is the pid output and the robot turns fast, as it reaches its goal, it slows down. I started with a low proportional gain (Kp < 0.1) and then eventually increased it (Kp = 5) till it reached pid control was satisfactory. I also had to change the gains when the motor speed ranges were changed. With a low Kp constant, the robot stops before reaching its goal, with a higher Kp constant the robot overshoots, reverse turns and oscillates.

Please take a look at Appendix A1 for the code.

First of all, I realized that my IMU yaw angle was not accurate. The part 1 of the video below shows the robot turning 360 degree with the above PID control. With the Kp constant set to 5, I changed the setpoints to find the angle that corresponded to 180 degree. Based on the battery charge, an angle in the range 85 to 100 worked as the setpoint to get the robot to turn 180 degrees. I also tried used two point calibration with the yaw angle, but it was not much help. The second and third part of the video below shows the robot turning 180 degrees and 360 degrees with tuned PID control (P term). Another trick I added was to reset the IMU at the beginning of the task to ensure that the IMU drift does not affect the yaw angle as much.

As seen in the video above the PID control works quite well, but I wanted to incorporate the I and D terms as well. The derivative prevents overshooting the target by reducing the robot speed quicker as it nears the goal. The integral term is based on the accumulated error sum and increases the robot speed. I added each of the terms indivually and tuned them. However, they did not improve the orientation control, and added more variables to tune. I eventually decided to not include them in the final run.

Task Implementation

Lastly, I added movement to the robot. When the START_TASK command is provided, the robot first moves forward for 1 second at a fast speed, then starts the pid control loop to turn 180 degrees once it reaches it goal it moves forward for another 1 second. I had to add a time out for the pid rotation, as the robot keeps oscillating due to imperfect gyroscope readings. I had initially added a more complicated logic, where if the robot osciallates more than 4 times and the error is less than five degrees, it stops the pid control. However, it did not seem to work as well, so I went with the easier solution. I had to put a momentary hard stop to the robot after the robot completed the pid control loop, to ensure that the dirft due to the turning did not affect the forward motion afterwards.

Demonstration

The video below demostrates five trials of the PID control on the robot. I was quite happy with the results. For some runs the robot did not return to the exact starting point due to drift in both the translational.forward motion and drift while rotating. When there was shift of position while turning, the robot returned along a parallel path to the original one. When there was drift in translational motion, the robot would return to the near same position but follow a different line of path.

I am glad that the basic PID control works so well on this robot. I am quite concerned by how much we need to change the setpoint, motor input ranges and pid gains based on the battery voltage. I had to retune these constants every time the battery was changed or the robot was used for a long time. I wonder if this will create problems in later labs. Regardless, this lab was very exciting as it was the first time I saw the robot work autonomously. I look forward to seeing what we can do with closed loop control in the future. Thank you to the course staff for giving us a flexible deadline with this lab.

Appendix

A1 - Code for PID Control

                                    
void pid_loop() 
{
  // Move forward 
  analogWrite(MOTOR_LEFT_1, 255); 
  analogWrite(MOTOR_LEFT_2, 255);
  analogWrite(MOTOR_RIGHT_1, 255); 
  analogWrite(MOTOR_RIGHT_2, 255);
  analogWrite(MOTOR_LEFT_1, 100); 
  analogWrite(MOTOR_LEFT_2, 0);
  analogWrite(MOTOR_RIGHT_1, 95); 
  analogWrite(MOTOR_RIGHT_2, 0);
  delay(1000);

  // Momentary hard stop to avoid drift while rotation
  analogWrite(MOTOR_LEFT_1, 255); 
  analogWrite(MOTOR_LEFT_2, 255);
  analogWrite(MOTOR_RIGHT_1, 255); 
  analogWrite(MOTOR_RIGHT_2, 255);

  // PID control variable initialization
  startPidMillis = millis();
  pidData[0].timestamp = 0;
  pidData[0].yaw = 0;
  prev = millis();
  counter = 1;
  while(millis() - startPidMillis <= interval)
  {
    // Get Current Yaw Value if IMU is ready
    bool sensorDataReady = get_yaw();
    if(sensorDataReady)
    {
      // Calculate PID output
      pidData[counter].error = setpoint - pidData[counter].yaw;
      pidData[counter].pidOutput = Kp * pidData[counter].error;

      // Set motor inputs based on pidOutput
      int motorspeed = (MOTOR_MIN + abs(pidData[counter].pidOutput));
      motorspeed = motorspeed > MOTOR_MAX ? MOTOR_MAX : motorspeed;
      pidData[counter].motorSpeed = motorspeed;

      if(pidData[counter].pidOutput < 0) 
      { 
        // Clockwise
        analogWrite(MOTOR_LEFT_1, 0); 
        analogWrite(MOTOR_LEFT_2, motorspeed);
        analogWrite(MOTOR_RIGHT_1, motorspeed); 
        analogWrite(MOTOR_RIGHT_2, 0);
      } else {
        // Anti-clockwise
        analogWrite(MOTOR_LEFT_1, motorspeed); 
        analogWrite(MOTOR_LEFT_2, 0);
        analogWrite(MOTOR_RIGHT_1, 0); 
        analogWrite(MOTOR_RIGHT_2, motorspeed);
      }
      counter++;
    }
    // Safety for data overflow
    if(counter == PID_DATA_SIZE)
    {
      break;
    }
  }

  // Move forward
  analogWrite(MOTOR_LEFT_1, 100); 
  analogWrite(MOTOR_LEFT_2, 0);
  analogWrite(MOTOR_RIGHT_1, 90); 
  analogWrite(MOTOR_RIGHT_2, 0);
  delay(1000);
  
  // Stop
  analogWrite(MOTOR_LEFT_1, 255); 
  analogWrite(MOTOR_LEFT_2, 255);
  analogWrite(MOTOR_RIGHT_1, 255); 
  analogWrite(MOTOR_RIGHT_2, 255);
 
}