diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6b6c7ad..9403200 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index a0d3fe8..7bb2231 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -22,6 +22,10 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.climber.Climber; +import frc.trigon.robot.subsystems.shooter.Shooter; +import frc.trigon.robot.subsystems.shooter.ShooterCommands; +import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.hood.Hood; import frc.trigon.robot.subsystems.hood.HoodCommands; import frc.trigon.robot.subsystems.intake.Intake; @@ -29,12 +33,9 @@ import frc.trigon.robot.subsystems.loader.Loader; import frc.trigon.robot.subsystems.loader.LoaderCommands; import frc.trigon.robot.subsystems.loader.LoaderConstants; -import frc.trigon.robot.subsystems.shooter.Shooter; -import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.spindexer.Spindexer; import frc.trigon.robot.subsystems.spindexer.SpindexerCommands; import frc.trigon.robot.subsystems.spindexer.SpindexerConstants; -import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.turret.Turret; import frc.trigon.robot.subsystems.turret.TurretCommands; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -47,6 +48,7 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); + public static final Climber CLIMBER = new Climber(); public static final Hood HOOD = new Hood(); public static final Intake INTAKE = new Intake(); public static final Loader LOADER = new Loader(); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java new file mode 100644 index 0000000..5a9eaa3 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.trigon.robot.subsystems.climber.ClimberCommands; +import frc.trigon.robot.subsystems.climber.ClimberConstants; + +public class ClimbCommands { + public static Command getClimbToL1Command(Trigger continueTrigger) { + return new SequentialCommandGroup( + ClimberCommands.getSetTargetExtendedStateCommand(ClimberConstants.ClimberState.CLIMB_L1).until(continueTrigger), + ClimberCommands.getSetTargetRetractedStateCommand() + ); + } + + public static Command getClimbDownFromL1Command() { + return ClimberCommands.getSetTargetExtendedStateCommand(ClimberConstants.ClimberState.CLIMB_L1); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..eae2fa4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -0,0 +1,136 @@ +package frc.trigon.robot.subsystems.climber; + +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.utilities.Conversions; +import frc.trigon.robot.subsystems.MotorSubsystem; +import org.littletonrobotics.junction.Logger; + +public class Climber extends MotorSubsystem { + private final TalonFXMotor + motor = ClimberConstants.MOTOR; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ClimberConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + 0, + ClimberConstants.DEFAULT_MAXIMUM_VELOCITY, + ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION + ).withEnableFOC(ClimberConstants.FOC_ENABLED); + private ClimberConstants.ClimberState targetState = ClimberConstants.ClimberState.REST; + + public Climber() { + setName("Climber"); + } + + @Override + public SysIdRoutine.Config getSysIDConfig() { + return ClimberConstants.SYSID_CONFIG; + } + + @Override + public void setBrake(boolean brake) { + motor.setBrake(brake); + } + + @Override + public void stop() { + motor.stopMotor(); + } + + @Override + public void updateLog(SysIdRoutineLog log) { + log.motor("Climber") + .angularPosition(Units.Rotations.of(getPositionRotations())) + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + } + + @Override + public void updateMechanism() { + ClimberConstants.MECHANISM.update( + getPositionRotations(), + motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ); + + Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); + } + + @Override + public void updatePeriodically() { + motor.update(); + } + + @Override + public void sysIDDrive(double targetVoltage) { + motor.setControl(voltageRequest.withOutput(targetVoltage)); + } + + + public boolean atTargetExtendedState() { + return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; + } + + void setTargetExtendedState(ClimberConstants.ClimberState targetState) { + this.targetState = targetState; + scalePositionRequestSpeed(targetState.extendedSpeedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters), ClimberConstants.CLIMBER_WEIGHT_SLOT); + } + + void setTargetRetractedState() { + scalePositionRequestSpeed(targetState.retractedSpeedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters), ClimberConstants.ROBOT_WEIGHT_SLOT); + } + + void setTargetPositionRotations(double targetPositionRotations, int slot) { + motor.setControl(positionRequest.withPosition(targetPositionRotations).withSlot(slot)); + } + + void resetPosition() { + targetState = ClimberConstants.ClimberState.REST; + motor.setControl(voltageRequest.withOutput(ClimberConstants.CLIMBER_RESET_VOLTAGE)); + } + + private void scalePositionRequestSpeed(double speedScalar) { + positionRequest.Velocity = ClimberConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; + positionRequest.Acceleration = ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + positionRequest.Jerk = positionRequest.Acceleration * 10; + } + + private double calculateTargetExtendedStateDistance() { + return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters()); + } + + private Pose3d calculateComponentPose() { + return ClimberConstants.CLIMBER_VISUALIZATION_ORIGIN_POINT.transformBy( + new Transform3d( + new Translation3d(0, 0, getPositionMeters()), + new Rotation3d() + ) + ); + } + + private double metersToRotations(double positionMeters) { + return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); + } + + private double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + + + private double getPositionRotations() { + return motor.getSignal(TalonFXSignal.POSITION); + } + + private double rotationsToMeters(double positionRotations) { + return Conversions.rotationsToDistance(positionRotations, ClimberConstants.DRUM_DIAMETER_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java new file mode 100644 index 0000000..d039927 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -0,0 +1,52 @@ +package frc.trigon.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.trigon.lib.commands.NetworkTablesCommand; +import frc.trigon.robot.RobotContainer; + +import java.util.Set; + +public class ClimberCommands { + public static Command getDebuggingCommand() { + return new NetworkTablesCommand( + (targetPosition) -> RobotContainer.CLIMBER.setTargetPositionRotations(targetPosition, ClimberConstants.CLIMBER_WEIGHT_SLOT), + false, + Set.of(RobotContainer.CLIMBER), + "Debugging/ClimberTargetPositionRotations" + ); + } + + public static Command getResetPositionCommand() { + return new StartEndCommand( + RobotContainer.CLIMBER::resetPosition, + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + + public static Command getSetTargetVoltageCommand(double targetVoltage) { + return new StartEndCommand( + () -> RobotContainer.CLIMBER.sysIDDrive(targetVoltage), + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + + public static Command getSetTargetExtendedStateCommand(ClimberConstants.ClimberState targetState) { + return new StartEndCommand( + () -> RobotContainer.CLIMBER.setTargetExtendedState(targetState), + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + + public static Command getSetTargetRetractedStateCommand() { + return new StartEndCommand( + RobotContainer.CLIMBER::setTargetRetractedState, + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + +} diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..81c8d9f --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,147 @@ +package frc.trigon.robot.subsystems.climber; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.hardware.simulation.ElevatorSimulation; +import frc.trigon.lib.utilities.mechanisms.ElevatorMechanism2d; + +public class ClimberConstants { + private static final int MOTOR_ID = 18; + private static final String MOTOR_NAME = "ClimberMasterMotor"; + static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); + + private static final double GEAR_RATIO = 280; + static final double + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, + DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; + static final boolean FOC_ENABLED = true; + + static final double MINIMUM_CLIMBER_HEIGHT_METERS = 0.73; + private static final int MOTOR_AMOUNT = 1; + private static final DCMotor GEARBOX = DCMotor.getKrakenX44Foc(MOTOR_AMOUNT); + private static final double + CLIMBER_MASS_KILOGRAMS = 60, + DRUM_RADIUS_METERS = 0.04, + MAXIMUM_CLIMBER_HEIGHT_METERS = 1.8; + private static final boolean SHOULD_SIMULATE_GRAVITY = true; + private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( + GEARBOX, + GEAR_RATIO, + CLIMBER_MASS_KILOGRAMS, + DRUM_RADIUS_METERS, + MINIMUM_CLIMBER_HEIGHT_METERS, + MAXIMUM_CLIMBER_HEIGHT_METERS, + SHOULD_SIMULATE_GRAVITY + ); + + static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(1.25).per(Units.Seconds), + Units.Volts.of(3), + Units.Second.of(1000) + ); + + public static final Pose3d CLIMBER_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0) + ); + + private static final String MECHANISM_NAME = "ClimberMechanism"; + private static final Color MECHANISM_COLOR = Color.kYellow; + static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( + MECHANISM_NAME, + MAXIMUM_CLIMBER_HEIGHT_METERS, + MINIMUM_CLIMBER_HEIGHT_METERS, + MECHANISM_COLOR + ); + + static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; + static final double POSITION_TOLERANCE_METERS = 0.07; + static final double CLIMBER_RESET_VOLTAGE = -0.5; + static final int CLIMBER_WEIGHT_SLOT = 0; + static final int ROBOT_WEIGHT_SLOT = 1; + + static { + configureMotor(); + } + + private static void configureMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0087929 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 2.7126 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.052127 : 0; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.Slot1.kP = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kS = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kV = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kA = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : 0; + + config.HardwareLimitSwitch.ReverseLimitEnable = true; + config.HardwareLimitSwitch.ReverseLimitType = ReverseLimitTypeValue.NormallyOpen; + config.HardwareLimitSwitch.ReverseLimitSource = ReverseLimitSourceValue.LimitSwitchPin; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0; + + config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = 80; + + MOTOR.applyConfiguration(config); + MOTOR.setPhysicsSimulation(SIMULATION); + + MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + } + + public enum ClimberState { + REST(0, 0, 0.7, 0.7), + CLIMB_L1(0.5, 0, 1, 0.5); + + public final double targetExtendedPositionMeters; + public final double targetRetractedPositionMeters; + final double extendedSpeedScalar; + final double retractedSpeedScalar; + + ClimberState(double targetExtendedPositionMeters, double targetRetractedPositionMeters, double extendedSpeedScalar, double retractedSpeedScalar) { + this.targetExtendedPositionMeters = targetExtendedPositionMeters; + this.targetRetractedPositionMeters = targetRetractedPositionMeters; + this.extendedSpeedScalar = extendedSpeedScalar; + this.retractedSpeedScalar = retractedSpeedScalar; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java index 31c43b7..5409b9c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java @@ -19,7 +19,7 @@ public class HoodConstants { private static final int - MOTOR_ID = 18, + MOTOR_ID = 17, ENCODER_ID = 18; private static final String MOTOR_NAME = "HoodMotor", diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 7e7c5b9..ad41af9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -16,8 +16,8 @@ public class ShooterConstants { private static final int - MASTER_MOTOR_ID = 16, - FOLLOWER_MOTOR_ID = 17; + MASTER_MOTOR_ID = 15, + FOLLOWER_MOTOR_ID = 16; private static final String MASTER_MOTOR_NAME = "ShooterMasterMotor", FOLLOWER_MOTOR_NAME = "ShooterFollowerMotor";