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

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);
}
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
@ -508,24 +496,36 @@ public class FtcTest extends FtcTeleOp
switch (button)
{
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);
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;
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.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)
{