From ed09a3a415bbbe570d6be1865ca158775660e826 Mon Sep 17 00:00:00 2001 From: Bread-n-Butter06 Date: Sun, 10 Mar 2024 13:55:44 -0400 Subject: [PATCH 1/3] negative encoder values are set to 0 --- .../java/frc/robot/subsystems/scoring/AimerIORoboRio.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index a8321860..490f136a 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -152,8 +152,11 @@ public void setBrakeMode(boolean brake) { } private double getEncoderPosition() { + double position = encoder.getAbsolutePosition(); + if (position < 0.0) + 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 From 904ccbf057536207e83cf1632bd7dadd127a1f42 Mon Sep 17 00:00:00 2001 From: Bread-n-Butter06 Date: Sun, 10 Mar 2024 14:41:07 -0400 Subject: [PATCH 2/3] fix encoder wrapping? --- src/main/java/frc/robot/Constants.java | 1 + .../robot/subsystems/scoring/AimerIORoboRio.java | 16 ++++++++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index faa949a7..fe7b97e8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -477,6 +477,7 @@ 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 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 490f136a..8ac0badb 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,8 @@ public class AimerIORoboRio implements AimerIO { double lastPosition = 0.0; double lastTime = Utils.getCurrentTimeSeconds(); + double encoderDiff = 0.0; + public AimerIORoboRio() { aimerLeft.setControl(new Follower(ScoringConstants.aimRightMotorId, true)); @@ -181,6 +185,14 @@ 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; + } + appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); aimerRight.setVoltage(appliedVolts); @@ -192,8 +204,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; From 249baeadeffebb9a44b9489ad809eb4f8f297bdd Mon Sep 17 00:00:00 2001 From: Bread-n-Butter06 Date: Wed, 13 Mar 2024 20:03:33 -0400 Subject: [PATCH 3/3] fix when position should be 0, started cont pid --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/subsystems/scoring/AimerIORoboRio.java | 10 ++++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe7b97e8..cb9c0c87 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -478,6 +478,7 @@ 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 8ac0badb..c1fed3dc 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -61,6 +61,8 @@ public class AimerIORoboRio implements AimerIO { double encoderDiff = 0.0; + int greatestExpectedAngle = 0; + public AimerIORoboRio() { aimerLeft.setControl(new Follower(ScoringConstants.aimRightMotorId, true)); @@ -157,7 +159,7 @@ public void setBrakeMode(boolean brake) { private double getEncoderPosition() { double position = encoder.getAbsolutePosition(); - if (position < 0.0) + if (position < ScoringConstants.aimerLowerLimit) position = 0.0; // return aimerRight.getPosition().getValueAsDouble() * 2.0 * Math.PI * (1.0 / 80.0); return position * 2.0 * Math.PI - ScoringConstants.aimerEncoderOffset; @@ -185,12 +187,16 @@ public void updateInputs(AimerIOInputs inputs) { feedforward.calculate(controlSetpoint, velocitySetpoint) + controllerVolts; } - encoderDiff = getEncoderPosition() - lastPosition; + /*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);