From 589faf845c4aad9cb13218806f5167243e24852d Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 6 Mar 2024 18:45:59 -0500 Subject: [PATCH 1/7] is this all i'm meant to do? charlie said that i should check for high velocity as well as stator current when i was using current to see if the motor was dead but convienently wpilib has an "isalive" function that hopefully works --- src/main/java/frc/robot/Constants.java | 6 ++++++ .../robot/subsystems/scoring/AimerIOTalon.java | 16 ++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index faa949a7..68398377 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -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; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index 975fed1d..bdc047ff 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -34,6 +34,9 @@ public class AimerIOTalon implements AimerIO { double lastPosition = 0.0; double lastTime = Utils.getCurrentTimeSeconds(); + boolean motorFailure = false; + boolean motorCheckOverriden = false; + public AimerIOTalon() { aimerRight.setControl(new Follower(ScoringConstants.aimLeftMotorId, true)); @@ -126,6 +129,17 @@ public void setFF(double kS, double kV, double kA, double kG) { aimerRight.getConfigurator().apply(slot0); } + private boolean checkForMotorFailure() { + if (!aimerLeft.isAlive() || !aimerRight.isAlive()) { + if (!motorCheckOverriden) setFF(ScoringConstants.aimerFaultkS, ScoringConstants.aimerFaultkV, ScoringConstants.aimerFaultkA, ScoringConstants.aimerFaultkG); + motorFailure = true; + } else if (motorFailure) { + if (!motorCheckOverriden) setFF(ScoringConstants.aimerkS, ScoringConstants.aimerkV, ScoringConstants.aimerkA, ScoringConstants.aimerkG); + motorFailure = false; + } + return motorFailure; + } + @Override public void updateInputs(AimerIOInputs inputs) { if (override) { @@ -149,5 +163,7 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble(); inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble(); inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); + + checkForMotorFailure(); } } From a6709791328ff0c13fd8a787b0914040f6df6d5f Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 6 Mar 2024 20:00:42 -0500 Subject: [PATCH 2/7] I added both volts vs current testing and checking for disconnection! should i add anything else? --- .../subsystems/scoring/AimerIOTalon.java | 66 +++++++++++++++---- 1 file changed, 55 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index bdc047ff..587cdaad 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; @@ -34,7 +35,7 @@ public class AimerIOTalon implements AimerIO { double lastPosition = 0.0; double lastTime = Utils.getCurrentTimeSeconds(); - boolean motorFailure = false; + boolean motorLeftFailure = false, motorRightFailure = false; boolean motorCheckOverriden = false; public AimerIOTalon() { @@ -129,23 +130,60 @@ public void setFF(double kS, double kV, double kA, double kG) { aimerRight.getConfigurator().apply(slot0); } - private boolean checkForMotorFailure() { - if (!aimerLeft.isAlive() || !aimerRight.isAlive()) { + private boolean checkForAimMotorFailure() { + + /*if (!aimerLeft.isAlive() || !aimerRight.isAlive()) { if (!motorCheckOverriden) setFF(ScoringConstants.aimerFaultkS, ScoringConstants.aimerFaultkV, ScoringConstants.aimerFaultkA, ScoringConstants.aimerFaultkG); motorFailure = true; } else if (motorFailure) { if (!motorCheckOverriden) setFF(ScoringConstants.aimerkS, ScoringConstants.aimerkV, ScoringConstants.aimerkA, ScoringConstants.aimerkG); motorFailure = false; } - return motorFailure; + return motorFailure;*/ + + //LEFT MOTOR + if ( + (!aimerLeft.isAlive()) || // if motor disconnected + (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) > 0.5) || //if motor voltage is really small when it shouldn't be + (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) < 0.5) // if motor voltage is really large when it shouldn't be + ) { + motorLeftFailure = true; + } + + //RIGHT MOTOR + if ( + (!aimerRight.isAlive()) || // if motor disconnected + (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) > 0.5) || //if motor voltage is really small when it shouldn't be + (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) < 0.5) // if motor voltage is really large when it shouldn't be + ) { + motorRightFailure = true; + } + + if (!motorCheckOverriden) shutOffFaultyAimMotors(); + + return motorLeftFailure || motorRightFailure; + } + + 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) { 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; @@ -160,10 +198,16 @@ 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(); - - checkForMotorFailure(); + 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(); + } + + checkForAimMotorFailure(); } } From 447bfe688a9d6780154c6c9b3dc41e31fce875d1 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 6 Mar 2024 20:05:28 -0500 Subject: [PATCH 3/7] formatting --- src/main/java/frc/robot/Constants.java | 2 +- .../subsystems/scoring/AimerIOTalon.java | 56 +++++++++++++------ 2 files changed, 39 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68398377..22d37757 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -445,7 +445,7 @@ public static final class ScoringConstants { public static final double aimerkV = 1.51; public static final double aimerkA = 0.01; - //TODO: find real values + // TODO: find real values public static final double aimerFaultkS = 0.4; public static final double aimerFaultkG = 0.2; public static final double aimerFaultkV = 3; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index 587cdaad..14531cff 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; @@ -131,7 +130,7 @@ public void setFF(double kS, double kV, double kA, double kG) { } private boolean checkForAimMotorFailure() { - + /*if (!aimerLeft.isAlive() || !aimerRight.isAlive()) { if (!motorCheckOverriden) setFF(ScoringConstants.aimerFaultkS, ScoringConstants.aimerFaultkV, ScoringConstants.aimerFaultkA, ScoringConstants.aimerFaultkG); motorFailure = true; @@ -141,21 +140,37 @@ private boolean checkForAimMotorFailure() { } return motorFailure;*/ - //LEFT MOTOR - if ( - (!aimerLeft.isAlive()) || // if motor disconnected - (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) > 0.5) || //if motor voltage is really small when it shouldn't be - (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) < 0.5) // if motor voltage is really large when it shouldn't be - ) { + // LEFT MOTOR + if ((!aimerLeft.isAlive()) + || // if motor disconnected + (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) + - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) + > 1 + && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) > 0.5) + || // if motor voltage is really small when it shouldn't be + (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) + - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) + > 1 + && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) + < 0.5) // if motor voltage is really large when it shouldn't be + ) { motorLeftFailure = true; } - //RIGHT MOTOR - if ( - (!aimerRight.isAlive()) || // if motor disconnected - (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) > 0.5) || //if motor voltage is really small when it shouldn't be - (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) > 1 && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) < 0.5) // if motor voltage is really large when it shouldn't be - ) { + // RIGHT MOTOR + if ((!aimerRight.isAlive()) + || // if motor disconnected + (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) + - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) + > 1 + && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) > 0.5) + || // if motor voltage is really small when it shouldn't be + (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) + - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) + > 1 + && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) + < 0.5) // if motor voltage is really large when it shouldn't be + ) { motorRightFailure = true; } @@ -167,12 +182,16 @@ private boolean checkForAimMotorFailure() { 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); + setFF( + ScoringConstants.aimerFaultkS, + ScoringConstants.aimerFaultkV, + ScoringConstants.aimerFaultkA, + ScoringConstants.aimerFaultkG); } } @@ -182,7 +201,8 @@ public void updateInputs(AimerIOInputs inputs) { if (!motorLeftFailure || motorCheckOverriden) aimerLeft.setVoltage(overrideVolts); else aimerRight.setVoltage(overrideVolts); } else { - if (!motorLeftFailure || motorCheckOverriden) aimerLeft.setControl(controller.withPosition(goalAngleRad)); + if (!motorLeftFailure || motorCheckOverriden) + aimerLeft.setControl(controller.withPosition(goalAngleRad)); else aimerRight.setControl(controller.withPosition(goalAngleRad)); } @@ -207,7 +227,7 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble(); inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); } - + checkForAimMotorFailure(); } } From 406a1db8cc19b44886917d231219d9e40370a0b3 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Sat, 9 Mar 2024 16:40:58 -0500 Subject: [PATCH 4/7] edited! --- .../subsystems/scoring/AimerIOTalon.java | 84 ++++++++++--------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index 14531cff..f23ac165 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc.robot.Constants.ScoringConstants; public class AimerIOTalon implements AimerIO { @@ -131,54 +132,46 @@ public void setFF(double kS, double kV, double kA, double kG) { private boolean checkForAimMotorFailure() { - /*if (!aimerLeft.isAlive() || !aimerRight.isAlive()) { - if (!motorCheckOverriden) setFF(ScoringConstants.aimerFaultkS, ScoringConstants.aimerFaultkV, ScoringConstants.aimerFaultkA, ScoringConstants.aimerFaultkG); - motorFailure = true; - } else if (motorFailure) { - if (!motorCheckOverriden) setFF(ScoringConstants.aimerkS, ScoringConstants.aimerkV, ScoringConstants.aimerkA, ScoringConstants.aimerkG); - motorFailure = false; - } - return motorFailure;*/ - // LEFT MOTOR - if ((!aimerLeft.isAlive()) - || // if motor disconnected - (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - > 1 - && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) > 0.5) - || // if motor voltage is really small when it shouldn't be - (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - > 1 - && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) - < 0.5) // if motor voltage is really large when it shouldn't be - ) { + if (motorFailureCheckPair(aimerLeft, aimerRight)) { motorLeftFailure = true; } // RIGHT MOTOR - if ((!aimerRight.isAlive()) - || // if motor disconnected - (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - > 1 - && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) > 0.5) - || // if motor voltage is really small when it shouldn't be - (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble()) - - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble()) - > 1 - && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) - < 0.5) // if motor voltage is really large when it shouldn't be - ) { + if (motorFailureCheckPair(aimerRight, aimerLeft)) { motorRightFailure = true; } - if (!motorCheckOverriden) shutOffFaultyAimMotors(); + 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) + { + // motor voltage is really large when it shouldn't be + return true; + } + return false; + } + private void shutOffFaultyAimMotors() { if (motorLeftFailure) { aimerLeft.setVoltage(0); @@ -197,13 +190,22 @@ private void shutOffFaultyAimMotors() { @Override public void updateInputs(AimerIOInputs inputs) { + checkForAimMotorFailure(); + if (override) { - if (!motorLeftFailure || motorCheckOverriden) aimerLeft.setVoltage(overrideVolts); - else aimerRight.setVoltage(overrideVolts); + if (!motorLeftFailure || motorCheckOverriden) { + aimerLeft.setVoltage(overrideVolts); + } + else { + aimerRight.setVoltage(overrideVolts); + } } else { - if (!motorLeftFailure || motorCheckOverriden) + if (!motorLeftFailure || motorCheckOverriden) { aimerLeft.setControl(controller.withPosition(goalAngleRad)); - else aimerRight.setControl(controller.withPosition(goalAngleRad)); + } + else { + aimerRight.setControl(controller.withPosition(goalAngleRad)); + } } inputs.aimGoalAngleRad = goalAngleRad; @@ -228,6 +230,6 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); } - checkForAimMotorFailure(); + } } From 9e281336184abf345a5d5181d37ed88a025d3234 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Sat, 9 Mar 2024 16:44:39 -0500 Subject: [PATCH 5/7] formatting :P --- .../subsystems/scoring/AimerIOTalon.java | 22 ++++++++----------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index f23ac165..0d6fd6ba 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc.robot.Constants.ScoringConstants; public class AimerIOTalon implements AimerIO { @@ -151,21 +150,22 @@ private boolean checkForAimMotorFailure() { private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) { if (!check.isAlive()) { - //motor no longer communicating with robot + // 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) { + - 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) - { + - Math.abs(compare.getStatorCurrent().getValueAsDouble()) + > 1 + && Math.abs(check.getMotorVoltage().getValueAsDouble()) < 0.5) { // motor voltage is really large when it shouldn't be return true; } @@ -195,15 +195,13 @@ public void updateInputs(AimerIOInputs inputs) { if (override) { if (!motorLeftFailure || motorCheckOverriden) { aimerLeft.setVoltage(overrideVolts); - } - else { + } else { aimerRight.setVoltage(overrideVolts); } } else { if (!motorLeftFailure || motorCheckOverriden) { aimerLeft.setControl(controller.withPosition(goalAngleRad)); - } - else { + } else { aimerRight.setControl(controller.withPosition(goalAngleRad)); } } @@ -229,7 +227,5 @@ public void updateInputs(AimerIOInputs inputs) { inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble(); inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); } - - } } From 957d66eac4e632b3cce1986af88290b9ba108f0b Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 13 Mar 2024 18:53:58 -0400 Subject: [PATCH 6/7] edited! --- src/main/java/frc/robot/Constants.java | 5 +++ .../subsystems/scoring/AimerIOTalon.java | 35 ++++++++++++++----- 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 22d37757..6ecb60fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -598,6 +598,11 @@ public static HashMap aimerAvoidElevatorTable() { return map; } + + //TODO: tune + public static final double allotedArmMotorErrorTime = 3.0; + public static final double allottedArmMotorCurrentDifference = 10; + public static final double voltageErrorCheckingThreshold = 2; } public static final class LEDConstants { diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index 0d6fd6ba..d54c6078 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants.ScoringConstants; public class AimerIOTalon implements AimerIO { @@ -37,6 +38,9 @@ public class AimerIOTalon implements AimerIO { boolean motorLeftFailure = false, motorRightFailure = false; boolean motorCheckOverriden = false; + private Timer errorTimeLeft = new Timer(); + private Timer errorTimeRight = new Timer(); + public AimerIOTalon() { aimerRight.setControl(new Follower(ScoringConstants.aimLeftMotorId, true)); @@ -78,6 +82,11 @@ public AimerIOTalon() { encoder.setDistancePerRotation(2 * Math.PI); encoder.setPositionOffset(ScoringConstants.aimerEncoderOffset); + + errorTimeLeft.reset(); + errorTimeLeft.start(); + errorTimeRight.reset(); + errorTimeRight.start(); } @Override @@ -132,13 +141,23 @@ public void setFF(double kS, double kV, double kA, double kG) { private boolean checkForAimMotorFailure() { // LEFT MOTOR - if (motorFailureCheckPair(aimerLeft, aimerRight)) { - motorLeftFailure = true; + if (!motorFailureCheckPair(aimerLeft, aimerRight)) { + errorTimeLeft.reset(); + errorTimeLeft.start(); + } else { + if (errorTimeLeft.get() > ScoringConstants.allotedArmMotorErrorTime) { + motorLeftFailure = true; + } } // RIGHT MOTOR - if (motorFailureCheckPair(aimerRight, aimerLeft)) { - motorRightFailure = true; + if (!motorFailureCheckPair(aimerRight, aimerLeft)) { + errorTimeRight.reset(); + errorTimeRight.start(); + } else { + if (errorTimeRight.get() > ScoringConstants.allotedArmMotorErrorTime) { + motorRightFailure = true; + } } if (!motorCheckOverriden) { @@ -156,16 +175,16 @@ private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) { if (Math.abs(compare.getStatorCurrent().getValueAsDouble()) - Math.abs(check.getStatorCurrent().getValueAsDouble()) - > 1 - && Math.abs(check.getMotorVoltage().getValueAsDouble()) > 0.5) { + > ScoringConstants.allottedArmMotorCurrentDifference + && Math.abs(check.getMotorVoltage().getValueAsDouble()) > ScoringConstants.voltageErrorCheckingThreshold) { // 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) { + > ScoringConstants.allottedArmMotorCurrentDifference + && Math.abs(check.getMotorVoltage().getValueAsDouble()) < ScoringConstants.voltageErrorCheckingThreshold) { // motor voltage is really large when it shouldn't be return true; } From df284dd04a6322a26e64a38c5a2b69eab3646a23 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 13 Mar 2024 19:01:11 -0400 Subject: [PATCH 7/7] format --- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/subsystems/scoring/AimerIOTalon.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6ecb60fb..7e4c7d67 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -599,7 +599,7 @@ public static HashMap aimerAvoidElevatorTable() { return map; } - //TODO: tune + // TODO: tune public static final double allotedArmMotorErrorTime = 3.0; public static final double allottedArmMotorCurrentDifference = 10; public static final double voltageErrorCheckingThreshold = 2; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java index d54c6078..09f32c8a 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOTalon.java @@ -176,7 +176,8 @@ private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) { if (Math.abs(compare.getStatorCurrent().getValueAsDouble()) - Math.abs(check.getStatorCurrent().getValueAsDouble()) > ScoringConstants.allottedArmMotorCurrentDifference - && Math.abs(check.getMotorVoltage().getValueAsDouble()) > ScoringConstants.voltageErrorCheckingThreshold) { + && Math.abs(check.getMotorVoltage().getValueAsDouble()) + > ScoringConstants.voltageErrorCheckingThreshold) { // motor voltage is really small when it shouldn't be return true; } @@ -184,7 +185,8 @@ private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) { if (Math.abs(check.getStatorCurrent().getValueAsDouble()) - Math.abs(compare.getStatorCurrent().getValueAsDouble()) > ScoringConstants.allottedArmMotorCurrentDifference - && Math.abs(check.getMotorVoltage().getValueAsDouble()) < ScoringConstants.voltageErrorCheckingThreshold) { + && Math.abs(check.getMotorVoltage().getValueAsDouble()) + < ScoringConstants.voltageErrorCheckingThreshold) { // motor voltage is really large when it shouldn't be return true; }