High Stakes
C++
PID
Agenda
Visual Studio Code
C++
PID
PID Example
Setting Up
Visual Studio Code
Visual Studio Code
Go to https://code.visualstudio.com/.
Install for your system.
MacOS
Windows
Extensions
Extensions Button
VEX Extension
Type
"VEX Robotics"
Click Install
C++ Extension
Type
"C/C++"
Click Install
Extensions
You should have 3 extensions installed
VEX Extension
1. Click VEX Extension Button
2. Click "New Project"
Creating Project
Click "V5"
Click "C++"
Creating Project
Click "V5 Competition Template Project"
Creating Project
Click "Create"
Transferring Code
to C++
Motor Setup
Go to Line 17
Drivetrain
Clamp
Intake
Motor Code
Add the controller.
Driver Control
usercontrol()
Note: default code is shown here. We will be adding 2 lines of code.
Driver Control
This line will use the joystick values from the controller to move the drivetrain.
Replaces:
Driver Control
This line sets the intake direction to 0 (stopped) because the intake automatically stops.
Replaces:
Main
main()
Note: default code is shown here. We will be changing it.
Intake Forward (L1)
Note: this code can be put anywhere before the main() function.
Challenge: code button L2 to spin the intake reverse.
Clamp Forward (R1)
Note: this code can be put anywhere before the main() function.
Challenge: code button R2 to move the clamp reverse.
Register Buttons
Writing the functions will not do anything on their own.
You must register the functions to each button on the controller.
Autonomous
autonomous()
Note: default code is shown here. We will be changing it.
Autonomous
Example Auton
C++ Resources
Resources:
https://www.learncpp.com/ - Learn C++
https://api.vexcode.cloud/v5/ - VEXcode API Documentation
A teacher or course is highly recommended if you would like to understand C++ in more detail.
Benefits of C++
PID
Autonomous Accuracy
With default VEX drive functions, robot will:
Results:
Solution
What is a solution to this problem?
Initial
Point
What is a Target?
Target Point
0
5
1
2
3
4
The target is where you want your robot to be.
Example: you want your robot to drive forwards 5 inches. Your target is 5 inches.
Example: you want your robot to turn 90 degrees and you're at 30 degrees. Your target is 120 degrees.
Initial
Point
Target Point
0
10
2
4
6
8
Initial
Point
What is Error?
Target Point
0
5
1
2
3
4
Target Point
0
5
1
2
3
4
Error: High
Error: Low
Error decreases as the robot gets to its target.
Initial
Point
Initial
Point
How to Use Error
Target Point
0
5
1
2
3
4
Target Point
0
5
1
2
3
4
Initial
Point
Error: Low
Movement Speed: Low
Error: High
Movement Speed: High
Decrease the movement speed as the robot reaches its target.
PID
PID stands for Proportional, Integral, Derivative.
Each value (P, I, and D) will have a constant that will be tuned to create an optimal PID system for your robot specifically.
Proportional: current error
Integral: sum of previous errors
Derivative: difference of previous error and current error
All of these values will be multiplied by the constants you have tuned, added together, and put onto a mechanism you have specified.
Proportional
The proportional constant is multiplied by the error.
Robot is 12 inches away, proportional constant is 0.05, so we put 0.6 (or 60%) speed on the drivetrain.
Robot is 6 inches away, proportional constant is 0.05, so we put 0.3 (or 30%) speed on the drivetrain.
speed
(percent)
error
(inches)
Issue
What may be an issue with only using the proportional value?
Integral
The integral is how much error has added up over time.
Integral = (integral constant * current error * time elapsed since last measurement) + previous integral value.
time
error
Integral
Example (shown in image on right):
If you combined the area of all the boxes on the right, you would get the integral.
After one second your robot is 11 inches away from the target.
Multiply the current error (11 in) with the time elapsed since last measurement (1 sec).
Add it to the past error, which is currently 0.
0 + 11*1 = 11.
Our integral constant is 0.001, so we add 0.011 (or 1.1%) speed to the drivetrain.
time
error
Integral
Example (shown in image on right):
If you combined the area of all the boxes on the right, you would get the integral.
After two seconds your robot is 9.5 inches away from the target.
Multiply the current error (9.5 in) with the time elapsed since last measurement (1 sec).
Add it to the past error, which is 11.
11 + 9.5*1 = 20.5.
Our integral constant is 0.001, so we add 0.0205 (or 2.05%) speed to the drivetrain.
time
error
Integral
If you combined the area of all the boxes on the right, you would get the integral.
Continue the steps until you've reached your target.
In this example, when the robot is 2 inches away from target, the integral would be around 40, meaning that 4% speed would be put onto the drivetrain from the integral.
time
error
Why use Integral?
A common error with only using the proportional value is known as steady state error.
When you get really close to your target, the robot will move very slowly - too slowly.
For example, when you are 0.5 inches away, the speed put onto the drivetrain with proportional would be 2.5% (0.5 * 0.05 = 0.025).
2.5% may not be physically enough to move the robot.
The integral adds more speed to the drivetrain, which prevents the slow speeds at the end of the movement when just using the proportional.
time
error
Derivative
The derivative is the difference between the last error and current error.
The derivative constant is multiplied by the derivative.
Ex. your robot was 12 inches away, and now it is 11 inches away.
The derivative is -1 because the error has reduced by 1 inch.
Derivative constant is 0.1, so we reduce the speed to the drivetrain by 0.1 (or 10%).
time
error
Purpose
Each constant has a specific purpose:
Proportional:
- Based on current error
- Is majority of output until close to target
Integral:
- Based on sum of previous and current error
- Speeds up movement
- Reduces steady state error
Derivative:
- Based on how much the error is changing
- Slows down movement
Stop Condition
How does your robot know when to stop and continue to the next movement?
Oscillation
When a mechanism goes farther than its target, then comes back too much (trying to correct itself), and continues repeating this until either
1) it reaches the target or
2) it goes forever because it never reaches its target (overshoots or undershoots the point).
Stop Condition
You must set a threshold, a certain error when the robot will stop. Otherwise, the robot may oscillate or take a while to reach its target point.
Example: set stop condition to ΒΌ inch. Once the robot is a fourth of an inch away from the target, it will stop and the next movement will be run. Example shown below.
Initial
Point
Target Point
0
5
1
2
3
4
PID Example
Inertial Sensor
For the example, we'll use the inertial sensor.
Inertial sensor measures absolute heading:
Instead of turning 90 degrees, you turn to 90 degrees.
Instead of turning another 90 degrees, you turn to 180 degrees.
What are the advantages of absolute heading?
PID Example (part 1)
This example is not actual code. It goes through the instructions you will have to perform for PID. You will then code that inside whichever language you are using to code the robot.
proportionalConstant = 0
integralConstant = 0
derivativeConstant = 0
targetHeading = 90 degrees
stopThreshold = 1 degree
delayTime = 10 milliseconds
totalError = 0 degrees
currentHeading = get_inertial_heading()
previousHeading = currentHeading
currentError = targetHeading - currentHeading
1. Create and tune P, I, and D constants
2. Create target, threshold, delay variables to determine how the PID loop should run
3. Create variables needed for integral, proportional, and derivative calculations
PID Example (part 2)
This example is not actual code.
while (currentError > stopThreshold):
previousHeading = currentHeading
currentHeading = get_inertial_heading()
currentError = targetHeading - currentHeading
totalError = totalError + (currentError * delayTime)
proportional = proportionalConstant * currentError
integral = integralConstant * totalError
derivative = derivativeConstant * (currentHeading - previousHeading)
drivetrainSpeed = proportional + integral + derivative
put_drivetrain_speed_onto_drivetrain(drivetrainSpeed)
wait(delayTime, msec)
4. Start loop which ends when the error is within threshold
5. Measure all the necessary variables
6. Calculate proportional, integral, and derivative using methods shown earlier in the slideshow
7. Add P, I, and D together to get the drivetrain speed, and put it on the drivetrain
8. Wait your delay time (specify milliseconds)
Tuning PID
There are many methods for tuning PID.
Here are some Vex Forum threads which may help in optimizing your PID constants:
https://www.vexforum.com/t/how-long-does-it-take-to-tune-pid-for-a-beginner/74950
https://www.vexforum.com/t/pid-tuning-turning/88356
This is the hardest part of a PID loop.
Balance between accuracy and speed when tuning PID.