SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 04:51:23 -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 TrcOpenCvColorBlobPipeline.FilterContourParams filterContourParams = null;
public static long exposure = 0;
} //class Vision
@Config
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);
public static double maxVelocity = 0.0;
public static double maxAcceleration = 0.0;
@ -92,6 +95,7 @@ public class Dashboard
public static double yTarget = 0.0;
public static double turnTarget = 0.0;
public static double drivePower = 1.0;
public static double turnPower = 1.0;
public static double driveTime = 0.0;
} //class Drive

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

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