SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 13:01:24 -07:00
Code Issues Packages Projects Releases Wiki Activity

Removed exposure tuning support, rawEocv doesn't support it.

Browse Source 
Combined TUNE_X_PID, TUNE_Y_PID and TUNE_TURN_PID to TUNE_DRIVE_PID.
This commit is contained in:
Titan Robotics Club
2025-04-25 22:56:45 -07:00
parent 0869026b76
commit fd28526ee3
2 changed files with 34 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/teamcode/Dashboard.java
View File

@ -77,13 +77,16 @@ public class Dashboard
{ {
public static double[] colorThresholds = null; public static double[] colorThresholds = null;
public static TrcOpenCvColorBlobPipeline.FilterContourParams filterContourParams = null; public static TrcOpenCvColorBlobPipeline.FilterContourParams filterContourParams = null;
public static long exposure = 0;
} //class Vision } //class Vision
@Config @Config
public static class Drive public static class Drive
{ {
public static TrcPidController.PidCoefficients pidCoeffs = public static TrcPidController.PidCoefficients xPidCoeffs =
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
public static TrcPidController.PidCoefficients yPidCoeffs =
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
public static TrcPidController.PidCoefficients turnPidCoeffs =
new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0); new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
public static double maxVelocity = 0.0; public static double maxVelocity = 0.0;
public static double maxAcceleration = 0.0; public static double maxAcceleration = 0.0;
@ -92,6 +95,7 @@ public class Dashboard
public static double yTarget = 0.0; public static double yTarget = 0.0;
public static double turnTarget = 0.0; public static double turnTarget = 0.0;
public static double drivePower = 1.0; public static double drivePower = 1.0;
public static double turnPower = 1.0;
public static double driveTime = 0.0; public static double driveTime = 0.0;
} //class Drive } //class Drive

87
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -68,9 +68,7 @@ public class FtcTest extends FtcTeleOp
Y_TIMED_DRIVE, Y_TIMED_DRIVE,
PID_DRIVE, PID_DRIVE,
PP_DRIVE, PP_DRIVE,
TUNE_X_PID, TUNE_DRIVE_PID,
TUNE_Y_PID,
TUNE_TURN_PID,
CALIBRATE_SWERVE_STEERING CALIBRATE_SWERVE_STEERING
} //enum Test } //enum Test
@ -101,6 +99,10 @@ public class FtcTest extends FtcTeleOp
private double maxTurnVelocity = 0.0; private double maxTurnVelocity = 0.0;
private double prevTime = 0.0; private double prevTime = 0.0;
private double prevVelocity = 0.0; private double prevVelocity = 0.0;
// Tune Drive.
private TrcPose2D tuneStartPoint;
private TrcPose2D tuneEndPoint;
private boolean tuneAtEnd;
// Swerve Steering Calibration. // Swerve Steering Calibration.
private boolean steerCalibrating = false; private boolean steerCalibrating = false;
private boolean teleOpControlEnabled = true; private boolean teleOpControlEnabled = true;
@ -225,8 +227,6 @@ public class FtcTest extends FtcTeleOp
robot.vision.setRawColorBlobVisionEnabled(true); robot.vision.setRawColorBlobVisionEnabled(true);
Dashboard.Vision.colorThresholds = robot.vision.getRawColorBlobThresholds(); Dashboard.Vision.colorThresholds = robot.vision.getRawColorBlobThresholds();
Dashboard.Vision.filterContourParams = Vision.colorBlobFilterContourParams.clone(); Dashboard.Vision.filterContourParams = Vision.colorBlobFilterContourParams.clone();
Dashboard.Vision.exposure =
robot.vision.vision != null? robot.vision.vision.getCurrentExposure(): 0;
} }
break; break;
@ -253,6 +253,17 @@ public class FtcTest extends FtcTeleOp
Dashboard.Drive.turnTarget)); Dashboard.Drive.turnTarget));
} }
break; break;
case TUNE_DRIVE_PID:
if (robot.robotDrive != null)
{
tuneStartPoint = robot.robotDrive.driveBase.getFieldPosition();
tuneEndPoint = tuneStartPoint.addRelativePose(
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
Dashboard.Drive.turnTarget));
tuneAtEnd = false;
}
break;
} }
} //startMode } //startMode
@ -404,9 +415,7 @@ public class FtcTest extends FtcTeleOp
} }
break; break;
case TUNE_X_PID: case TUNE_DRIVE_PID:
case TUNE_Y_PID:
case TUNE_TURN_PID:
if (robot.robotDrive != null) if (robot.robotDrive != null)
{ {
robot.dashboard.putObject( robot.dashboard.putObject(
@ -491,57 +500,19 @@ public class FtcTest extends FtcTeleOp
switch (button) switch (button)
{ {
case A: case A:
if (testChoices.test == Test.TUNE_COLORBLOB_VISION && if (robot.robotDrive != null && testChoices.test == Test.TUNE_DRIVE_PID)
robot.vision != null && robot.vision.rawColorBlobVision != null)
{ {
if (pressed) if (pressed)
{ {
// Update all vision tuning parameters. robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients(Dashboard.Drive.xPidCoeffs);
robot.vision.vision.setManualExposure(Dashboard.Vision.exposure, null); robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients(Dashboard.Drive.yPidCoeffs);
} robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.turnPidCoeffs);
passToTeleOp = false; robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower);
} robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.turnPower);
else if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive.start(
(testChoices.test == Test.TUNE_X_PID || testChoices.test == Test.TUNE_Y_PID || false, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration,
testChoices.test == Test.TUNE_TURN_PID)) Dashboard.Drive.maxDeclearion, tuneAtEnd? tuneStartPoint: tuneEndPoint);
{ tuneAtEnd = !tuneAtEnd;
if (pressed)
{
robot.robotDrive.driveBase.resetOdometry();
switch (testChoices.test)
{
case TUNE_X_PID:
robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients(
Dashboard.Drive.pidCoeffs);
robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower);
robot.robotDrive.purePursuitDrive.start(
true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration,
Dashboard.Drive.maxDeclearion,
new TrcPose2D(Dashboard.Drive.xTarget*12.0, 0.0, 0.0));
Dashboard.Drive.xTarget *= -1.0;
break;
case TUNE_Y_PID:
robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients(
Dashboard.Drive.pidCoeffs);
robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower);
robot.robotDrive.purePursuitDrive.start(
true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration,
Dashboard.Drive.maxDeclearion,
new TrcPose2D(0.0, Dashboard.Drive.yTarget*12.0, 0.0));
Dashboard.Drive.yTarget *= -1.0;
break;
case TUNE_TURN_PID:
robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.pidCoeffs);
robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.drivePower);
robot.robotDrive.purePursuitDrive.start(
true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration,
Dashboard.Drive.maxDeclearion,
new TrcPose2D(0.0, 0.0, Dashboard.Drive.turnTarget));
break;
}
} }
passToTeleOp = false; passToTeleOp = false;
} }
@ -773,9 +744,7 @@ public class FtcTest extends FtcTeleOp
testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false); testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false);
testMenu.addChoice("PID drive", Test.PID_DRIVE, false); testMenu.addChoice("PID drive", Test.PID_DRIVE, false);
testMenu.addChoice("Pure Pursuit Drive", Test.PP_DRIVE, false); testMenu.addChoice("Pure Pursuit Drive", Test.PP_DRIVE, false);
testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false); testMenu.addChoice("Tune X PID", Test.TUNE_DRIVE_PID, false);
testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false);
testMenu.addChoice("Tune Turn PID", Test.TUNE_TURN_PID, false);
testMenu.addChoice("Calibrate Swerve Steering", Test.CALIBRATE_SWERVE_STEERING, false); testMenu.addChoice("Calibrate Swerve Steering", Test.CALIBRATE_SWERVE_STEERING, false);
// //
// Traverse menus. // Traverse menus.