diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f343dbb..2e159df 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 400)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); // turn 45 degrees new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 69bf200..259c571 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -44,9 +44,9 @@ public class DriveStraightToPositionMM extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); }