diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713d..18414213 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "reefscape" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 4d46333e..e714e54a 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -1,3 +1,103 @@ package frc.robot.constants; -public class BiscuitConstants {} +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; + +public class BiscuitConstants { + // These are all wrong right now because we don't have any actual info + + public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined + public static int talonID = 3; + public static double kCloseEnough = 2137473647; // This is a little out of wack. + public static final Angle kMaxFwd = Rotations.of(100); + public static final Angle kMaxRev = Rotations.of(-100); + // public static Angle Level1 = ; + + // Disables the TalonFX by setting it's voltage to zero. Not very shocking. + public static VoltageConfigs disableTalon() { + VoltageConfigs voltage = + new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0); + getFXConfig().Voltage = voltage; + return voltage; + } + + // I copied and pasted this because I'm lazy + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(10) + .withStatorCurrentLimitEnable(false) + .withStatorCurrentLimit(20) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + fxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kMaxFwd) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kMaxRev); + fxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0) + .withKA(0); + fxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration(0) + .withMotionMagicCruiseVelocity(0) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(0); + fxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Coast); + fxConfig.MotorOutput = motorOut; + + return fxConfig; + } +} diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java index 45a776f2..02d8743e 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java @@ -1,3 +1,28 @@ package frc.robot.subsystems.biscuit; -public class BiscuitIO {} +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface BiscuitIO { + + @AutoLog + public class BiscuitIOInputs { + public Angle position = Rotations.of(0); + public AngularVelocity velocity = RotationsPerSecond.of(0); + public boolean fwdLimitSwitchOpen = false; + public boolean didZero; + } + + public default void setPosition(Angle position) {} + + public default void updateInputs(BiscuitIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetry) {} + + public default void zero() {} +} diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index 2c5a726e..e519e701 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -1,3 +1,83 @@ package frc.robot.subsystems.biscuit; -public class BiscuitIOFX {} +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.robot.constants.BiscuitConstants; +import frc.robot.subsystems.biscuit.BiscuitIO.BiscuitIOInputs; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; + +public class BiscuitIOFX implements BiscuitIO { + + private Logger logger; + private TalonFX talon; + + private final Angle sensorInitial; + private StatusSignal position; + private StatusSignal velocity; + private StatusSignal fwdLimitSwitch; + private boolean didZero; + private boolean fwdLimitSwitchOpen; + private Angle offset; + private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError); + + TalonFXConfigurator configurator; + private MotionMagicDutyCycle positionRequest = + new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0); + + public BiscuitIOFX() { + // Logger initialization with class name + logger = LoggerFactory.getLogger(this.getClass()); + // Moter initialization with ID from constants + talon = new TalonFX(BiscuitConstants.talonID); + // Set the starting encoder position + sensorInitial = talon.getPosition().getValue(); + + // Reset and configure moter settings + configurator = talon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); + configurator.apply(BiscuitConstants.getFXConfig()); + // Set our variables + velocity = talon.getVelocity(); + position = talon.getPosition(); + } + + public void setPosition(Angle position) { + talon.setControl(positionRequest.withPosition(position)); + } + + public void updateInputs(BiscuitIOInputs inputs) { + BaseStatusSignal.refreshAll(velocity, position, fwdLimitSwitch); + inputs.velocity = velocity.getValue(); + inputs.position = position.getValue(); + inputs.fwdLimitSwitchOpen = fwdLimitSwitch.getValueAsDouble() == 1; + inputs.didZero = didZero; + } + + public void registerWith(TelemetryService telemetry) { + telemetry.register(talon, true); + } + + public void zero() { + didZero = false; + if (fwdLimitSwitchOpen == true) { + Angle pos = position.getValue(); + offset = BiscuitConstants.kZero.minus(pos); + didZero = true; + } else { + rangeAlert.set(true); + logger.error("Biscuit overextended! Shutting down movement!"); + configurator.apply(BiscuitConstants.disableTalon()); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index 213d3f21..d6e69c31 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -1,3 +1,62 @@ +// Blessed by the great tech-priests of the Adeptus Mechanicus + package frc.robot.subsystems.biscuit; -public class BiscuitSubsystem {} +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.*; +import frc.robot.constants.BiscuitConstants; +import java.util.Set; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class BiscuitSubsystem extends MeasurableSubsystem { + + private BiscuitIO io; + private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged(); + private Angle setPoint; + + public void setPosition(Angle position) { + io.setPosition(position); + setPoint = position; + } + + public Angle getPosition(Angle position) { + return inputs.position; + } + + public AngularVelocity getVelocity(AngularVelocity velocity) { + return inputs.velocity; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + Logger.recordOutput("Biscuit setPoint", setPoint); + Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); + } + + @Override + public void registerWith(TelemetryService telemetry) { + super.registerWith(telemetry); + io.registerWith(telemetry); + } + + public boolean isFinished() { + return setPoint.minus(inputs.position).abs(Rotations) <= BiscuitConstants.kCloseEnough; + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("Is Biscuit Finished", () -> isFinished() ? 1.0 : 0.0), + new Measure("Biscuit Set Point", () -> setPoint.in(Rotations))); + } + + public void zero() { + io.zero(); + } +}