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

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

54
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,24 +496,36 @@ 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 (pressed) if (robot.robotDrive != null && robot.robotDrive.purePursuitDrive != null)
{ {
robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients(Dashboard.Drive.xPidCoeffs); if (pressed)
robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients(Dashboard.Drive.yPidCoeffs); {
robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.turnPidCoeffs); if (!tuneDriveAtEndPoint)
robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower); {
robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.turnPower); robot.robotDrive.driveBase.resetOdometry();
robot.robotDrive.purePursuitDrive.start( tuneDriveStartPoint = robot.robotDrive.driveBase.getFieldPosition();
false, tuneDriveEndPoint = tuneDriveStartPoint.addRelativePose(
Dashboard.Drive.maxVelocity, new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0,
Dashboard.Drive.maxAcceleration, Dashboard.Drive.turnTarget));
Dashboard.Drive.maxDeceleration, tuneDriveAtEndPoint = false;
tuneDriveAtEndPoint? tuneDriveStartPoint: tuneDriveEndPoint); }
tuneDriveAtEndPoint = !tuneDriveAtEndPoint; 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.maxDeceleration,
tuneDriveAtEndPoint? tuneDriveStartPoint: tuneDriveEndPoint);
tuneDriveAtEndPoint = !tuneDriveAtEndPoint;
}
passToTeleOp = false;
} }
passToTeleOp = false;
} }
else if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING) else if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING)
{ {