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

Fixed re-localization issue. When pressing a button to relocalize, we should not allow the robot to move.

Browse Source
This commit is contained in:
Titan Robotics Club
2024-06-03 22:29:39 -07:00
parent 8d97226513
commit 62d33f39d1
1 changed files with 21 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
TeamCode/src/main/java/teamcode/FtcTeleOp.java
View File

@ -167,25 +167,31 @@ public class FtcTeleOp extends FtcOpMode
// //
if (robot.robotDrive != null) if (robot.robotDrive != null)
{ {
double[] inputs = driverGamepad.getDriveInputs( // We are trying to re-localize the robot and vision hasn't seen AprilTag yet.
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale); if (relocalizing)
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
{ {
robot.robotDrive.driveBase.holonomicDrive( if (robotFieldPose == null)
null, inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveGyroAngle()); {
robotFieldPose = robot.vision.getRobotFieldPose();
}
} }
else else
{ {
robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]); double[] inputs = driverGamepad.getDriveInputs(
} RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
robot.dashboard.displayPrintf(
1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s", if (robot.robotDrive.driveBase.supportsHolonomicDrive())
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation()); {
// We are trying to re-localize the robot and vision hasn't seen AprilTag yet. robot.robotDrive.driveBase.holonomicDrive(
if (relocalizing && robotFieldPose == null) null, inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveGyroAngle());
{ }
robotFieldPose = robot.vision.getRobotFieldPose(); else
{
robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]);
}
robot.dashboard.displayPrintf(
1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
} }
} }
// //