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

Made Drive PID tuning re-read Dashboard whenever the robot is back at startPoint.

Browse Source
This commit is contained in:
Titan Robotics Club
2025-04-26 19:57:22 -07:00
parent 088ea7cde1
commit 33ea6d16cf
1 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -248,18 +248,6 @@ public class FtcTest extends FtcTeleOp
TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false);
} }
break; break;
case TUNE_DRIVE_PID:
if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{
robot.robotDrive.driveBase.resetOdometry();
tuneDriveStartPoint = robot.robotDrive.driveBase.getFieldPosition();
tuneDriveEndPoint = tuneDriveStartPoint.addRelativePose(
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
Dashboard.Drive.turnTarget));
tuneDriveAtEndPoint = false;
}
break;
} }
} //startMode } //startMode
@ -508,10 +496,21 @@ public class FtcTest extends FtcTeleOp
switch (button) switch (button)
{ {
case A: case A:
if (robot.robotDrive != null && testChoices.test == Test.TUNE_DRIVE_PID) if (testChoices.test == Test.TUNE_DRIVE_PID)
{
if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{ {
if (pressed) if (pressed)
{ {
if (!tuneDriveAtEndPoint)
{
robot.robotDrive.driveBase.resetOdometry();
tuneDriveStartPoint = robot.robotDrive.driveBase.getFieldPosition();
tuneDriveEndPoint = tuneDriveStartPoint.addRelativePose(
new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
Dashboard.Drive.turnTarget));
tuneDriveAtEndPoint = false;
}
robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients(Dashboard.Drive.xPidCoeffs); robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients(Dashboard.Drive.xPidCoeffs);
robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients(Dashboard.Drive.yPidCoeffs); robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients(Dashboard.Drive.yPidCoeffs);
robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.turnPidCoeffs); robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.turnPidCoeffs);
@ -527,6 +526,7 @@ public class FtcTest extends FtcTeleOp
} }
passToTeleOp = false; passToTeleOp = false;
} }
}
else if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING) else if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING)
{ {
if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)