Aim motor adjustments in case of failure #154
Conversation
…h 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
… should i add anything else?
jkleiber
left a comment
jkleiber
left a comment
There was a problem hiding this comment.
First pass comments, we will need to test this on the robot before merging as well
| 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; | ||
| } |
There was a problem hiding this comment.
Since we are duplicating a lot of this code below, could we make this into a function that returns a boolean on if the motor is faulty or not?
| if (!motorLeftFailure || motorCheckOverriden) aimerLeft.setVoltage(overrideVolts); | ||
| else aimerRight.setVoltage(overrideVolts); |
There was a problem hiding this comment.
Can we use curly braces on if/else statements?
| if (!motorLeftFailure || motorCheckOverriden) | ||
| aimerLeft.setControl(controller.withPosition(goalAngleRad)); | ||
| else aimerRight.setControl(controller.withPosition(goalAngleRad)); |
There was a problem hiding this comment.
Can we use curly braces on if/else statements?
| /*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;*/ |
There was a problem hiding this comment.
This looks like it can be removed
| motorRightFailure = true; | ||
| } | ||
|
|
||
| if (!motorCheckOverriden) shutOffFaultyAimMotors(); |
There was a problem hiding this comment.
Can we use curly braces on if/else statements?
| inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble(); | ||
| } | ||
|
|
||
| checkForAimMotorFailure(); |
There was a problem hiding this comment.
Should we check for motor failure at the top of this function (that way we can detect failure on the same frame we set the motor values)?
jkleiber
left a comment
jkleiber
left a comment
There was a problem hiding this comment.
One bigger comment, but once addressed I think we should test this on the robot
| > 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) { |
There was a problem hiding this comment.
Some points of feedback here:
- These thresholds should be set as constants in Constants.java
- 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.
- 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
- Some of this logic looks duplicated?
jkleiber
left a comment
jkleiber
left a comment
There was a problem hiding this comment.
We should test this (with single and double motors) before merging
I added checking for both motor disconnection + disparities in current: what else should I add?