diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dbedcf8..1d7f329 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,6 +31,9 @@ import frc.robot.Superstructure.SuperState; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberSubsystem; +import frc.robot.subsystems.climber.EmptyClimberSubsystem; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; import frc.robot.subsystems.indexer.SpindexerSubsystem; @@ -166,6 +169,7 @@ public enum RobotEdition { // swervesubsystem decides on its own whether or not to use alpha or comp swerve constants private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); private final LEDSubsystem leds; + private final ClimberSubsystem climber; // climber only exists for the comp bot - this is accounted for later @@ -323,10 +327,12 @@ public Robot() { ? new HoodIO(HoodIO.getCompHood(), canivore, 11) : new HoodIOSim( canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); - - // TODO climber break; } + climber = + ROBOT_EDITION == RobotEdition.ALPHA + ? new EmptyClimberSubsystem(canivore) + : new ClimberSubsystem(new ClimberIO(canivore)); // now that we've assigned the correct subsystems based on robot edition, we can pass them into // the superstructure superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); @@ -337,9 +343,6 @@ public Robot() { () -> Superstructure.getState() == SuperState.SPIN_UP_SCORE || Superstructure.getState() == SuperState.SCORE); - // if this is alpha, we won't have assigned a climber yet - // this creates a placeholder "no-operation" climber that will just not do anything, but is not - // null (and we need it to be not null) DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); @@ -525,7 +528,10 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // TODO: autoaim (comp) // autoAimReq.and(() -> ROBOT_EDITION == RobotEdition.COMP).whileTrue(); - // TODO add binding for climb + + // TODO ACTUAL BUTTON BINDING FOR CLIMBER + driver.x().onTrue(climber.extendClimber()); + driver.y().onTrue(climber.retractClimber()); // current zero shooter hood driver.b().whileTrue(shooter.runCurrentZeroing()); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..88b1b93 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,126 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; + +public class ClimberIO { + + @AutoLog + public static class ClimberIOInputs { + public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorVelocityMetersPerSec = 0.0; + public double motorStatorCurrentAmps = 0.0; + public double motorSupplyCurrentAmps = 0.0; + public double motorVoltage = 0.0; + public double motorTempC = 0.0; + } + + protected final TalonFX climberMotor; + + private final StatusSignal motorPositionRotations; + private final StatusSignal angularVelocityRotsPerSec; + private final StatusSignal statorCurrentAmps; + private final StatusSignal supplyCurrentAmps; + private final StatusSignal motorVoltage; + private final StatusSignal motorTemp; + + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + + private double climberSetpoint = 0.0; + + public ClimberIO(CANBus canBus) { + // todo: set correct motor ID + climberMotor = new TalonFX(30, canBus); + climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); + + angularVelocityRotsPerSec = climberMotor.getVelocity(); + supplyCurrentAmps = climberMotor.getSupplyCurrent(); + motorVoltage = climberMotor.getMotorVoltage(); + statorCurrentAmps = climberMotor.getStatorCurrent(); + motorTemp = climberMotor.getDeviceTemp(); + motorPositionRotations = climberMotor.getPosition(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + angularVelocityRotsPerSec, + supplyCurrentAmps, + motorVoltage, + statorCurrentAmps, + motorTemp, + motorPositionRotations); + climberMotor.optimizeBusUtilization(); + } + + public static TalonFXConfiguration getClimberConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + // todo: find and make climber gear ratio variable + config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; + + // todo: tune + config.Slot0.kS = 0.0; + config.Slot0.kG = 0.0; + config.Slot0.kV = 0.0; + config.Slot0.kP = 0.0; + config.Slot0.kD = 0.0; + + // todo: find actual current limits + config.CurrentLimits.StatorCurrentLimit = 50.00; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.00; + + return config; + } + + public void setClimberPosition(double climberPosition) { + climberSetpoint = climberPosition; + climberMotor.setControl(positionVoltage.withPosition(climberSetpoint)); + } + + public void setClimberVoltage(double climberVoltage) { + climberMotor.setControl(voltageOut.withOutput(climberVoltage)); + } + + public void setClimberVelocity(double climberVelocity) { + climberMotor.setControl(velocityVoltage.withVelocity(climberVelocity)); + } + + public void updateInputs(ClimberIOInputs inputs) { + BaseStatusSignal.refreshAll( + motorPositionRotations, + angularVelocityRotsPerSec, + statorCurrentAmps, + supplyCurrentAmps, + motorVoltage, + motorTemp); + + inputs.motorPositionRotations = + Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorVoltage = motorVoltage.getValueAsDouble(); + inputs.motorTempC = motorTemp.getValueAsDouble(); + inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); + inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..315d344 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.climber; + +// unfinished + +// public class ClimberIOSim extends ClimberIO { +// TalonFXSimState climberMotorSim; + +// private final DCMotorSim climberPhysicsSim = +// new DCMotorSim( +// LinearSystemId.createDCMotorSystem( +// DCMotor.getKrakenX44Foc(1), 0.01, ClimberSubsystem.GEAR_RATIO), +// DCMotor.getKrakenX44Foc(1)); + +// private final double simLoopPeriod = 0.002; +// private Notifier simNotifier = null; +// private double lastSimTime = 0.0; + +// public ClimberIOSim(CANBus canbus) { +// super(ClimberIO.getClimberConfiguration(), canbus); +// climberMotorSim = climberMotor.getSimState(); +// climberMotorSim.setMotorType(MotorType.KrakenX60); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..57c1c46 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class ClimberSubsystem extends SubsystemBase { + // todo: find actual constants + public static double GEAR_RATIO = (45.0 / 1.0); + public static double MAX_EXTENSION_METERS = 0.2413; + public static double MAX_ACCELERATION = 10.0; + public static double MAX_VELOCITY = 2.0; + + ClimberIO climberIO; + ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); + + @Override + public void periodic() { + climberIO.updateInputs(climberInputs); + Logger.processInputs("Climber", climberInputs); + } + + // member variables here? + + public ClimberSubsystem(ClimberIO climberIO) { + this.climberIO = climberIO; + } + + public Command extendClimber() { + return this.run( + () -> { + climberIO.setClimberPosition(MAX_EXTENSION_METERS); + }); + } + + public Command retractClimber() { + return this.run( + () -> { + climberIO.setClimberPosition(0.0); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java new file mode 100644 index 0000000..1dbe0c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.first.wpilibj2.command.Command; + +public class EmptyClimberSubsystem extends ClimberSubsystem { + + @Override + public void periodic() {} + + public EmptyClimberSubsystem(CANBus canbus) { + super(new ClimberIO(canbus)); + } + + @Override + public Command extendClimber() { + return this.idle(); + } + + @Override + public Command retractClimber() { + return this.idle(); + } +}