diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 7ec5330..0a1dcdd 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -34,17 +34,15 @@ public static final class TurretConstants { public static final Rotation2d mechanismOffset = Rotation2d.kCCW_Pi_2; // Absolute encoder + public static final int absEncoderPort = 5; public static final double encoderPositionFactor = (2 * Math.PI) / 5.0; // Radians public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * 5.0); // Rad/sec // Position controller - public static final double minInput = 0.0; - public static final double maxInput = 2 * Math.PI; public static final double kPReal = 0.5; // Motor controller public static final int port = 12; - public static final int DIOPort = 5; public static final double motorReduction = 5.0; public static final AngularVelocity maxAngularVelocity = NEO550Constants.kFreeSpeed.div(motorReduction); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index 1a8278a..169ea61 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -25,7 +25,9 @@ public TurretIOSim() { new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); // Enable wrapping for turn PID - positionController.enableContinuousInput(-Math.PI, Math.PI); + // positionController.enableContinuousInput(-Math.PI, Math.PI); + + turnSim.setAngle(-mechanismOffset.getRadians()); } @Override @@ -46,7 +48,7 @@ public void updateInputs(TurretIOInputs inputs) { // Update turn inputs inputs.motorControllerConnected = true; - inputs.relativePosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.relativePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(mechanismOffset); inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); @@ -61,10 +63,13 @@ public void setOpenLoop(double output) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { closedLoop = true; + double setpoint = + MathUtil.inputModulus( + rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2.0 * Math.PI); this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); - positionController.setSetpoint(rotation.getRadians()); + positionController.setSetpoint(setpoint); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 07e5198..0ebe2a0 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -44,7 +44,7 @@ public TurretIOSpark() { controller = new PIDController(kPReal, 0.0, 0.0); absoluteEncoder = new DutyCycleEncoder( - new DigitalInput(DIOPort), + new DigitalInput(absEncoderPort), 2 * Math.PI, absEncoderOffset.getRadians() + mechanismOffset.getRadians()); @@ -120,7 +120,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { closedLoop = true; double setpoint = MathUtil.inputModulus( - rotation.getRadians() - mechanismOffset.getRadians(), minInput, maxInput); + rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2 * Math.PI); this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond)