diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..ee77706 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; 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.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; @@ -260,7 +261,7 @@ public Robot() { indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); - climber = new ClimberSubsystem(); // TODO climber + climber = new ClimberSubsystem(); break; } // now that we've assigned the correct subsystems based on robot edition, we can pass them into @@ -270,7 +271,7 @@ public Robot() { // 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) if (climber == null) - climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such + climber = new ClimberSubsystem(new ClimberIO(canivore) {}); DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); 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..d1ce283 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,137 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +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 com.ctre.phoenix6.signals.GravityTypeValue; +import org.littletonrobotics.junction.AutoLogOutput; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Frequency; +import frc.robot.subsystems.shooter.HoodIO; + +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 Rotation2d climberSetpoint = Rotation2d.kZero; + + +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(Rotation2d climberPosition) { + climberSetpoint = climberPosition; + climberMotor.setControl(positionVoltage.withPosition(climberPosition.getRotations())); +} + +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/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 2b5d698..eb75606 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,17 +1,55 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems.climber; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.components.rollers.RollerIO; +import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; + + public class ClimberSubsystem extends SubsystemBase { - /** Creates a new ClimberSubsystem. */ - public ClimberSubsystem() {} + //todo: find actual constants + public static double GEAR_RATIO = (45.0 / 1.0); + public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180); + public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); + public static double MAX_ACCELERATION = 10.0; + public static double MAX_VELOCITY = 2.0; + +ClimberIO climberIO; +ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); @Override public void periodic() { - // This method will be called once per scheduler run + climberIO.updateInputs(climberInputs); + Logger.processInputs("Climber", climberInputs); } + +//member variables here? + +public ClimberSubsystem(ClimberIO climberIO) { + this.climberIO = climberIO; +} + +public Command climbUp() { + return this.run( + () -> { + climberIO.setClimberPosition(MAX_ANGLE); + }); + +} + +public Command climbDown() { + return this.run( + () -> { + climberIO.setClimberPosition(MIN_ANGLE); + }); + +} + }