ECE 5160: FAST ROBOTS

Lab 5: Linear PID Control and Linear Interpolation

Wenyi Fu • ECE • wf223@cornell.edu

INTRODUCTION

This lab section is to implement the PID control to the motors according to the distance from the wall. The BLE is used to store the data and debug the system.

PRELAB

Parts Required

  • 1 x R/C stunt car
  • 1 x SparkFun RedBoard Artemis Nano
  • 1 x USB cable
  • 2 x Li-Po 3.7V 650mAh (or more) battery
  • 2 x Dual motor driver
  • 2 x 4m ToF sensor
  • 1 x 9DOF IMU sensor
  • 1 x Qwiic connector

BLE

First, I created these arrays to store the data that needed to be transferred to the laptop for analysis through the Bluetooth.

int pid_time[500];
int pid_tof[500];
int pid_speed[500];
float pid_p[500];
float pid_i[500];
float pid_d[500];

Also, I use these command to start the PID motion control on the robot through BLE:

ble.start_notify(ble.uuid['RX_STRING'], pid_control)
ble.send_command(CMD.PID_POSITION_CONTROL, "0.08|0.005|1|300")

Hence, with the data sent through the channel, it can be received by the laptop and plot in the figure by the following code. The first block is for PID motor input and TOF distance vs time, and the second block is for plotting each PID component, which helps debugging.

plt.plot(time, tof_data,'y')
plt.plot(time, pid_speed_data,'r')
plt.xlabel('Time (ms)')
plt.ylabel('Distance (mm)')
plt.title('Time v Distance')
plt.show()
view raw plot_tof_pid.py hosted with ❤ by GitHub
plt.plot(time, pid_p_data,'r')
plt.plot(time, pid_i_data,'b')
plt.plot(time, pid_d_data,'g')
plt.xlabel('Time (ms)')
plt.ylabel('PID')
plt.title('PID components')
plt.show()

LAB TASKS

P/I/D discussion

As it is stated in the lab section, the PID controller is comprised of three part, propotional, integral and derivative. Therefore, the equation can be represented as:

correction=Kperror+Kiintegral error+Kdderivative error

The proportional gain, Kp, adjusts the current error (the gap between the desired and actual values) in proportion to its magnitude. A higher Kp intensifies the corrective action, yet it may lead to instability or overshooting.

The integral gain, Ki, addresses accumulated past errors. It amplifies the corrective action when the error persists over time, but excessive Ki values can result in overshooting or weaving due to the gradual reduction of accumulated errors.

The derivative gain, Kd, anticipates future errors by assessing the rate of error change. It serves as a damping mechanism to minimize overshooting and moderate the speed of corrective actions, although it may also amplify noise.

Range/Sampling time discussion

To make sure that there will be valid data for each loop of the TOF distance reading, the following line is put inside the function pid_position_tof(), but it will delay a lot of the sampling time and make the system unstable. Consequently, I delete those lines. However, if it is deleted, the data might not be valid due to the low frequency of the TOF sensor reading, so there is a problem that will be fully illustrated in next section.

while (!distanceSensor1.checkForDataReady()) {
delay(1);
}
view raw data_ready.c hosted with ❤ by GitHub

Implementation

To implement this, the case PID_POSITION_CONTROL is created to start that recording function. After recording certain amount of the PID sample data, which is 500 in this situation, it will begin to send data through BLE.

case PID_POSITION_CONTROL:
start_time = millis();
// initialization for Kp, Ki, Kd, Target distance
success = robot_cmd.get_next_value(Kp);
if (!success) return;
success = robot_cmd.get_next_value(Ki);
if (!success) return;
success = robot_cmd.get_next_value(Kd);
if (!success) return;
success = robot_cmd.get_next_value(target_tof);
if (!success) return;
tx_estring_value.clear();
// Recording the data
for (int i = 0; i < pid_data_num; i++) {
distanceSensor2.startRanging();
pid_time[i] = millis() - start_time;
pid_tof[i] = distanceSensor2.getDistance();
distanceSensor2.clearInterrupt();
distanceSensor2.stopRanging();
pid_speed[i] = pid_position_tof(Kp, Ki, Kd, pid_tof[i], target_tof);
pid_p[i] = Kp * err;
pid_i[i] = Ki * integral_err;
pid_d[i] = Kd * err_d;
}
stop(); // Stop the motor and begin to send data
// Data for time
tx_estring_value.clear();
tx_estring_value.append("T");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_time[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
// Data for distance
tx_estring_value.clear();
tx_estring_value.append("S");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_tof[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
// Data for pid signal
tx_estring_value.clear();
tx_estring_value.append("C");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_speed[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
// Data for pid p propotion
tx_estring_value.clear();
tx_estring_value.append("P");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_p[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
// Data for pid i propotion
tx_estring_value.clear();
tx_estring_value.append("I");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_i[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
// Data for pid d propotion
tx_estring_value.clear();
tx_estring_value.append("D");
tx_characteristic_string.writeValue(tx_estring_value.c_str());
for (int i = 0; i < pid_data_num; i ++) {
tx_estring_value.clear();
tx_estring_value.append(pid_d[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
break;

Where the funtion pid_position_tof() for calculation the motor control signals is shown below:

int pid_position_tof (float Kp, float Ki, float Kd, float current_pos, float target_pos) {
current_time = millis();
dt = current_time - prev_time; // in ms
prev_time = current_time;
err = current_pos - target_pos;
err_d = (err - prev_err) / dt;
// Wind-up protection
if (abs(err)<0.01) {
integral_err = 0;
}
else {
integral_err = integral_err + (err * dt);
}
if (integral_err > 200) {
integral_err = 200;
}
else if (integral_err < -200) {
integral_err = -200;
}
// Calculate speed control signal
speed_control = (int)(Kp * err + Ki * integral_err + Kd * err_d);
if (speed_control > max_speed) {
speed_control = max_speed;
}
if (speed_control < (-1 * max_speed)) {
speed_control = -1 * max_speed;
}
if (speed_control > 0) {
forward(speed_control);
}
else if (speed_control < 0) {
backward(-1 * speed_control);
}
prev_err = err;
return speed_control;
}
void forward(int speed) {
analogWrite(AB1IN_LEFT,speed*1.8);
analogWrite(AB2IN_LEFT,0);
analogWrite(AB1IN_RIGHT,speed);
analogWrite(AB2IN_RIGHT,0);
}
void backward(int speed) {
analogWrite(AB1IN_LEFT,0);
analogWrite(AB2IN_LEFT,speed*1.8);
analogWrite(AB1IN_RIGHT,0);
analogWrite(AB2IN_RIGHT,speed);
}
void stop() {
analogWrite(AB1IN_LEFT,0);
analogWrite(AB2IN_LEFT,0);
analogWrite(AB1IN_RIGHT,0);
analogWrite(AB2IN_RIGHT,0);
}

Therefore, with the notification handler, the data can be stored in the array in Python through the command ble.send_command(CMD.PID_POSITION_CONTROL, "0.1|0.1|0.5|304"), where the PID parameter and the target distance can be modified by changing the numbers in"0.1|0.1|0.5|304".

import matplotlib.pyplot as plt
import time
time = []
tof_data = []
pid_speed_data = []
pid_p_data = []
pid_i_data = []
pid_d_data = []
STATE = 0
# Kp = 1
# Ki = 0
# Kd = 0
# DIS = 150
def pid_control(uuid, byte_arr):
global time
global tof_data
global pid_speed_data
global pid_p_data
global pid_i_data
global pid_d_data
global STATE
s = ble.bytearray_to_string(byte_arr)
if (s == "T"):
STATE = 1
elif (s == "S"):
STATE = 2
elif (s == "C"):
STATE = 3
elif (s == "P"):
STATE = 4
elif (s == "I"):
STATE = 5
elif (s == "D"):
STATE = 6
if (STATE == 1):
time.append(float(s))
elif (STATE == 2):
tof_data.append(float(s))
elif (STATE == 3):
pid_speed_data.append(float(s))
elif (STATE == 4):
pid_p_data.append(float(s))
elif (STATE == 5):
pid_i_data.append(float(s))
elif (STATE == 6):
pid_d_data.append(float(s))
ble.start_notify(ble.uuid['RX_STRING'], pid_control)
ble.send_command(CMD.PID_POSITION_CONTROL, "0.1|0.1|0.5|304")
view raw pid_plot.py hosted with ❤ by GitHub

Graph data results

The movement of the robot with PID controller is shown in the video, where the car distance from the wall is set to 304mm (1ft).

And when the parameter Kp is increased, the result is shown below:

Additionally, the data plot in the diagrams indicates the process of the PID control, which will be demonstrated as follows

When Kp = 0.05, Ki = 0.02, Kd = 1, the diagram is shown below:

pinout

When Kp = 0.1, Ki = 0.01, Kd = 0.8, the diagram is shown below:

pinout

When Kp = 0.1, Ki = 0.005, Kd = 1, the diagram is shown below:

pinout

When Kp = 0.08, Ki = 0.005, Kd = 1, the diagram is shown below:

pinout

Furthermore, each PID component is also sent to the laptop for analyzing, which is beneficial for debugging.

pinout

From all the figures above, it can be concluded that only with propotional term the oscillating waveform of motor input will be obstained, so the integral and derivative terms should be added as well. Moreover, the D-term controller values shown in figure are discrete, indicating that some of the TOF values are not valid, since the sampling frequency of the sensor is not high enough. Thus, the extrapolation method should be implemented, which will be illustrated in the following section.

Extrapolation

Since the PID loop run time is around 15ms, while the TOF reading is nearly 8ms, the extrapolation should be implemented to ensure the validity of each data in the pid_tof array. Therefore, the function checkForDataReady() can be used for extrapolating data when it is invalid. Also, according to the interpolation equation reprensented:

y=y1+(xx1)(y2y1)x2x1

We can create the program as below:

int val = distanceSensor2.checkForDataReady();
pid_time[i] = millis() - start_time;
if (val == 1) {
pid_tof[i] = distanceSensor2.getDistance();
}
else {
Serial.println("not");
if (i >= 2) {
pid_tof[i] = pid_tof[i-1] + (pid_time[i] - pid_time[i-1]) *
(pid_tof[i-1] - pid_tof[i-2]) / (pid_time[i-1] - pid_time[i-2]);
}
else {
pid_tof[i] = pid_tof[0];
}
}
view raw extrapolation.c hosted with ❤ by GitHub

Wind-up protection

Wind-up protection is an essential feature of PID controller, particularly in systems susceptible to integrator wind-up. Integrator wind-up arises when the integral component accumulates excessively due to prolonged error conditions, resulting in issues like overshooting, instability, or other undesirable system behaviors. Therefore, the following code can be implemented to eliminate the wind-up problem. With this applied, the integral term will not be too large to calculate.

if (abs(err)<0.01) {
integral_err = 0;
}
else {
integral_err = integral_err + (err * dt);
}
if (integral_err > 800) {
integral_err = 800;
}
else if (integral_err < -800) {
integral_err = -800;
}

DISCUSSION

In this lab, we learnt how to implement the PID controller to the motor according to the TOF distance set up. Furthermore, the extrapolation and wind-up protection are demonstrated as well.