diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5813025..78c6890 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -158,9 +158,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); - // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); - // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); + // SmartDashboard.putNumber("yaw", m_robotContainer.m_robotMap.gyro.getAngle()); + SmartDashboard.putNumber("Robot.java Pitch", m_robotContainer.m_robotMap.gyro.getPitch()); + // SmartDashboard.putNumber("roll", m_robotContainer.m_robotMap.gyro.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bb7afe4..2a2e90c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,7 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); @@ -99,8 +99,9 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ - // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> gyroRef.reset())); + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); + // .onFalse() /* Operator Buttons */ // activates "Lit Mode" diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index c0c95f8..aeb4610 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -19,7 +19,11 @@ public class AutoBalance extends PelvicInflammatoryDisease { /** Creates a new AutoBalanceTF2. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(.7, .1, 15, 0); + super(1.0, 0, 0, 0); + + this.gyro = gyro; + this.drive = drive; + addRequirements(drive); } @@ -40,7 +44,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public void initialize() { super.initialize(); - this.gyro.reset(); + // this.gyro.reset(); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 082991d..e8dd497 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -140,7 +140,7 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond); - driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); }