diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 40262dbf..37fda07c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -21,6 +21,9 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.elevator.Elevator; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.arm.Arm; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; @@ -50,6 +53,7 @@ public class RobotContainer { public static final Swerve SWERVE = new Swerve(); public static final Arm ARM = new Arm(); public static final Climber CLIMBER = new Climber(); + public static final Elevator ELEVATOR = new Elevator(); public static final Intake INTAKE = new Intake(); public static final Transporter TRANSPORTER = new Transporter(); @@ -77,6 +81,7 @@ private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); ARM.setDefaultCommand(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST)); CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); + ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 67ba1e9a..d1d9c6e4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -48,7 +48,7 @@ public class ClimberConstants { GROUNDED_PID_SLOT = 0, ON_CAGE_PID_SLOT = 1; private static final double GEAR_RATIO = 37.5; - private static final double REVERSE_LIMIT_SENSOR_RESET_POSITION = 0; + private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3; static final boolean FOC_ENABLED = true; @@ -129,7 +129,7 @@ private static void configureMotor() { config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -147,7 +147,7 @@ private static void configureMotor() { private static void configureReverseLimitSensor() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION)); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); } public enum ClimberState { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java new file mode 100644 index 00000000..7063a1d5 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,135 @@ +package frc.trigon.robot.subsystems.elevator; + +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.robot.subsystems.MotorSubsystem; +import org.littletonrobotics.junction.Logger; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import lib.utilities.Conversions; + +public class Elevator extends MotorSubsystem { + private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ElevatorConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + 0, + ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY, + ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION, + ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10 + ).withEnableFOC(ElevatorConstants.FOC_ENABLED); + private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST; + + public Elevator() { + setName("Elevator"); + } + + @Override + public SysIdRoutine.Config getSysIDConfig() { + return ElevatorConstants.SYSID_CONFIG; + } + + @Override + public void setBrake(boolean brake) { + masterMotor.setBrake(brake); + } + + @Override + public void stop() { + masterMotor.stopMotor(); + } + + @Override + public void updateLog(SysIdRoutineLog log) { + log.motor("Elevator") + .linearPosition(Units.Meters.of(getPositionRotations())) + .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + } + + @Override + public void sysIDDrive(double targetVoltage) { + masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + } + + @Override + public void updateMechanism() { + ElevatorConstants.MECHANISM.update( + getPositionMeters(), + rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + + Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); + Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); + } + + @Override + public void updatePeriodically() { + masterMotor.update(); + Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); + } + + void setTargetState(ElevatorConstants.ElevatorState targetState) { + this.targetState = targetState; + scalePositionRequestSpeed(targetState.speedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); + } + + void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + } + + private Pose3d getFirstStageComponentPose() { + return calculateComponentPose(ElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight()); + } + + private Pose3d getSecondStageComponentPose() { + return calculateComponentPose(ElevatorConstants.ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); + } + + private double getFirstStageComponentPoseHeight() { + if (isSecondStageComponentLimitReached()) + return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + return 0; + } + + private boolean isSecondStageComponentLimitReached() { + return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + } + + private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) { + final Transform3d elevatorTransform = new Transform3d( + new Translation3d(0, 0, currentHeight), + new Rotation3d() + ); + return originPoint.transformBy(elevatorTransform); + } + + private void scalePositionRequestSpeed(double speedScalar) { + positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; + positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + positionRequest.Jerk = positionRequest.Acceleration * 10; + } + + private double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + + private double rotationsToMeters(double positionsRotations) { + return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); + } + + private double metersToRotations(double positionsMeters) { + return Conversions.distanceToRotations(positionsMeters, ElevatorConstants.DRUM_DIAMETER_METERS); + } + + private double getPositionRotations() { + return masterMotor.getSignal(TalonFXSignal.POSITION); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java new file mode 100644 index 00000000..aba71757 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -0,0 +1,35 @@ +package frc.trigon.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.trigon.robot.RobotContainer; +import lib.commands.NetworkTablesCommand; + +import java.util.Set; + +public class ElevatorCommands { + public static Command getDebbugingCommand() { + return new NetworkTablesCommand( + RobotContainer.ELEVATOR::setTargetPositionRotations, + false, + Set.of(RobotContainer.ELEVATOR), + "Debugging/ElevatorTargetPositionRotations" + ); + } + + public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { + return new StartEndCommand( + () -> RobotContainer.ELEVATOR.setTargetState(targetState), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } + + public static Command getSetTargetStateCommand(double targetPositionRotations) { + return new StartEndCommand( + () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 00000000..b56f6d03 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,184 @@ +package frc.trigon.robot.subsystems.elevator; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +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.event.BooleanEvent; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import lib.hardware.RobotHardwareStats; +import lib.hardware.misc.simplesensor.SimpleSensor; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import lib.hardware.simulation.ElevatorSimulation; +import lib.utilities.mechanisms.ElevatorMechanism2d; + +import java.util.function.DoubleSupplier; + +public class ElevatorConstants { + private static final int + MASTER_MOTOR_ID = 16, + FOLLOWER_MOTOR_ID = 17, + REVERSE_LIMIT_SENSOR_CHANNEL = 0; + private static final String + MASTER_MOTOR_NAME = "ElevatorMasterMotor", + FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", + REVERSE_LIMIT_SENSOR_NAME = "ElevatorReverseLimitSensor"; + static final TalonFXMotor + MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), + FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); + private static final SimpleSensor REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); + + private static final double GEAR_RATIO = 4; + private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; + private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; + static final double + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, + DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; + static final boolean FOC_ENABLED = true; + + private static final int MOTOR_AMOUNT = 2; + private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); + private static final double + ELEVATOR_MASS_KILOGRAMS = 7, + DRUM_RADIUS_METERS = 0.04, + MINIMUM_ELEVATOR_HEIGHT_METERS = 0, + MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.382; + private static final boolean SHOULD_SIMULATE_GRAVITY = true; + private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( + GEARBOX, + GEAR_RATIO, + ELEVATOR_MASS_KILOGRAMS, + DRUM_RADIUS_METERS, + MINIMUM_ELEVATOR_HEIGHT_METERS, + MAXIMUM_ELEVATOR_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 ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, -0.17, 0.0504), + new Rotation3d(0, 0, 0) + ); + static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, -0.17 ,0.0814), + new Rotation3d(0, 0, 0) + ); + static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( + "ElevatorMechanism", + MAXIMUM_ELEVATOR_HEIGHT_METERS + 0.1, + MINIMUM_ELEVATOR_HEIGHT_METERS, + Color.kYellow + ); + static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.603; + static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; + private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; + private static final BooleanEvent REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + REVERSE_LIMIT_SENSOR::getBinaryValue + ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + + private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; + + static { + configureMasterMotor(); + configureFollowerMotor(); + configureReverseLimitSensor(); + } + + private static void configureMasterMotor() { + 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() ? 3.5:0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4:0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165:0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766:0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239:0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202:0; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; + + config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + MASTER_MOTOR.applyConfiguration(config); + MASTER_MOTOR.setPhysicsSimulation(SIMULATION); + + MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureFollowerMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower followerRequest = new Follower(MASTER_MOTOR.getID(), SHOULD_FOLLOWER_OPPOSE_MASTER); + FOLLOWER_MOTOR.setControl(followerRequest); + } + + private static void configureReverseLimitSensor() { + REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); + } + + public enum ElevatorState { + REST(0.603, 0.7), + SCORE_L1(0.603, 1), + SCORE_L2(0.603, 1), + SCORE_L3(1.003, 1), + SCORE_L4(1.382, 1), + PREPARE_L1(0.603, 1), + PREPARE_L2(0.603, 1), + PREPARE_L3(1.003, 1), + PREPARE_L4(1.382, 1), + COLLECT_ALGAE_FROM_L2(0.603, 1), + COLLECT_ALGAE_FROM_L3(0.953, 1), + COLLECT_ALGAE_FROM_GROUND(0, 0.7), + REST_WITH_ALGAE(0.603, 0.3), + SCORE_NET(1.382, 0.3); + + public final double targetPositionMeters; + final double speedScalar; + + ElevatorState(double targetPositionMeters, double speedScalar) { + this.targetPositionMeters = targetPositionMeters; + this.speedScalar = speedScalar; + } + } +}