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() |
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:
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); | |
} |
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") |
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:

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

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

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

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

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:
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]; | |
} | |
} |
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.