Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Issue #112 Robust Aimer, Fix Encoder Wrapping #162

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Issue #112 Robust Aimer, Fix Encoder Wrapping #162

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ed09a3a
negative encoder values are set to 0
Mar 10, 2024
904ccbf
fix encoder wrapping?
Mar 10, 2024
249baea
fix when position should be 0, started cont pid
Mar 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
27 changes: 24 additions & 3 deletions src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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));

Expand Down Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's actually this position we want to zero out if it drops below 0.

Today we learned though that the mech team might make our resting position negative 🙃. So we'll actually want to create a Constants.java constant that can be used as the lower limit when the shooter drops below that value

}

@Override
Expand All @@ -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);

Expand All @@ -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;
Expand Down
Loading