Topics Covered
Viper Drive with NO error correction
private void viperDrive(double maxSpeed, int inches, double timeoutS) {
int targetPosition;
// Ensure that the OpMode is still active
if (opModeIsActive()) {
// Calculate the target position
targetPosition = viper.getCurrentPosition() + (int)(inches * COUNTS_PER_INCH);
viper.setTargetPosition(targetPosition);
// Turn On RUN_TO_POSITION
viper.setMode(DcMotor.RunMode.RUN_TO_POSITION);
runtime.reset();
// Set motor power
viper.setPower(maxSpeed);
// Loop until timeout or position reached
while (opModeIsActive() && (runtime.seconds() < timeoutS) && (viper.isBusy())) {
// Display information for the driver
telemetry.addData("Viper Target", targetPosition);
telemetry.addData("Current Position", viper.getCurrentPosition());
telemetry.addData("Power", maxSpeed);
telemetry.update();
}
// Stop all motion
viper.setPower(0);
}
}Implement Proportional (P) Controller
private void viperDrive(double maxSpeed, int inches, double timeoutS) {
int targetPosition;
double Kp = 0.1; // Proportional constant (adjust based on system behavior)
double power;
double error;
// Ensure that the OpMode is still active
if (opModeIsActive()) {
// Calculate the target position
targetPosition = viper.getCurrentPosition() + (int)(inches * COUNTS_PER_INCH);
viper.setTargetPosition(targetPosition);
// Turn On RUN_TO_POSITION
viper.setMode(DcMotor.RunMode.RUN_TO_POSITION);
runtime.reset();
// Loop until timeout or position reached
while (opModeIsActive() && (runtime.seconds() < timeoutS) && (viper.isBusy())) {
// Calculate the error
error = targetPosition - viper.getCurrentPosition();
// Calculate proportional control power
power = Kp * error;
// Limit power to the maximum speed
power = Math.max(-Math.abs(maxSpeed), Math.min(Math.abs(maxSpeed), power));
// Set motor power
viper.setPower(power);
// Display information for the driver
telemetry.addData("Viper Target", targetPosition);
telemetry.addData("Current Position", viper.getCurrentPosition());
telemetry.addData("Error", error);
telemetry.addData("Power", power);
telemetry.update();
}
// Stop all motion
viper.setPower(0);
}
}Tune the Proportional Controller (Kp)
1. Understand the System
Baseline Behavior: Run the actuator with a small, conservative Kp value (e.g., 0.1) and observe its response.
Desired Outcome:Â The actuator should move smoothly to the target position without overshooting, oscillating, or stopping too early.
2. Initial Setup
Set a Safe Maximum Power:Â Use a lower maximum power initially to prevent damaging the hardware.
Define Test Scenarios:Â Use different target positions and speeds (e.g., short distances, long distances).
Enable Real-Time Monitoring:Â Ensure telemetry or debugging information displays relevant data such as position, error, and power.
3. Increase Kp​ Gradually
Start Small: Begin with a small Kp​ (e.g., 0.1 or smaller) and gradually increase it in small increments (e.g., 0.05).
Observe the Motion: Each time you change Kp​, test the actuator:
Does it reach the target smoothly?
Is there noticeable overshoot or oscillation?
Does the actuator stop at the target position, or does it stop too early?
4. Evaluate Performance
Underdamped Response (Low Kp​):
Actuator moves too slowly or never fully reaches the target.
Increase Kp to make the system respond more aggressively.
Overdamped Response (High Kp):
Actuator moves quickly but overshoots or oscillates around the target.
Decrease Kp​ to reduce overshoot.
Critical Damping (Optimal Kp):
Actuator moves quickly and smoothly to the target without overshooting or oscillating.
5. Fine-Tune Kp​ & Test Edge Cases
Once Kp​ produces a stable and accurate response, test with various target positions and loads to ensure consistency.
Adjust Kp​ if necessary for specific use cases, such as when the actuator is under different loads.
Test with very short and very long distances to ensure Kp​ handles both cases effectively.
Simulate real-world scenarios, such as varying the actuator load or introducing disturbances, to verify robustness.
6. Log Results & Iterate
Record the error, response time, and behavior for different Kp​ values.
Plot error vs. time if possible to visualize the system's response and help identify the best Kp.
Tuning is an iterative process. Repeat the steps until the actuator's response is smooth, accurate, and stable across all expected operating conditions.
Implement Proportional-Derivative (PD) Controller
private void viperDrive(double maxSpeed, int inches, double timeoutS) {
int targetPosition;
double Kp = 0.1; // Proportional gain (tune this value)
double Kd = 0.01; // Derivative gain (tune this value)
double previousError = 0; // Store the error from the previous iteration
double power;
double error;
double derivative;
// Ensure that the OpMode is still active
if (opModeIsActive()) {
// Calculate the target position
targetPosition = viper.getCurrentPosition() + (int)(inches * COUNTS_PER_INCH);
viper.setTargetPosition(targetPosition);
// Turn On RUN_TO_POSITION
viper.setMode(DcMotor.RunMode.RUN_TO_POSITION);
runtime.reset();
// Loop until timeout or position reached
while (opModeIsActive() && (runtime.seconds() < timeoutS) && (viper.isBusy())) {
// Calculate the error
error = targetPosition - viper.getCurrentPosition();
// Calculate the derivative (rate of change of error)
derivative = error - previousError;
// Compute control power
power = (Kp * error) + (Kd * derivative);
// Limit power to the maximum speed
power = Math.max(-Math.abs(maxSpeed), Math.min(Math.abs(maxSpeed), power));
// Set motor power
viper.setPower(power);
// Update the previous error for the next loop
previousError = error;
// Display information for the driver
telemetry.addData("Viper Target", targetPosition);
telemetry.addData("Current Position", viper.getCurrentPosition());
telemetry.addData("Error", error);
telemetry.addData("Derivative", derivative);
telemetry.addData("Power", power);
telemetry.update();
}
// Stop all motion
viper.setPower(0);
}
}How the PD Controller Works in the Code
Proportional Term (P):
P Term = Kp × Error
Drives the motor power proportionally to how far the actuator is from the target position.
Derivative Term (D):
D Term = Kd × (Error − Previous Error)
Predicts how quickly the error is changing to reduce overshoot and oscillation.
Power Calculation:
The motor power is determined as the sum of the proportional and derivative terms: Power = (Kp × Error) + (Kd × Derivative)
Feedback Update:
After each iteration, the current error is saved as previousError to calculate the derivative in the next loop.
Tune Kp​ and Kd​:
Tune Kp​ First:
Start with Kp​ only (set Kd=0).
Increase Kp​ until the system responds quickly but doesn’t oscillate excessively or overshoot significantly.
Add Kd​ for Damping:
Gradually increase Kd​ to reduce overshooting and oscillations.
Avoid setting Kd too high, as it can cause sluggish response or amplify noise.
Test and Refine:
Test the system under different conditions (e.g., various distances, loads).
Adjust Kp and Kd​ iteratively for optimal performance.
Implement Proportional-Integral-Derivative (PID) Controller
private void viperDrive(double maxSpeed, int inches, double timeoutS) {
int targetPosition;
double Kp = 0.1; // Proportional gain (tune this value)
double Ki = 0.01; // Integral gain (tune this value)
double Kd = 0.01; // Derivative gain (tune this value)
double previousError = 0; // Store the error from the previous iteration
double integralSum = 0; // Accumulate the error over time
double power;
double error;
double derivative;
// Ensure that the OpMode is still active
if (opModeIsActive()) {
// Calculate the target position
targetPosition = viper.getCurrentPosition() + (int)(inches * COUNTS_PER_INCH);
viper.setTargetPosition(targetPosition);
// Turn On RUN_TO_POSITION
viper.setMode(DcMotor.RunMode.RUN_TO_POSITION);
runtime.reset();
// Loop until timeout or position reached
while (opModeIsActive() && (runtime.seconds() < timeoutS) && (viper.isBusy())) {
// Calculate the error
error = targetPosition - viper.getCurrentPosition();
// Accumulate the error for the integral term
integralSum += error;
// Calculate the derivative (rate of change of error)
derivative = error - previousError;
// Compute control power
power = (Kp * error) + (Ki * integralSum) + (Kd * derivative);
// Limit power to the maximum speed
power = Math.max(-Math.abs(maxSpeed), Math.min(Math.abs(maxSpeed), power));
// Set motor power
viper.setPower(power);
// Update the previous error for the next loop
previousError = error;
// Display information for the driver
telemetry.addData("Viper Target", targetPosition);
telemetry.addData("Current Position", viper.getCurrentPosition());
telemetry.addData("Error", error);
telemetry.addData("Integral Sum", integralSum);
telemetry.addData("Derivative", derivative);
telemetry.addData("Power", power);
telemetry.update();
}
// Stop all motion
viper.setPower(0);
}
}
How the PID Controller Works in the Code
Proportional Term (P):
Directly drives the motor power based on the current error: P Term=Kp×Error
Integral Term (I):
Accumulates the error over time to address steady-state error: I Term=Ki×Integral Sum
The integral sum grows over time if the system has a persistent error, ensuring it gets corrected.
Derivative Term (D):
Predicts future error based on the rate of change: D Term=Kd×(Error−Previous Error)
Power Calculation:
The motor power is the sum of the three terms: Power=(PÂ Term)+(IÂ Term)+(DÂ Term)
Feedback Updates:
Integral term (integralSum) accumulates over time.
Derivative term (derivative) is calculated using the current and previous error.
Previous error is updated at each iteration.
Tune KpK_pKp​, KiK_iKi​, and KdK_dKd​:
Start with P Control:
Set Ki=0 and Kd=0. Adjust Kp until the system responds quickly and with minimal oscillation.
Add Integral (PI Control):
Introduce Ki​ to address any steady-state error.
Gradually increase Ki​ to reduce the error but avoid excessive overshooting or instability.
Add Derivative (PID Control):
Add Kd​ to reduce overshoot and oscillations.
Increase Kd​ incrementally to smooth the response.
Iterate and Test:
Test the system under various conditions, such as different distances and loads.
Fine-tune Kp​, Ki​, and Kd​ for optimal performance.
Benefits of PID Control
Precise Control: Handles steady-state error (I), overshoot (D), and overall responsiveness (P).
Robustness: Works effectively under varying loads and target positions.
Scalability: Easily adaptable to more complex systems by adjusting gains.
Tips for Successful Tuning
Use a Graphical Debug Tool:Â If possible, plot the error over time to visually analyze the system response.
Watch for Instability: If the actuator oscillates wildly or vibrates, Kp​ is too high—reduce it immediately.
Consider System Lag: If the actuator is slow to respond even with higher Kp​, there might be mechanical or electrical lag in the system, requiring adjustments or a different control strategy (e.g., adding derivative control).
Avoid Overloading the Actuator: Ensure the motor doesn’t stall or overheat during testing, as this could damage the hardware.
By systematically adjusting Kp, Kd and Ki​ and observing the results, you’ll find the optimal values that achieves the best balance of speed, accuracy, and stability for your actuator.