diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index faa949a7..cb9c0c87 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -477,6 +477,8 @@ public static final class ScoringConstants { public static final int aimEncoderPort = 0; public static final double aimerEncoderOffset = 1.78; + public static final int aimerEncoderMaxValue = 2048; + public static final int aimerLowerLimit = -1; public static final double aimPositionTolerance = 0.015; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index a8321860..c1fed3dc 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.google.flatbuffers.Constants; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; @@ -57,6 +59,10 @@ public class AimerIORoboRio implements AimerIO { double lastPosition = 0.0; double lastTime = Utils.getCurrentTimeSeconds(); + double encoderDiff = 0.0; + + int greatestExpectedAngle = 0; + public AimerIORoboRio() { aimerLeft.setControl(new Follower(ScoringConstants.aimRightMotorId, true)); @@ -152,8 +158,11 @@ public void setBrakeMode(boolean brake) { } private double getEncoderPosition() { + double position = encoder.getAbsolutePosition(); + if (position < ScoringConstants.aimerLowerLimit) + position = 0.0; // return aimerRight.getPosition().getValueAsDouble() * 2.0 * Math.PI * (1.0 / 80.0); - return encoder.getAbsolutePosition() * 2.0 * Math.PI - ScoringConstants.aimerEncoderOffset; + return position * 2.0 * Math.PI - ScoringConstants.aimerEncoderOffset; } @Override @@ -178,6 +187,18 @@ public void updateInputs(AimerIOInputs inputs) { feedforward.calculate(controlSetpoint, velocitySetpoint) + controllerVolts; } + /*encoderDiff = getEncoderPosition() - lastPosition; + if (Math.abs(encoderDiff) > (ScoringConstants.aimerEncoderMaxValue)/2){ //checks if difference is ridiculous -> indicates wrapping + if (encoderDiff < 0.0) + encoderDiff = encoderDiff + ScoringConstants.aimerEncoderMaxValue; + if (encoderDiff > 0.0) + encoderDiff = encoderDiff - ScoringConstants.aimerEncoderMaxValue; + }*/ + + if (getEncoderPosition() > greatestExpectedAngle){ + controller.SetContinuous(); + } + appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); aimerRight.setVoltage(appliedVolts); @@ -189,8 +210,8 @@ public void updateInputs(AimerIOInputs inputs) { double diffTime = currentTime - lastTime; lastTime = currentTime; - inputs.aimVelocityRadPerSec = (getEncoderPosition() - lastPosition) / diffTime; - velocity = (getEncoderPosition() - lastPosition) / diffTime; + inputs.aimVelocityRadPerSec = encoderDiff / diffTime; + velocity = encoderDiff / diffTime; lastPosition = getEncoderPosition(); inputs.aimAppliedVolts = appliedVolts;