SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-04 06:21: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

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

@ -166,6 +166,16 @@ public class FtcTeleOp extends FtcOpMode
// DriveBase subsystem. // DriveBase subsystem.
// //
if (robot.robotDrive != null) if (robot.robotDrive != null)
{
// We are trying to re-localize the robot and vision hasn't seen AprilTag yet.
if (relocalizing)
{
if (robotFieldPose == null)
{
robotFieldPose = robot.vision.getRobotFieldPose();
}
}
else
{ {
double[] inputs = driverGamepad.getDriveInputs( double[] inputs = driverGamepad.getDriveInputs(
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale); RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
@ -182,10 +192,6 @@ public class FtcTeleOp extends FtcOpMode
robot.dashboard.displayPrintf( robot.dashboard.displayPrintf(
1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s", 1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation()); 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.
if (relocalizing && robotFieldPose == null)
{
robotFieldPose = robot.vision.getRobotFieldPose();
} }
} }
// //