Skip to content

Aim motor adjustments in case of failure #154

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
minhlpnguyen wants to merge 8 commits into
base: main
Choose a base branch
from
Open

Aim motor adjustments in case of failure #154

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
589faf8
is this all i'm meant to do? charlie said that i should check for hig…
minhlpnguyen Mar 6, 2024
a670979
I added both volts vs current testing and checking for disconnection!…
minhlpnguyen Mar 7, 2024
447bfe6
formatting
minhlpnguyen Mar 7, 2024
406a1db
edited!
minhlpnguyen Mar 9, 2024
9e28133
formatting :P
minhlpnguyen Mar 9, 2024
957d66e
edited!
minhlpnguyen Mar 13, 2024
df284dd
format
minhlpnguyen Mar 13, 2024
0ccde55
Merge branch 'main' into motor-recover
minhlpnguyen Mar 13, 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
6 changes: 6 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 @@ -445,6 +445,12 @@ public static final class ScoringConstants {
public static final double aimerkV = 1.51;
public static final double aimerkA = 0.01;

// TODO: find real values
public static final double aimerFaultkS = 0.4;
public static final double aimerFaultkG = 0.2;
public static final double aimerFaultkV = 3;
public static final double aimerFaultkA = 0.02;

public static final double shooterkP = 0.05;
public static final double shooterkI = 0.2;
public static final double shooterkD = 0.0;
Expand Down
88 changes: 83 additions & 5 deletions src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,9 @@ public class AimerIOTalon implements AimerIO {
double lastPosition = 0.0;
double lastTime = Utils.getCurrentTimeSeconds();

boolean motorLeftFailure = false, motorRightFailure = false;
boolean motorCheckOverriden = false;

public AimerIOTalon() {
aimerRight.setControl(new Follower(ScoringConstants.aimLeftMotorId, true));

Expand Down Expand Up @@ -126,12 +129,81 @@ public void setFF(double kS, double kV, double kA, double kG) {
aimerRight.getConfigurator().apply(slot0);
}

private boolean checkForAimMotorFailure() {

// LEFT MOTOR
if (motorFailureCheckPair(aimerLeft, aimerRight)) {
motorLeftFailure = true;
}

// RIGHT MOTOR
if (motorFailureCheckPair(aimerRight, aimerLeft)) {
motorRightFailure = true;
}

if (!motorCheckOverriden) {
shutOffFaultyAimMotors();
}

return motorLeftFailure || motorRightFailure;
}

private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) {
if (!check.isAlive()) {
// motor no longer communicating with robot
return true;
}

if (Math.abs(compare.getStatorCurrent().getValueAsDouble())
- Math.abs(check.getStatorCurrent().getValueAsDouble())
> 1
&& Math.abs(check.getMotorVoltage().getValueAsDouble()) > 0.5) {
// motor voltage is really small when it shouldn't be
return true;
}

if (Math.abs(check.getStatorCurrent().getValueAsDouble())
- Math.abs(compare.getStatorCurrent().getValueAsDouble())
> 1
&& Math.abs(check.getMotorVoltage().getValueAsDouble()) < 0.5) {
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.

Some points of feedback here:

  1. These thresholds should be set as constants in Constants.java
  2. These are really tight thresholds. 1 A or 0.5 V is common to see as a difference between two motors. Would recommend a much bigger value for amps, but this should be pulled from match/robot data where the aimer motor was not connected most likely.
  3. We should add a time aspect to this. "If the motors disagree by X current for 1 cycle" will yield many false positives. Let's add that they must disagree for T time
  4. Some of this logic looks duplicated?

// motor voltage is really large when it shouldn't be
return true;
}
return false;
}

private void shutOffFaultyAimMotors() {
if (motorLeftFailure) {
aimerLeft.setVoltage(0);
}
if (motorRightFailure) {
aimerRight.setVoltage(0);
}
if (motorLeftFailure || motorRightFailure) {
setFF(
ScoringConstants.aimerFaultkS,
ScoringConstants.aimerFaultkV,
ScoringConstants.aimerFaultkA,
ScoringConstants.aimerFaultkG);
}
}

@Override
public void updateInputs(AimerIOInputs inputs) {
checkForAimMotorFailure();

if (override) {
aimerLeft.setVoltage(overrideVolts);
if (!motorLeftFailure || motorCheckOverriden) {
aimerLeft.setVoltage(overrideVolts);
} else {
aimerRight.setVoltage(overrideVolts);
}
} else {
aimerLeft.setControl(controller.withPosition(goalAngleRad));
if (!motorLeftFailure || motorCheckOverriden) {
aimerLeft.setControl(controller.withPosition(goalAngleRad));
} else {
aimerRight.setControl(controller.withPosition(goalAngleRad));
}
}

inputs.aimGoalAngleRad = goalAngleRad;
Expand All @@ -146,8 +218,14 @@ public void updateInputs(AimerIOInputs inputs) {
lastPosition = encoder.getAbsolutePosition();
inputs.aimVelocityRadPerSec = 0.0;

inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble();
inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
if (!motorLeftFailure || motorCheckOverriden) {
inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble();
inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
} else {
inputs.aimAppliedVolts = aimerRight.getMotorVoltage().getValueAsDouble();
inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
}
}
}