diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b7a4e4c..e40aaaf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,7 +29,7 @@ public final class Constants { public static final int DRIVE_SLOT_IDX = 0; public static final int DRIVE_PID_LOOP_IDX = 0; public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0); + public static final Gains DRIVE_GAINS = new Gains(10.0, 0.0, 0.0, 0.2, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 72d42fe..7264254 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -70,6 +70,7 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(() -> m_robotDrive.goToTargetPos()); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index babc27d..f0f5bf5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -114,6 +114,12 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI); SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD); SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF); + + int closedLoopTimeMs = 1; + m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + + } @Override @@ -185,6 +191,6 @@ public class Drive extends SubsystemBase { double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0; m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos); - m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos); + m_rightFrontMotor.follow(m_leftFrontMotor); } }