diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e83560f..6e2ca79 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -141,6 +141,7 @@ public class RobotContainer { TenBallAutoMiddle m_tenBallAutoMiddle; + Slalom m_slalom; Barrel m_barrel; @@ -198,6 +199,7 @@ public class RobotContainer { // runs the storage not m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); + } /** @@ -208,13 +210,13 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Test Buttons */ + // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) .whenReleased(new InterruptSubystem(m_robotShooter)) .whenReleased(new InterruptSubystem(m_robotShooterHood)); - // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); @@ -434,6 +436,7 @@ public class RobotContainer { }; m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths)); + } /** @@ -461,6 +464,7 @@ public class RobotContainer { //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); //return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index cba7de3..8431166 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -345,7 +345,8 @@ public class Drive extends SubsystemBase { public void updateOdometry(boolean reversed){ if (reversed){ - m_odometry.update(Rotation2d.fromDegrees( -getHeading()-180), + + m_odometry.update(Rotation2d.fromDegrees( -getGyroYaw()-180), -inchesToMeters(getDistanceInches(m_rightFrontMotor)), inchesToMeters(getDistanceInches(m_leftFrontMotor))); }