Skip to content

Issue #112 Robust Aimer, Fix Encoder Wrapping #162

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

Open
Bread-n-Butter06 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 2 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
1 change: 1 addition & 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,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;

Expand Down
21 changes: 18 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,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));

Expand Down Expand Up @@ -152,8 +156,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;
Copy link
Collaborator

@jkleiber jkleiber Mar 12, 2024

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 +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;
}
Copy link
Collaborator

@jkleiber jkleiber Mar 12, 2024

Choose a reason for hiding this comment

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

Your inclusion of the max encoder position made me think about this a bit differently than I had in the past.

We have the following known information:

  1. Our offset that sets the encoder output to 0 when the aimer is level. This is roughly the minimum value we should see before the encoder gets put into our coordinate frame
  2. A maximum angle we expect to read from the encoder, post transformation. In other words, we know the maximum angle the aimer can go from the 0 caused by the offset

Therefore, we should be able to determine the bounds of our sensor reading before the wrap. Then we could set continuous output mode on the PID with the correct values. It may be a good idea to draw this out and convince ourselves that this is a legit strategy

Copy link
Author

@Bread-n-Butter06 Bread-n-Butter06 Mar 14, 2024

Choose a reason for hiding this comment

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

How do we find the maximum angle? Also controller.SetContinuous(); sets the PID to continuous output, no? The method sad in code.

Copy link
Collaborator

@jkleiber jkleiber Mar 14, 2024

Choose a reason for hiding this comment

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

Yeah, looks like SetContinuous is actually not a good plan. But maybe the thing to do here is to identify when the encoder is out of bounds, clamp it to the bounds and disable motor output in the direction of "more out of bounds"

We do something similar for the climber - if it is above a "soft stop" we don't allow it to drive up anymore, but do allow it to drive down. So we could implement something similar for the aimer and its encoder here.

To determine the raw angular range, you will need the offset (a constant we already have) and a maximum throw of the arm (we may or may not have this constant already). Then the min raw angle is the offset, and the maximum raw angle is the offset + the throw, which then needs to be modulus 2 pi. Then we would check that the encoder value is inside the arc length between the two values (as treating the max as a pure max doesn't work if the max is wrapped to 0 or something)


appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0);
aimerRight.setVoltage(appliedVolts);

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