diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index 772a2fa..f4f9f25 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -17,6 +17,8 @@ public class HoodConstants { // TODO Real Values public static final int kMotorCANID = 0; + public static final double kConversionFactor = 0; + public static final double kP = 0; public static final double kI = 0; public static final double kD = 0; @@ -26,6 +28,12 @@ public class HoodConstants { public static final double kStartupAngle = 0; public static final double kMaxManualSpeedMultiplier = .5; + public static final double kAmpsToTriggerPositionReset = 10; + + // TODO This is just barely longer than the default frame time for output current information + // Should this be longer? + public static final double kTimeAboveThresholdToReset = .25; + public static final int kCurrentLimit = 15; public static final boolean kInverted = false; @@ -45,12 +53,13 @@ public class HoodConstants { .idleMode(kIdleMode) .inverted(kInverted) .smartCurrentLimit(kCurrentLimit); + kConfig.encoder + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); kConfig.closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pid(kP, kI, kD) .outputRange(-1, 1) - .positionWrappingEnabled(true) - .positionWrappingInputRange(0, Math.PI * 2) .feedForward .sva(kS, kV, kA); diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 4c73378..327233e 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -5,51 +5,84 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.HoodConstants; public class Hood extends SubsystemBase { private SparkMax motor; - private AbsoluteEncoder encoder; + private RelativeEncoder encoder; private SparkClosedLoopController controller; - private double currentTargetRadians; + private Trigger resetTrigger; + private Trigger timerTrigger; + + private Timer resetTimer; + + private double currentTargetDegrees; public Hood() { motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless); - encoder = motor.getAbsoluteEncoder(); + motor.configure( + HoodConstants.kConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + encoder = motor.getEncoder(); controller = motor.getClosedLoopController(); - currentTargetRadians = HoodConstants.kStartupAngle; + resetTimer = new Timer(); + resetTimer.reset(); + + resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset)); + resetTrigger.onTrue(new InstantCommand(resetTimer::start)); + resetTrigger.onFalse(new InstantCommand(() -> { + resetTimer.stop(); + resetTimer.reset(); + })); + + timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset)); + timerTrigger.onTrue(new InstantCommand(() -> { + encoder.setPosition(0); + resetTimer.reset(); + })); + + currentTargetDegrees = HoodConstants.kStartupAngle; } @Override public void periodic() { - Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians); + Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees); Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition()); Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint()); } - public Command trackToAngle(DoubleSupplier radianAngleSupplier) { + public Command trackToAngle(DoubleSupplier degreeAngleSupplier) { return run(() -> { - currentTargetRadians = radianAngleSupplier.getAsDouble(); + currentTargetDegrees = degreeAngleSupplier.getAsDouble(); - controller.setSetpoint(currentTargetRadians, ControlType.kPosition); + controller.setSetpoint(currentTargetDegrees, ControlType.kPosition); }); } public Command manualSpeed(DoubleSupplier speed) { - currentTargetRadians = 0; + currentTargetDegrees = 0; return run(() -> { motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier); @@ -60,7 +93,7 @@ public class Hood extends SubsystemBase { return manualSpeed(() -> 0); } - public double getTargetRadians() { - return currentTargetRadians; + public double getTargetDegrees() { + return currentTargetDegrees; } }