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)
{
double[] inputs = driverGamepad.getDriveInputs(
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
// We are trying to re-localize the robot and vision hasn't seen AprilTag yet.
if (relocalizing)
{
robot.robotDrive.driveBase.holonomicDrive(
null, inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveGyroAngle());
if (robotFieldPose == null)
{
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());
// We are trying to re-localize the robot and vision hasn't seen AprilTag yet.
if (relocalizing && robotFieldPose == null)
{
robotFieldPose = robot.vision.getRobotFieldPose();
double[] inputs = driverGamepad.getDriveInputs(
RobotParams.ROBOT_DRIVE_MODE, true, drivePowerScale, turnPowerScale);
if (robot.robotDrive.driveBase.supportsHolonomicDrive())
{
robot.robotDrive.driveBase.holonomicDrive(
null, inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveGyroAngle());
}
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());
}
}
//