From 53851413831aa89fc489c0b5b1271704dd279f1f Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Mon, 1 Sep 2025 21:00:02 +0300 Subject: [PATCH 01/21] made elevator --- .../java/frc/trigon/robot/RobotContainer.java | 2 + .../robot/subsystems/MotorSubsystem.java | 4 + .../robot/subsystems/elevator/Elevator.java | 93 ++++++++++ .../subsystems/elevator/ElevatorCommands.java | 35 ++++ .../elevator/ElevatorConstants.java | 166 ++++++++++++++++++ 5 files changed, 300 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 1a871d6e..28e38297 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; 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.swerve.Swerve; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import trigon.utilities.flippable.Flippable; @@ -32,6 +33,7 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); + public static final Elevator ELEVATOR = new Elevator(); private LoggedDashboardChooser autoChooser; public RobotContainer() { diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 75b736f9..5dc2ce76 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -154,6 +154,8 @@ public void changeDefaultCommand(Command newDefaultCommand) { setDefaultCommand(newDefaultCommand); } + public abstract SysIdRoutine.Config getSysIdConfig(); + public abstract void stop(); private SysIdRoutine createSysIDRoutine() { @@ -170,4 +172,6 @@ private SysIdRoutine createSysIDRoutine() { ) ); } + + public abstract void sysIdDrive(double targetVoltage); } \ No newline at end of file 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..5f356889 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,93 @@ +package frc.trigon.robot.subsystems.elevator; + +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +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 trigon.hardware.phoenix6.talonfx.TalonFXMotor; +import trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import trigon.utilities.Conversions; + +public class Elevator extends MotorSubsystem { + private final TalonFXMotor + masterMotor = ElevatorConstants.MASTER_MOTOR, + followerMotor = ElevatorConstants.FOLLOWER_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); + followerMotor.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( + getPositionsMeters(), + rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + } + + public double rotationsToMeters(double positionsRotations) { + return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); + } + + public double metersToRotations(double positionsMeters) { + return Conversions.distanceToRotations(positionsMeters, ElevatorConstants.DRUM_DIAMETER_METERS); + } + + void setTargetState(ElevatorConstants.ElevatorState targetState) { + this.targetState = targetState; + scalePositionRequestSpeed(targetState.speedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); + } + + private void scalePositionRequestSpeed(double speedScalar) { + positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY; + positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION; + positionRequest.Jerk = positionRequest.Acceleration * 10; + } + + void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + } + + private double getPositionRotations() { + return masterMotor.getSignal(TalonFXSignal.POSITION); + } + + private double getPositionsMeters() { + return rotationsToMeters(getPositionRotations()); + } +} 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..2c9eb4bb --- /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 trigon.commands.NetworkTablesCommand; + +import java.util.Set; + +public class ElevatorCommands { + public static Command getDebbugingCommand() { + return new NetworkTablesCommand( + RobotContainer.ELEVATOR::setTargetPositionRotations, + false, + Set.of(RobotContainer.ELEVATOR), + "Debuggin/ElevatorTargetPositionRotations" + ); + } + + public static Command getSetTargetVoltageCommand(double targetVoltage) { + return new StartEndCommand( + () -> RobotContainer.ELEVATOR.sysIdDrive(targetVoltage), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } + + public static Command getSetTargetVoltageCommand(ElevatorConstants.ElevatorState targetState) { + return new StartEndCommand( + () -> RobotContainer.ELEVATOR.setTargetState(targetState), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } +} 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..38b4efc4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,166 @@ +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.util.Color; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import trigon.hardware.RobotHardwareStats; +import trigon.hardware.phoenix6.talonfx.TalonFXMotor; +import trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import trigon.hardware.simulation.ElevatorSimulation; +import trigon.utilities.mechanisms.ElevatorMechanism2d; + +public class ElevatorConstants { + private static final int + MASTER_MOTOR_ID = 21, + FOLLOWER_MOTOR_ID = 22; + private static final String + MASTER_MOTOR_NAME = "ElevatorMasterMotor", + FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor"; + 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 double GEAR_RATIO = 4; + private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = true; + static final double + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0 : 0, + DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 0 : 0; + static final boolean FOC_ENABLED = true; + + private static final int MOTOR_AMOUNT = 2; + private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); + private static final double + ELEVATOR_MASS_KILOGRAMS = 7, + DRUM_RADIUS_METERS = 0.050, + MINIMUM_ELEVATOR_HEIGHT_METERS = 0.73, + MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.8; + 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 FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0) + ); + static final Pose3d SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0 ,0), + 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 DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; + + + + static { + configureMasterMotor(); + configureFollowerMotor(); + } + + 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() ? 0:0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0:0; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.HardwareLimitSwitch.ReverseLimitEnable = true; + config.HardwareLimitSwitch.ReverseLimitType = ReverseLimitTypeValue.NormallyOpen; + config.HardwareLimitSwitch.ReverseLimitSource = ReverseLimitSourceValue.LimitSwitchPin; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = 0; + + config.HardwareLimitSwitch.ForwardLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + + 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.CounterClockwise_Positive; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower followerRequest = new Follower(MASTER_MOTOR.getID(), SHOULD_FOLLOWER_OPPOSE_MASTER); + FOLLOWER_MOTOR.setControl(followerRequest); + } + + public enum ElevatorState { + REST(0, 0.7), + SCORE_L1(0, 1), + SCORE_L2(0, 1), + SCORE_L3(0.4, 1), + SCORE_L4(1.045, 1), + COLLECT_ALGAE_FROM_L3(0.35, 1), + REST_WITH_ALGAE(0, 0.3), + SCORE_NET(1.045, 0.3); + + public final double targetPositionMeters; + final double speedScalar; + + ElevatorState(double targetPositionMeters, double speedScalar) { + this.targetPositionMeters = targetPositionMeters; + this.speedScalar = speedScalar; + } + } +} From d8d27d73406c878f806aa0b6fd5c16fb84d7690f Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Mon, 1 Sep 2025 21:07:40 +0300 Subject: [PATCH 02/21] im stupid --- src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java | 3 --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- .../trigon/robot/subsystems/elevator/ElevatorCommands.java | 2 +- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 5dc2ce76..960658e6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -154,8 +154,6 @@ public void changeDefaultCommand(Command newDefaultCommand) { setDefaultCommand(newDefaultCommand); } - public abstract SysIdRoutine.Config getSysIdConfig(); - public abstract void stop(); private SysIdRoutine createSysIDRoutine() { @@ -173,5 +171,4 @@ private SysIdRoutine createSysIDRoutine() { ); } - public abstract void sysIdDrive(double targetVoltage); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 5f356889..84f7bcf1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -23,7 +23,7 @@ public Elevator() { } @Override - public SysIdRoutine.Config getSysIdConfig() { + public SysIdRoutine.Config getSysIDConfig() { return ElevatorConstants.SYSID_CONFIG; } @@ -47,7 +47,7 @@ public void updateLog(SysIdRoutineLog log) { } @Override - public void sysIdDrive(double targetVoltage) { + public void sysIDDrive(double targetVoltage) { masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index 2c9eb4bb..429030f4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -19,7 +19,7 @@ public static Command getDebbugingCommand() { public static Command getSetTargetVoltageCommand(double targetVoltage) { return new StartEndCommand( - () -> RobotContainer.ELEVATOR.sysIdDrive(targetVoltage), + () -> RobotContainer.ELEVATOR.sysIDDrive(targetVoltage), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ); From 978d1b35aaf42c172c9bb2c0fe96a1a78ae90e4a Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Tue, 2 Sep 2025 18:50:54 +0300 Subject: [PATCH 03/21] added reverse limit switch constants --- .../java/frc/trigon/robot/RobotContainer.java | 3 ++ .../robot/subsystems/MotorSubsystem.java | 1 - .../subsystems/elevator/ElevatorCommands.java | 2 +- .../elevator/ElevatorConstants.java | 28 ++++++++++++++++--- 4 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 28e38297..61794af2 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,6 +20,8 @@ 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.swerve.Swerve; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import trigon.utilities.flippable.Flippable; @@ -56,6 +58,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); + ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); } /** diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 960658e6..75b736f9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -170,5 +170,4 @@ private SysIdRoutine createSysIDRoutine() { ) ); } - } \ 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 index 429030f4..39065edf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -25,7 +25,7 @@ public static Command getSetTargetVoltageCommand(double targetVoltage) { ); } - public static Command getSetTargetVoltageCommand(ElevatorConstants.ElevatorState targetState) { + public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { return new StartEndCommand( () -> RobotContainer.ELEVATOR.setTargetState(targetState), RobotContainer.ELEVATOR::stop, diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 38b4efc4..b3313c97 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -8,26 +8,35 @@ 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 trigon.hardware.RobotHardwareStats; +import trigon.hardware.misc.simplesensor.SimpleSensor; import trigon.hardware.phoenix6.talonfx.TalonFXMotor; import trigon.hardware.phoenix6.talonfx.TalonFXSignal; import trigon.hardware.simulation.ElevatorSimulation; import trigon.utilities.mechanisms.ElevatorMechanism2d; +import java.util.function.DoubleSupplier; + public class ElevatorConstants { private static final int MASTER_MOTOR_ID = 21, - FOLLOWER_MOTOR_ID = 22; + FOLLOWER_MOTOR_ID = 22, + REVERSE_LIMIT_SENSOR_CHANNEL = 0; private static final String MASTER_MOTOR_NAME = "ElevatorMasterMotor", - FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor"; + FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", + REVERSE_LIMIT_SWITCH_NAME = "ElevatorReverseLimitSwitch"; 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_SWITCH_NAME); private static final double GEAR_RATIO = 4; + private static final double REVERSE_LIMIT_SWITCH_RESET_POSITION = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = true; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0 : 0, @@ -72,13 +81,13 @@ public class ElevatorConstants { Color.kYellow ); + private static final DoubleSupplier REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER = () -> 0; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; - - static { configureMasterMotor(); configureFollowerMotor(); + configureReverseLimitSwitch(); } private static void configureMasterMotor() { @@ -145,6 +154,17 @@ private static void configureFollowerMotor() { FOLLOWER_MOTOR.setControl(followerRequest); } + private static void configureReverseLimitSwitch() { + REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER); + + final BooleanEvent reverseLimitSwitchBooleanEvent = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + REVERSE_LIMIT_SENSOR::getBinaryValue + ); + } + + + public enum ElevatorState { REST(0, 0.7), SCORE_L1(0, 1), From 2959e642fe92cc84afa271b32bb789d000191745 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Tue, 2 Sep 2025 20:21:26 +0300 Subject: [PATCH 04/21] fixed minor changes --- .../trigon/robot/subsystems/elevator/Elevator.java | 13 +++++-------- .../subsystems/elevator/ElevatorConstants.java | 10 +++++----- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 84f7bcf1..91718382 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -11,9 +11,7 @@ import trigon.utilities.Conversions; public class Elevator extends MotorSubsystem { - private final TalonFXMotor - masterMotor = ElevatorConstants.MASTER_MOTOR, - followerMotor = ElevatorConstants.FOLLOWER_MOTOR; + 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; @@ -30,7 +28,6 @@ public SysIdRoutine.Config getSysIDConfig() { @Override public void setBrake(boolean brake) { masterMotor.setBrake(brake); - followerMotor.setBrake(brake); } @Override @@ -83,11 +80,11 @@ void setTargetPositionRotations(double targetPositionRotations) { masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); } - private double getPositionRotations() { - return masterMotor.getSignal(TalonFXSignal.POSITION); - } - private double getPositionsMeters() { return rotationsToMeters(getPositionRotations()); } + + private double getPositionRotations() { + return masterMotor.getSignal(TalonFXSignal.POSITION); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b3313c97..38f477be 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -37,17 +37,17 @@ public class ElevatorConstants { private static final double GEAR_RATIO = 4; private static final double REVERSE_LIMIT_SWITCH_RESET_POSITION = 0; - private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = true; + private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0 : 0, DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 0 : 0; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; - private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); + private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); private static final double ELEVATOR_MASS_KILOGRAMS = 7, - DRUM_RADIUS_METERS = 0.050, + DRUM_RADIUS_METERS = 0.05, MINIMUM_ELEVATOR_HEIGHT_METERS = 0.73, MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.8; private static final boolean SHOULD_SIMULATE_GRAVITY = true; @@ -146,7 +146,7 @@ private static void configureFollowerMotor() { config.Audio.BeepOnConfig = false; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; FOLLOWER_MOTOR.applyConfiguration(config); @@ -156,7 +156,7 @@ private static void configureFollowerMotor() { private static void configureReverseLimitSwitch() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER); - + final BooleanEvent reverseLimitSwitchBooleanEvent = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), REVERSE_LIMIT_SENSOR::getBinaryValue From 6146f3a07a051c006d3b4515597fa6e07044b0f6 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Wed, 3 Sep 2025 19:34:55 +0300 Subject: [PATCH 05/21] added poses and did sysid --- .../robot/subsystems/elevator/Elevator.java | 49 +++++++++++++++++++ .../elevator/ElevatorConstants.java | 19 +++---- 2 files changed, 59 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 91718382..416eb21c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -1,11 +1,17 @@ package frc.trigon.robot.subsystems.elevator; import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.FireAnimation; 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 trigon.hardware.phoenix6.talonfx.TalonFXMotor; import trigon.hardware.phoenix6.talonfx.TalonFXSignal; import trigon.utilities.Conversions; @@ -50,12 +56,21 @@ public void sysIDDrive(double targetVoltage) { @Override public void updateMechanism() { + Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); + Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); + ElevatorConstants.MECHANISM.update( getPositionsMeters(), rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); } + @Override + public void updatePeriodically() { + masterMotor.update(); + Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionsMeters()); + } + public double rotationsToMeters(double positionsRotations) { return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); } @@ -70,6 +85,40 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); } + public Pose3d getFirstStageComponentPose() { + return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT); + } + + public Pose3d getSecondStageComponentPose() { + return calculateComponentPose(getSecondPoseHeight(), ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT); + } + + private double getSecondPoseHeight() { + if (firstComponentLimitReached()) + return getPositionsMeters() - ElevatorConstants.FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + return 0; + } + + private boolean firstComponentLimitReached() { + return getPositionsMeters() > ElevatorConstants.FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + } + + private Pose3d calculateCurrentElevatorPoseFromOrigin(Pose3d originPoint) { + return calculateComponentPose(getPositionsMeters(), originPoint); + } + + private Pose3d calculateComponentPose(double poseHeight, Pose3d originPoint) { + final Transform3d elevatorTransform = new Transform3d( + new Translation3d(0, 0, poseHeight), + new Rotation3d() + ); + return originPoint.transformBy(elevatorTransform); + } + + + + + private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY; positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 38f477be..83de6059 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -23,8 +23,8 @@ public class ElevatorConstants { private static final int - MASTER_MOTOR_ID = 21, - FOLLOWER_MOTOR_ID = 22, + MASTER_MOTOR_ID = 16, + FOLLOWER_MOTOR_ID = 17, REVERSE_LIMIT_SENSOR_CHANNEL = 0; private static final String MASTER_MOTOR_NAME = "ElevatorMasterMotor", @@ -39,8 +39,8 @@ public class ElevatorConstants { private static final double REVERSE_LIMIT_SWITCH_RESET_POSITION = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double - DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0 : 0, - DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 0 : 0; + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, + DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; @@ -82,6 +82,7 @@ public class ElevatorConstants { ); private static final DoubleSupplier REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER = () -> 0; + static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.6; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static { @@ -100,13 +101,13 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0.15526:0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0:0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.020957:0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.47532:0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.02273:0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.72511:0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; From 6e05c54ad827286f63b8728684b3ebb4fda2b3db Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Fri, 5 Sep 2025 01:07:27 +0300 Subject: [PATCH 06/21] stuff --- .../java/frc/trigon/robot/RobotContainer.java | 1 + .../robot/constants/OperatorConstants.java | 3 +- .../robot/subsystems/elevator/Elevator.java | 50 +++++++++---------- .../subsystems/elevator/ElevatorCommands.java | 8 --- .../elevator/ElevatorConstants.java | 42 +++++++--------- 5 files changed, 43 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 61794af2..47ddd2ab 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -76,6 +76,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.TEST.whileTrue(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L3)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 614b73f2..bc0295c0 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -33,5 +33,6 @@ public class OperatorConstants { FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), - BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), + TEST = OPERATOR_CONTROLLER.w(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 416eb21c..ab895519 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -56,13 +56,13 @@ public void sysIDDrive(double targetVoltage) { @Override public void updateMechanism() { - Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); - Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); - ElevatorConstants.MECHANISM.update( getPositionsMeters(), rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); + + Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); + Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); } @Override @@ -71,26 +71,22 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionsMeters()); } - public double rotationsToMeters(double positionsRotations) { - return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); - } - - public double metersToRotations(double positionsMeters) { - return Conversions.distanceToRotations(positionsMeters, ElevatorConstants.DRUM_DIAMETER_METERS); - } - void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); } - public Pose3d getFirstStageComponentPose() { + void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + } + + private Pose3d getFirstStageComponentPose() { return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT); } - public Pose3d getSecondStageComponentPose() { - return calculateComponentPose(getSecondPoseHeight(), ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT); + private Pose3d getSecondStageComponentPose() { + return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getSecondPoseHeight()); } private double getSecondPoseHeight() { @@ -104,10 +100,10 @@ private boolean firstComponentLimitReached() { } private Pose3d calculateCurrentElevatorPoseFromOrigin(Pose3d originPoint) { - return calculateComponentPose(getPositionsMeters(), originPoint); + return calculateComponentPose(originPoint, getPositionsMeters()); } - private Pose3d calculateComponentPose(double poseHeight, Pose3d originPoint) { + private Pose3d calculateComponentPose(Pose3d originPoint, double poseHeight) { final Transform3d elevatorTransform = new Transform3d( new Translation3d(0, 0, poseHeight), new Rotation3d() @@ -115,25 +111,25 @@ private Pose3d calculateComponentPose(double poseHeight, Pose3d originPoint) { return originPoint.transformBy(elevatorTransform); } - - - - private void scalePositionRequestSpeed(double speedScalar) { - positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY; - positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION; + positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; + positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; positionRequest.Jerk = positionRequest.Acceleration * 10; } - void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); - } - private double getPositionsMeters() { 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 index 39065edf..9519403a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -17,14 +17,6 @@ public static Command getDebbugingCommand() { ); } - public static Command getSetTargetVoltageCommand(double targetVoltage) { - return new StartEndCommand( - () -> RobotContainer.ELEVATOR.sysIDDrive(targetVoltage), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ); - } - public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { return new StartEndCommand( () -> RobotContainer.ELEVATOR.setTargetState(targetState), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 83de6059..f8b2965e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -47,9 +47,9 @@ public class ElevatorConstants { private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); private static final double ELEVATOR_MASS_KILOGRAMS = 7, - DRUM_RADIUS_METERS = 0.05, - MINIMUM_ELEVATOR_HEIGHT_METERS = 0.73, - MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.8; + DRUM_RADIUS_METERS = 0.04, + MINIMUM_ELEVATOR_HEIGHT_METERS = 0.78, + MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.752; private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( GEARBOX, @@ -81,8 +81,14 @@ public class ElevatorConstants { Color.kYellow ); + 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_SWITCH_SIMULATION_SUPPLIER = () -> 0; - static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.6; + static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static { @@ -101,25 +107,17 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0.15526:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 6:0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.020957:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.47532:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.02273:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.72511:0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.3:0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016527:0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4771:0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014197:0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58959:0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.HardwareLimitSwitch.ReverseLimitEnable = true; - config.HardwareLimitSwitch.ReverseLimitType = ReverseLimitTypeValue.NormallyOpen; - config.HardwareLimitSwitch.ReverseLimitSource = ReverseLimitSourceValue.LimitSwitchPin; - config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; - config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = 0; - - config.HardwareLimitSwitch.ForwardLimitEnable = false; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; @@ -157,15 +155,9 @@ private static void configureFollowerMotor() { private static void configureReverseLimitSwitch() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER); - - final BooleanEvent reverseLimitSwitchBooleanEvent = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SWITCH_RESET_POSITION)); } - - public enum ElevatorState { REST(0, 0.7), SCORE_L1(0, 1), From aef54d8f5c67a54fe4e0e095c3bc50ffaeeed96b Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Fri, 5 Sep 2025 01:20:00 +0300 Subject: [PATCH 07/21] limit switch incostent fixed --- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../robot/constants/OperatorConstants.java | 3 +-- .../subsystems/elevator/ElevatorCommands.java | 11 ++++++++++- .../subsystems/elevator/ElevatorConstants.java | 16 ++++++++-------- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 47ddd2ab..61794af2 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -76,7 +76,6 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.TEST.whileTrue(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L3)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index bc0295c0..614b73f2 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -33,6 +33,5 @@ public class OperatorConstants { FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), - BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), - TEST = OPERATOR_CONTROLLER.w(); + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); } \ 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 index 9519403a..ffca7fda 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -24,4 +24,13 @@ public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState t RobotContainer.ELEVATOR ); } -} + + + public static Command getSetTargetStateCommand(double targetRotations) { + return new StartEndCommand( + () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetRotations), + 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 index f8b2965e..2a30e505 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -29,14 +29,14 @@ public class ElevatorConstants { private static final String MASTER_MOTOR_NAME = "ElevatorMasterMotor", FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", - REVERSE_LIMIT_SWITCH_NAME = "ElevatorReverseLimitSwitch"; + 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_SWITCH_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_SWITCH_RESET_POSITION = 0; + private static final double REVERSE_LIMIT_SENSOR_RESET_POSITION = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, @@ -87,14 +87,14 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - private static final DoubleSupplier REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER = () -> 0; + private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static { configureMasterMotor(); configureFollowerMotor(); - configureReverseLimitSwitch(); + configureReverseLimitSensor(); } private static void configureMasterMotor() { @@ -153,9 +153,9 @@ private static void configureFollowerMotor() { FOLLOWER_MOTOR.setControl(followerRequest); } - private static void configureReverseLimitSwitch() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SWITCH_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SWITCH_RESET_POSITION)); + private static void configureReverseLimitSensor() { + REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION)); } public enum ElevatorState { From cf4f03d8879a9d4b4d0d3f02c388bcd674a9b873 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Fri, 5 Sep 2025 01:31:11 +0300 Subject: [PATCH 08/21] Changed trigon to lib nahum is mean --- .../trigon/robot/subsystems/elevator/Elevator.java | 6 +++--- .../robot/subsystems/elevator/ElevatorCommands.java | 2 +- .../robot/subsystems/elevator/ElevatorConstants.java | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index ab895519..943de291 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; -import trigon.hardware.phoenix6.talonfx.TalonFXMotor; -import trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import trigon.utilities.Conversions; +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; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index ffca7fda..bc6b5328 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; -import trigon.commands.NetworkTablesCommand; +import lib.commands.NetworkTablesCommand; import java.util.Set; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 2a30e505..350a99f4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -12,12 +12,12 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import trigon.hardware.RobotHardwareStats; -import trigon.hardware.misc.simplesensor.SimpleSensor; -import trigon.hardware.phoenix6.talonfx.TalonFXMotor; -import trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import trigon.hardware.simulation.ElevatorSimulation; -import trigon.utilities.mechanisms.ElevatorMechanism2d; +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; From 32ad2051135582b3d90e2f50f8f906e616799158 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Fri, 5 Sep 2025 01:44:31 +0300 Subject: [PATCH 09/21] Update ElevatorConstants.java --- .../trigon/robot/subsystems/elevator/ElevatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 350a99f4..8600879e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -107,9 +107,9 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 6:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 4:0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.3:0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0:0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016527:0; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4771:0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014197:0; From 64b20877b512e1a6894bf695f4bbd3cdb96fe7ca Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 5 Sep 2025 05:04:38 +0300 Subject: [PATCH 10/21] added proper visualization origin points --- .../trigon/robot/subsystems/elevator/ElevatorCommands.java | 3 +-- .../trigon/robot/subsystems/elevator/ElevatorConstants.java | 5 +++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index bc6b5328..9507d949 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -24,8 +24,7 @@ public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState t RobotContainer.ELEVATOR ); } - - + public static Command getSetTargetStateCommand(double targetRotations) { return new StartEndCommand( () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetRotations), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 350a99f4..c8d59cf6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -67,11 +67,11 @@ public class ElevatorConstants { ); public static final Pose3d FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0, 0), + new Translation3d(0, -0.17, 0.0504), new Rotation3d(0, 0, 0) ); static final Pose3d SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0 ,0), + new Translation3d(0, -0.17 ,0.684), new Rotation3d(0, 0, 0) ); static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( @@ -164,6 +164,7 @@ public enum ElevatorState { SCORE_L2(0, 1), SCORE_L3(0.4, 1), SCORE_L4(1.045, 1), + COLLECT_ALGAE_FROM_L2(0, 1), COLLECT_ALGAE_FROM_L3(0.35, 1), REST_WITH_ALGAE(0, 0.3), SCORE_NET(1.045, 0.3); From f02551888e86af58ec4bd92879b02b5f9e535b45 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 5 Sep 2025 05:34:20 +0300 Subject: [PATCH 11/21] Update ElevatorCommands.java --- .../trigon/robot/subsystems/elevator/ElevatorCommands.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index 9507d949..aba71757 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -13,7 +13,7 @@ public static Command getDebbugingCommand() { RobotContainer.ELEVATOR::setTargetPositionRotations, false, Set.of(RobotContainer.ELEVATOR), - "Debuggin/ElevatorTargetPositionRotations" + "Debugging/ElevatorTargetPositionRotations" ); } @@ -25,9 +25,9 @@ public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState t ); } - public static Command getSetTargetStateCommand(double targetRotations) { + public static Command getSetTargetStateCommand(double targetPositionRotations) { return new StartEndCommand( - () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetRotations), + () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ); From 341a63eb740d37c607d698fa5f63d3beaf7d7a5a Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Sun, 7 Sep 2025 19:45:44 +0300 Subject: [PATCH 12/21] Tuned elevator. --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 + .../trigon/robot/constants/OperatorConstants.java | 3 ++- .../robot/subsystems/elevator/ElevatorConstants.java | 12 ++++++------ 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 4e69b7df..6589ca12 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -100,6 +100,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.TEST.whileTrue(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L3)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 6c0c769c..39a792a7 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -37,5 +37,6 @@ public class OperatorConstants { FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), - BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), + TEST = OPERATOR_CONTROLLER.w(); } \ 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 index 93fa821f..cebc6a7f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -107,13 +107,13 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 4:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 1.8:0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016527:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4771:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014197:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58959:0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.2: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; From 47752cfe2de054b3cf714177429d1836c0b5ca75 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Mon, 8 Sep 2025 16:27:24 +0300 Subject: [PATCH 13/21] blar blar --- .../java/frc/trigon/robot/RobotContainer.java | 5 ++--- .../robot/constants/OperatorConstants.java | 3 +-- .../robot/subsystems/elevator/Elevator.java | 22 +++++-------------- .../elevator/ElevatorConstants.java | 15 ++++++++----- 4 files changed, 19 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6589ca12..7f597e83 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -50,9 +50,9 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); - public static final Elevator ELEVATOR = new Elevator(); 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(); @@ -78,9 +78,9 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); - ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); 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)); } @@ -100,7 +100,6 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.TEST.whileTrue(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L3)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 39a792a7..6c0c769c 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -37,6 +37,5 @@ public class OperatorConstants { FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), - BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), - TEST = OPERATOR_CONTROLLER.w(); + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 943de291..9c9ab7f3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.elevator; import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import com.ctre.phoenix6.controls.FireAnimation; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -18,6 +17,7 @@ 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; @@ -57,7 +57,7 @@ public void sysIDDrive(double targetVoltage) { @Override public void updateMechanism() { ElevatorConstants.MECHANISM.update( - getPositionsMeters(), + getPositionMeters(), rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); @@ -68,7 +68,7 @@ public void updateMechanism() { @Override public void updatePeriodically() { masterMotor.update(); - Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionsMeters()); + Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } void setTargetState(ElevatorConstants.ElevatorState targetState) { @@ -86,21 +86,11 @@ private Pose3d getFirstStageComponentPose() { } private Pose3d getSecondStageComponentPose() { - return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getSecondPoseHeight()); - } - - private double getSecondPoseHeight() { - if (firstComponentLimitReached()) - return getPositionsMeters() - ElevatorConstants.FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; - return 0; - } - - private boolean firstComponentLimitReached() { - return getPositionsMeters() > ElevatorConstants.FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT) } private Pose3d calculateCurrentElevatorPoseFromOrigin(Pose3d originPoint) { - return calculateComponentPose(originPoint, getPositionsMeters()); + return calculateComponentPose(originPoint, getPositionMeters()); } private Pose3d calculateComponentPose(Pose3d originPoint, double poseHeight) { @@ -117,7 +107,7 @@ private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Jerk = positionRequest.Acceleration * 10; } - private double getPositionsMeters() { + private double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index cebc6a7f..fdd3605e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -36,7 +36,7 @@ public class ElevatorConstants { 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_SENSOR_RESET_POSITION = 0; + private static final double REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, @@ -81,6 +81,9 @@ public class ElevatorConstants { Color.kYellow ); + static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; + 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(), @@ -88,8 +91,6 @@ public class ElevatorConstants { ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; - static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; - static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static { configureMasterMotor(); @@ -122,7 +123,7 @@ private static void configureMasterMotor() { config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0; config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; @@ -155,7 +156,7 @@ private static void configureFollowerMotor() { private static void configureReverseLimitSensor() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION)); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS)); } public enum ElevatorState { @@ -164,6 +165,10 @@ public enum ElevatorState { SCORE_L2(0, 1), SCORE_L3(0.4, 1), SCORE_L4(1.045, 1), + PREPARE_SCORE_L1(0, 1), + PREPARE_SCORE_L2(0, 1), + PREPARE_SCORE_L3(0.4, 1), + PREPARE_SCORE_L4(1.045, 1), COLLECT_ALGAE_FROM_L2(0, 1), COLLECT_ALGAE_FROM_L3(0.35, 1), REST_WITH_ALGAE(0, 0.3), From 9c7420bf5d277d36c0c0dba331415c190e10e3c9 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Mon, 8 Sep 2025 16:29:15 +0300 Subject: [PATCH 14/21] The last commit name is: fixed poses calculation --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 9c9ab7f3..29f10a2f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -86,7 +86,7 @@ private Pose3d getFirstStageComponentPose() { } private Pose3d getSecondStageComponentPose() { - return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT) + return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT); } private Pose3d calculateCurrentElevatorPoseFromOrigin(Pose3d originPoint) { From d809559036900cf41815fbd76442fdb9cc6250be Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Mon, 8 Sep 2025 17:06:16 +0300 Subject: [PATCH 15/21] Fixed constants minor issues and in Elevator fixed readability. --- .../trigon/robot/subsystems/elevator/Elevator.java | 8 ++++++-- .../robot/subsystems/elevator/ElevatorConstants.java | 11 +++++------ 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 29f10a2f..d1751215 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -17,9 +17,13 @@ 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 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() { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index fdd3605e..595c2923 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -83,7 +83,6 @@ public class ElevatorConstants { static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; 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(), @@ -123,7 +122,7 @@ private static void configureMasterMotor() { config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS; config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; @@ -165,10 +164,10 @@ public enum ElevatorState { SCORE_L2(0, 1), SCORE_L3(0.4, 1), SCORE_L4(1.045, 1), - PREPARE_SCORE_L1(0, 1), - PREPARE_SCORE_L2(0, 1), - PREPARE_SCORE_L3(0.4, 1), - PREPARE_SCORE_L4(1.045, 1), + PREPARE_L1(0, 1), + PREPARE_L2(0, 1), + PREPARE_L3(0.4, 1), + PREPARE_L4(1.045, 1), COLLECT_ALGAE_FROM_L2(0, 1), COLLECT_ALGAE_FROM_L3(0.35, 1), REST_WITH_ALGAE(0, 0.3), From 981b8a4b85c6bcb34958ff124d5224175f58c7d2 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Tue, 9 Sep 2025 12:53:22 +0300 Subject: [PATCH 16/21] Removed redundant function. --- .../trigon/robot/subsystems/elevator/Elevator.java | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index d1751215..7277b15a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -86,20 +86,16 @@ void setTargetPositionRotations(double targetPositionRotations) { } private Pose3d getFirstStageComponentPose() { - return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT); + return calculateComponentPose(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); } private Pose3d getSecondStageComponentPose() { - return calculateCurrentElevatorPoseFromOrigin(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT); + return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); } - private Pose3d calculateCurrentElevatorPoseFromOrigin(Pose3d originPoint) { - return calculateComponentPose(originPoint, getPositionMeters()); - } - - private Pose3d calculateComponentPose(Pose3d originPoint, double poseHeight) { + private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) { final Transform3d elevatorTransform = new Transform3d( - new Translation3d(0, 0, poseHeight), + new Translation3d(0, 0, currentHeight), new Rotation3d() ); return originPoint.transformBy(elevatorTransform); From 4254ae031aff873f7ad015a67fd71c9943abe837 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Tue, 9 Sep 2025 12:55:51 +0300 Subject: [PATCH 17/21] fixed a mixup --- .../trigon/robot/subsystems/elevator/ElevatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 595c2923..9468f137 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -119,10 +119,10 @@ private static void configureMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0; config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; From 8d3984c544a052a3d7f3bbf09d61479773c02cb8 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Tue, 9 Sep 2025 21:20:53 +0300 Subject: [PATCH 18/21] Fixed pose logic and recalibrated pid. --- .../robot/subsystems/elevator/Elevator.java | 14 +++++- .../elevator/ElevatorConstants.java | 44 +++++++++---------- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 7277b15a..57f77a0d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -86,11 +86,21 @@ void setTargetPositionRotations(double targetPositionRotations) { } private Pose3d getFirstStageComponentPose() { - return calculateComponentPose(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); + return calculateComponentPose(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstPoseHeight()); } private Pose3d getSecondStageComponentPose() { - return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); + return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); + } + + private double getFirstPoseHeight() { + if (secondComponentLimitReached()) + return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + return 0; + } + + private boolean secondComponentLimitReached() { + return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; } private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 9468f137..4840e378 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -39,7 +39,7 @@ public class ElevatorConstants { private static final double REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double - DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; static final boolean FOC_ENABLED = true; @@ -48,8 +48,8 @@ public class ElevatorConstants { private static final double ELEVATOR_MASS_KILOGRAMS = 7, DRUM_RADIUS_METERS = 0.04, - MINIMUM_ELEVATOR_HEIGHT_METERS = 0.78, - MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.752; + 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, @@ -71,7 +71,7 @@ public class ElevatorConstants { new Rotation3d(0, 0, 0) ); static final Pose3d SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17 ,0.684), + new Translation3d(0, -0.17 ,0.0814), new Rotation3d(0, 0, 0) ); static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( @@ -80,8 +80,7 @@ public class ElevatorConstants { MINIMUM_ELEVATOR_HEIGHT_METERS, Color.kYellow ); - - static final double FIRST_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.78; + 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( @@ -107,9 +106,9 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 1.8:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5:0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.2: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; @@ -122,7 +121,7 @@ private static void configureMasterMotor() { config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; @@ -159,19 +158,20 @@ private static void configureReverseLimitSensor() { } public enum ElevatorState { - REST(0, 0.7), - SCORE_L1(0, 1), - SCORE_L2(0, 1), - SCORE_L3(0.4, 1), - SCORE_L4(1.045, 1), - PREPARE_L1(0, 1), - PREPARE_L2(0, 1), - PREPARE_L3(0.4, 1), - PREPARE_L4(1.045, 1), - COLLECT_ALGAE_FROM_L2(0, 1), - COLLECT_ALGAE_FROM_L3(0.35, 1), - REST_WITH_ALGAE(0, 0.3), - SCORE_NET(1.045, 0.3); + 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; From 7cea435b4dd8f94a22a177f45428dea0ad215ddf Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Wed, 10 Sep 2025 13:57:15 +0300 Subject: [PATCH 19/21] Changed names of things. --- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 10 +++++----- .../robot/subsystems/elevator/ElevatorConstants.java | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 57f77a0d..50ddd20b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -86,20 +86,20 @@ void setTargetPositionRotations(double targetPositionRotations) { } private Pose3d getFirstStageComponentPose() { - return calculateComponentPose(ElevatorConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstPoseHeight()); + return calculateComponentPose(ElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight()); } private Pose3d getSecondStageComponentPose() { - return calculateComponentPose(ElevatorConstants.SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); + return calculateComponentPose(ElevatorConstants.ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); } - private double getFirstPoseHeight() { - if (secondComponentLimitReached()) + private double getFirstStageComponentPoseHeight() { + if (isSecondComponentLimitReached()) return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; return 0; } - private boolean secondComponentLimitReached() { + private boolean isSecondComponentLimitReached() { return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 4840e378..c2945a44 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -36,7 +36,7 @@ public class ElevatorConstants { 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_SENSOR_RESET_POSITION_ROTATIONS = 0; + private static final double REVERSE_LIMIT_SENSOR_THRESHOLD_POSITION_ROTATIONS = 0; private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, @@ -66,11 +66,11 @@ public class ElevatorConstants { Units.Second.of(1000) ); - public static final Pose3d FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + 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 SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(0, -0.17 ,0.0814), new Rotation3d(0, 0, 0) ); @@ -118,7 +118,7 @@ private static void configureMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_THRESHOLD_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; @@ -154,7 +154,7 @@ private static void configureFollowerMotor() { private static void configureReverseLimitSensor() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION_ROTATIONS)); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_THRESHOLD_POSITION_ROTATIONS)); } public enum ElevatorState { From bb6b511e89fde9cb60aa413005e790c7f06ec40b Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Wed, 10 Sep 2025 15:53:13 +0300 Subject: [PATCH 20/21] Changed name issue. --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 50ddd20b..7063a1d5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -94,12 +94,12 @@ private Pose3d getSecondStageComponentPose() { } private double getFirstStageComponentPoseHeight() { - if (isSecondComponentLimitReached()) + if (isSecondStageComponentLimitReached()) return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; return 0; } - private boolean isSecondComponentLimitReached() { + private boolean isSecondStageComponentLimitReached() { return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; } From 222216c8b9f638471b9d66bb6bfd78059a85f987 Mon Sep 17 00:00:00 2001 From: noamgreen12 Date: Wed, 10 Sep 2025 17:37:36 +0300 Subject: [PATCH 21/21] Changed name to be accurate. --- .../trigon/robot/subsystems/climber/ClimberConstants.java | 6 +++--- .../trigon/robot/subsystems/elevator/ElevatorConstants.java | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) 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/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index c2945a44..b56f6d03 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -36,7 +36,7 @@ public class ElevatorConstants { 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_SENSOR_THRESHOLD_POSITION_ROTATIONS = 0; + 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, @@ -118,7 +118,7 @@ private static void configureMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_THRESHOLD_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; @@ -154,7 +154,7 @@ private static void configureFollowerMotor() { private static void configureReverseLimitSensor() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_SENSOR_THRESHOLD_POSITION_ROTATIONS)); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); } public enum ElevatorState {