PD Controller
For accurate stopping at a desired position, I coded the PD thread for the autonomous mode and tuned them by obtaining the Kp and Kd values from a PD auto-tuner I customized using MATLAB and its tutorial for building the circuit for auto-tuning.
​
The program for the auto-tuner can be accessed from my Github repository: https://github.com/c29park/VEX-Robotics-PDAutoTuning
​
The code for the PD loop, function calls, and functions will be available on the "autons.cpp" from the "src" folder of "rushSkills" in my VEX Project Github repository.
​
PD Thread
The following image is a snippet of the main PD controller function code. This function runs in a loop when the boolean variable "enableDrivePD" is true. Suppose the robot needs to travel straight 12 inches on the field. The motors' and the encoders' sensor values will be reset to 0 degrees at the start, and the positions of the tracking wheels and the 4 mecanum wheels' will be updated constantly. Then, we determine the average position by taking the average of all position values. The error is the distance between the robot's current average position and the robot's desired position, in this case 12 inch point from the start. The derivative is the rate of change of the error, and using both derivative and error the motor power in voltage is calculated. Finally, the program spins the drivetrain motors and updates the prevError value.
MATLAB PD AutoTuner
For tuning my PD loop (i.e. determining the correct values for both Kp and Kd), I followed the tutorial of how to make a PID auto tuner to build the following diagram and customized it so that the closed loop PID outputs only the Proportional and Derivative values. I had to run this program individually for all four motors and get the average of Kp's and Kd's values. When the conversion rate is close to 100%, I would read the Kp and Kd values and stop the program immediately.
​
The link for the tutorial is: https://www.mathworks.com/videos/how-to-automatically-tune-pid-controllers-1600850427273.html
