From 0fad1ac8b4b7c8b48621f1231f6e956720e4e91a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 30 Sep 2025 05:27:34 +0300 Subject: [PATCH 01/53] Intake --- .../robot/constants/OperatorConstants.java | 2 +- .../armelevator/ArmElevatorConstants.java | 2 +- .../robot/subsystems/climber/Climber.java | 4 +- .../subsystems/climber/ClimberConstants.java | 48 ++++++++--------- .../endeffector/EndEffectorConstants.java | 2 +- .../subsystems/intake/IntakeConstants.java | 54 ++++++++++--------- .../transporter/TransporterCommands.java | 15 +++--- .../transporter/TransporterConstants.java | 14 ++--- 8 files changed, 73 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d130351..4ae0fc3 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -44,7 +44,7 @@ public class OperatorConstants { BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); public static final Trigger - CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), + CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.z()), SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(), SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(), SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 8a9efe3..12e0fa8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -32,7 +32,7 @@ public class ArmElevatorConstants { ANGLE_ENCODER_ID = 13, ELEVATOR_MASTER_MOTOR_ID = 16, ELEVATOR_FOLLOWER_MOTOR_ID = 17, - REVERSE_LIMIT_SENSOR_CHANNEL = 0; + REVERSE_LIMIT_SENSOR_CHANNEL = 1; private static final String ARM_MASTER_MOTOR_NAME = "ArmMasterMotor", ARM_FOLLOWER_MOTOR_NAME = "ArmFollowerMotor", diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index c112dbd..0d0c72a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -101,8 +101,8 @@ void setTargetVoltage(double targetVoltage) { } private void setServoPowers(double power) { - rightServo.setTargetSpeed(power); - leftServo.setTargetSpeed(-power); +// rightServo.setTargetSpeed(power); +// leftServo.setTargetSpeed(-power); } private Pose3d calculateVisualizationPose() { 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 9d2e699..dbaf510 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -26,10 +26,10 @@ public class ClimberConstants { private static final int MOTOR_ID = 18, - REVERSE_LIMIT_SENSOR_CHANNEL = 0, - RIGHT_SERVO_CHANNEL = 1, - LEFT_SERVO_CHANNEL = 2, - CAGE_SENSOR_CHANNEL = 3; + REVERSE_LIMIT_SENSOR_CHANNEL = 8, + RIGHT_SERVO_CHANNEL = 7, + LEFT_SERVO_CHANNEL = 8, + CAGE_SENSOR_CHANNEL = 2; private static final String MOTOR_NAME = "ClimberMotor", REVERSE_LIMIT_SENSOR_NAME = "ClimberReverseLimitSensor", @@ -50,12 +50,12 @@ public class ClimberConstants { private static final double GEAR_RATIO = 37.5; private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3; - private static final int - SERVO_PULSE_WIDTH_MICROSECONDS = 20000, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS = 1500, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS = 1000, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS = 2000; +// private static final int +// SERVO_PULSE_WIDTH_MICROSECONDS = 20000, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS = 1500, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS = 1000, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS = 2000; static final boolean FOC_ENABLED = true; @@ -154,20 +154,20 @@ private static void configureMotor() { } private static void configureServos() { - RIGHT_SERVO.setPWMBoundaries( - SERVO_PULSE_WIDTH_MICROSECONDS, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS - ); - LEFT_SERVO.setPWMBoundaries( - SERVO_PULSE_WIDTH_MICROSECONDS, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS - ); +// RIGHT_SERVO.setPWMBoundaries( +// SERVO_PULSE_WIDTH_MICROSECONDS, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS +// ); +// LEFT_SERVO.setPWMBoundaries( +// SERVO_PULSE_WIDTH_MICROSECONDS, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS +// ); } private static void configureReverseLimitSensor() { diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index e0c3afb..9f6270a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -18,7 +18,7 @@ public class EndEffectorConstants { private static final int END_EFFECTOR_MOTOR_ID = 15, - DISTANCE_SENSOR_CHANNEL = 4; + DISTANCE_SENSOR_CHANNEL = 0; private static final String END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index b7d1209..f5d2a2b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -14,7 +14,9 @@ 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.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.simplesensor.SimpleSensor; @@ -31,9 +33,9 @@ public class IntakeConstants { private static final int INTAKE_MOTOR_ID = 9, ANGLE_MOTOR_ID = 10, - REVERSE_LIMIT_SENSOR_CHANNEL = 0, - FORWARD_LIMIT_CHANNEL = 1, - DISTANCE_SENSOR_CHANNEL = 2; + REVERSE_LIMIT_SENSOR_CHANNEL = 3, + FORWARD_LIMIT_CHANNEL = 4, + DISTANCE_SENSOR_CHANNEL = 5; private static final String INTAKE_MOTOR_NAME = "IntakeMotor", ANGLE_MOTOR_NAME = "IntakeAngleMotor", @@ -50,7 +52,7 @@ public class IntakeConstants { private static final double INTAKE_MOTOR_GEAR_RATIO = 4, - ANGLE_MOTOR_GEAR_RATIO = 28; + ANGLE_MOTOR_GEAR_RATIO = 40; static final boolean FOC_ENABLED = true; private static final int @@ -64,8 +66,8 @@ public class IntakeConstants { INTAKE_LENGTH_METERS = 0.365, INTAKE_MASS_KILOGRAMS = 3.26; private static final Rotation2d - MINIMUM_ANGLE = Rotation2d.fromDegrees(-12), - MAXIMUM_ANGLE = Rotation2d.fromDegrees(110); + MINIMUM_ANGLE = Rotation2d.fromDegrees(0), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(-127); private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final SimpleMotorSimulation INTAKE_SIMULATION = new SimpleMotorSimulation( INTAKE_GEARBOX, @@ -87,8 +89,8 @@ public class IntakeConstants { DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.2).per(Units.Second), - Units.Volts.of(0.6), + Units.Volts.of(1.2).per(Units.Second), + Units.Volts.of(1.5), null ); @@ -149,7 +151,7 @@ private static void configureIntakeMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.RotorToSensorRatio = INTAKE_MOTOR_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = INTAKE_MOTOR_GEAR_RATIO; config.CurrentLimits.SupplyCurrentLimit = 60; INTAKE_MOTOR.applyConfiguration(config); @@ -169,28 +171,28 @@ private static void configureAngleMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.RotorToSensorRatio = ANGLE_MOTOR_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ANGLE_MOTOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 500 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 500 : 30; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0054454 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 3.3247 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0.5; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0054454 : 0.61; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 3.3247 : 4; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.047542 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.35427 : 0.32151; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.35427 : 0.3; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 12 / config.Slot0.kV : 2; + config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 12 / config.Slot0.kV : 3; config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 8 : 8; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MINIMUM_ANGLE.getRotations(); - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAXIMUM_ANGLE.getRotations(); +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MINIMUM_ANGLE.getRotations(); +// +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAXIMUM_ANGLE.getRotations(); config.CurrentLimits.SupplyCurrentLimit = 60; @@ -204,23 +206,25 @@ private static void configureAngleMotor() { ANGLE_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); ANGLE_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); ANGLE_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + + OperatorConstants.DEBUGGING_TRIGGER.onTrue(new InstantCommand(() -> ANGLE_MOTOR.setPosition(0)).ignoringDisable(true)); } private static void configureLimitSensor(SimpleSensor limitSensor, DoubleSupplier simulationSupplier, BooleanEvent booleanEvent, Rotation2d resetPosition) { limitSensor.setSimulationSupplier(simulationSupplier); - booleanEvent.ifHigh(() -> ANGLE_MOTOR.setPosition(resetPosition.getRotations())); +// booleanEvent.ifHigh(() -> ANGLE_MOTOR.setPosition(resetPosition.getRotations())); } private static void configureDistanceSensor() { - DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); +// DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); } public enum IntakeState { REST(0, MINIMUM_ANGLE), REST_FOR_CLIMB(0, MAXIMUM_ANGLE), - COLLECT(5, MAXIMUM_ANGLE), - EJECT(-5, MAXIMUM_ANGLE); + COLLECT(-8, MAXIMUM_ANGLE), + EJECT(5, MINIMUM_ANGLE); public final double targetVoltage; public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java index 95e32ae..5774876 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java @@ -1,6 +1,9 @@ package frc.trigon.robot.subsystems.transporter; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.trigon.robot.RobotContainer; import lib.commands.NetworkTablesCommand; @@ -18,12 +21,10 @@ public static Command getDebuggingCommand() { } public static Command getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState targetState) { - return new RepeatCommand( - new SequentialCommandGroup( - getSetTargetStateCommand(targetState).withTimeout(TransporterConstants.PULSE_VOLTAGE_APPLIED_TIME_SECONDS), - new WaitCommand(TransporterConstants.PULSE_WAIT_TIME_SECONDS) - ) - ); + return new SequentialCommandGroup( + getSetTargetStateCommand(targetState).withTimeout(TransporterConstants.PULSE_VOLTAGE_APPLIED_TIME_SECONDS), + new WaitCommand(TransporterConstants.PULSE_WAIT_TIME_SECONDS) + ).repeatedly(); } public static Command getSetTargetStateCommand(TransporterConstants.TransporterState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 1c02c7c..8dfd9e2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -21,7 +21,7 @@ public class TransporterConstants { private static final int RIGHT_MOTOR_ID = 11, LEFT_MOTOR_ID = 12, - BEAM_BREAK_CHANNEL = 3; + BEAM_BREAK_CHANNEL = 6; private static final String RIGHT_MOTOR_NAME = "TransporterRightMotor", LEFT_MOTOR_NAME = "TransporterLeftMotor", @@ -60,8 +60,8 @@ public class TransporterConstants { MAXIMUM_DISPLAYABLE_VELOCITY ); - static final double PULSE_VOLTAGE_APPLIED_TIME_SECONDS = 0.1; - static final double PULSE_WAIT_TIME_SECONDS = 0.05; + static final double PULSE_VOLTAGE_APPLIED_TIME_SECONDS = 0.08; + static final double PULSE_WAIT_TIME_SECONDS = 0.03; private static final double HAS_CORAL_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent HAS_CORAL_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), @@ -102,10 +102,10 @@ private static void configureBeamBreak() { public enum TransporterState { REST(0, 0), - COLLECT(5, 5), - ALIGN_CORAL(5, 6), - HOLD_CORAL(1, 1), - EJECT(-5, -5); + COLLECT(-4, 5), + ALIGN_CORAL(-5, 6), + HOLD_CORAL(-1, 1), + EJECT(5, -5); public final double targetRightMotorVoltage, From 7b281c24c617c22fcb385818c37495b2c484ee8e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 30 Sep 2025 16:15:04 +0300 Subject: [PATCH 02/53] Commit so people can continue --- .../subsystems/armelevator/ArmElevator.java | 9 +++--- .../armelevator/ArmElevatorConstants.java | 31 ++++++++++++------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 567e26e..c18ae78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -74,8 +74,8 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updateMechanism() { ArmElevatorConstants.ARM_MECHANISM.update( - Rotation2d.fromRotations(getCurrentArmAngle().getRotations() + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), - Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) + Rotation2d.fromRotations(getCurrentArmAngle().getRotations()), + Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET) ); ArmElevatorConstants.ELEVATOR_MECHANISM.update( getCurrentElevatorPositionMeters(), @@ -141,7 +141,7 @@ public boolean elevatorAtPosition(double positionMeters) { } public Rotation2d getCurrentArmAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET); } public double getCurrentElevatorPositionMeters() { @@ -196,7 +196,8 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { } void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { - armMasterMotor.setControl(armPositionRequest.withPosition(ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); + final double targetPosition = ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()); + armMasterMotor.setControl(armPositionRequest.withPosition(targetPosition - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); } void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignoreConstraints) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 12e0fa8..ad569e0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -48,21 +48,21 @@ public class ArmElevatorConstants { static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME); private static final SimpleSensor REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); - private static final double - ARM_GEAR_RATIO = 40, + static final double + ARM_GEAR_RATIO = 42, ELEVATOR_GEAR_RATIO = 4; - private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; + private static final double ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0; - static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0.3466796875 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double - ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0, + ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 20, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 2, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; @@ -109,7 +109,7 @@ public class ArmElevatorConstants { static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), - Units.Volts.of(3), + Units.Volts.of(1.5), Units.Second.of(1000) ); @@ -207,12 +207,17 @@ private static void configureArmMasterMotor() { config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Feedback.VelocityFilterTimeConstant = 0.2; + config.MotionMagic.MotionMagicCruiseVelocity = ARM_DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(270); +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(270); + +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(-270); config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; @@ -225,6 +230,8 @@ private static void configureArmMasterMotor() { ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); } private static void configureArmFollowerMotor() { @@ -267,7 +274,7 @@ private static void configureElevatorMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; @@ -310,7 +317,7 @@ private static void configureElevatorFollowerMotor() { private static void configureAngleEncoder() { final CANcoderConfiguration config = new CANcoderConfiguration(); - config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_GRAVITY_OFFSET; config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; @@ -323,7 +330,7 @@ private static void configureAngleEncoder() { private static void configureReverseLimitSensor() { REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> ELEVATOR_MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> ELEVATOR_MASTER_MOTOR.setPosition(ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); } public enum ArmElevatorState { From aff073dfdd760f3f49abcbfe4b1949251cd15a89 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 02:01:48 +0300 Subject: [PATCH 03/53] arm tuned --- src/main/deploy/logs/.gitkeep | 0 .../java/frc/trigon/robot/RobotContainer.java | 22 +++++-------- .../robot/subsystems/MotorSubsystem.java | 4 +-- .../subsystems/armelevator/ArmElevator.java | 24 ++++++++------ .../armelevator/ArmElevatorConstants.java | 31 ++++++++++--------- 5 files changed, 41 insertions(+), 40 deletions(-) delete mode 100644 src/main/deploy/logs/.gitkeep diff --git a/src/main/deploy/logs/.gitkeep b/src/main/deploy/logs/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2f7bb6f..e804f0d 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -25,18 +25,10 @@ import frc.trigon.robot.subsystems.armelevator.ArmElevator; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; -import frc.trigon.robot.subsystems.climber.ClimberCommands; -import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.endeffector.EndEffector; -import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; -import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; -import frc.trigon.robot.subsystems.intake.IntakeCommands; -import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; -import frc.trigon.robot.subsystems.transporter.TransporterCommands; -import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -76,12 +68,14 @@ private void configureBindings() { } private void bindDefaultCommands() { - SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); - ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); - CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); - END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); - INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); - TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); +// SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); +// ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); +// CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); +// END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); +// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); +// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); + configureSysIDBindings(ARM_ELEVATOR); + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); } /** diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 8e74ec3..5473629 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; -import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; import lib.hardware.RobotHardwareStats; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; import java.util.ArrayList; import java.util.List; @@ -27,7 +27,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); - private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); + private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", true);//TODO: AHAHAHAHAHHA static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index c18ae78..3f3ed27 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -17,6 +17,7 @@ import org.littletonrobotics.junction.Logger; public class ArmElevator extends MotorSubsystem { + private static final boolean SHOULD_CALIBRATE_ELEVATOR = false; private final TalonFXMotor armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR, elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR; @@ -43,8 +44,7 @@ public ArmElevator() { @Override public SysIdRoutine.Config getSysIDConfig() { - return ArmElevatorConstants.ARM_SYSID_CONFIG; - //return ArmElevatorConstants.ELEVATOR_SYSID_CONFIG; + return SHOULD_CALIBRATE_ELEVATOR ? ArmElevatorConstants.ELEVATOR_SYSID_CONFIG : ArmElevatorConstants.ARM_SYSID_CONFIG; } @Override @@ -61,14 +61,17 @@ public void stop() { @Override public void updateLog(SysIdRoutineLog log) { + if (SHOULD_CALIBRATE_ELEVATOR) { + log.motor("Elevator") + .linearPosition(Units.Meters.of(getElevatorPositionRotations())) + .linearVelocity(Units.MetersPerSecond.of(elevatorMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ELEVATOR_GEAR_RATIO)) + .voltage(Units.Volts.of(elevatorMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + return; + } log.motor("Arm") - .angularPosition(Units.Rotations.of(getCurrentArmAngle().getRotations())) - .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.VELOCITY))) + .angularPosition(Units.Rotations.of(armMasterMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ARM_GEAR_RATIO)) .voltage(Units.Volts.of(armMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); -// 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 @@ -98,8 +101,11 @@ public void updatePeriodically() { @Override public void sysIDDrive(double targetVoltage) { + if (SHOULD_CALIBRATE_ELEVATOR) { + elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + return; + } armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); -// elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } public Pose3d calculateGamePieceCollectionPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index ad569e0..c93db26 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -55,14 +55,14 @@ public class ArmElevatorConstants { private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0.3466796875 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.059326171875; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : edu.wpi.first.math.util.Units.degreesToRotations(-24.7) - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double - ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 20, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 2, + ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2.5, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 7, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; @@ -115,7 +115,7 @@ public class ArmElevatorConstants { static final SysIdRoutine.Config ELEVATOR_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.25).per(Units.Seconds), - Units.Volts.of(3), + Units.Volts.of(2), Units.Second.of(1000) ); @@ -196,18 +196,18 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 70; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.02; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.4; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.Feedback.VelocityFilterTimeConstant = 0.2; +// config.Feedback.VelocityFilterTimeConstant = 0.2; config.MotionMagic.MotionMagicCruiseVelocity = ARM_DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; @@ -273,11 +273,11 @@ private static void configureElevatorMasterMotor() { config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS; +// +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; config.MotionMagic.MotionMagicCruiseVelocity = ELEVATOR_DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION; @@ -294,6 +294,7 @@ private static void configureElevatorMasterMotor() { ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); } private static void configureElevatorFollowerMotor() { @@ -319,7 +320,7 @@ private static void configureAngleEncoder() { config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_GRAVITY_OFFSET; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; ANGLE_ENCODER.applyConfiguration(config); ANGLE_ENCODER.setSimulationInputsFromTalonFX(ARM_MASTER_MOTOR); From a45d1064c59ebc21f4addaf20e0d7bfd538d663f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 02:08:53 +0300 Subject: [PATCH 04/53] fix kg and vis offset --- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index c93db26..fcfaa18 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -56,7 +56,7 @@ public class ArmElevatorConstants { ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.059326171875; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : edu.wpi.first.math.util.Units.degreesToRotations(-24.7) - ANGLE_ENCODER_GRAVITY_OFFSET; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : edu.wpi.first.math.util.Units.degreesToRotations(-23.56) - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; @@ -202,7 +202,7 @@ private static void configureArmMasterMotor() { config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.02; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.4; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.39; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; From a93400d80907b7fec21c1e2f83c349c3832adf0e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 02:12:52 +0300 Subject: [PATCH 05/53] fix arm setpoints --- .../subsystems/armelevator/ArmElevator.java | 8 ++-- .../armelevator/ArmElevatorConstants.java | 40 +++++++++---------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 3f3ed27..b45b7f4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -128,7 +128,7 @@ public boolean atState(ArmElevatorConstants.ArmElevatorState targetState) { public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { if (targetState == null) return false; - return armAtAngle(isStateReversed ? subtractFrom360Degrees(targetState.targetAngle) : targetState.targetAngle) + return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle) && elevatorAtPosition(targetState.targetPositionMeters); } @@ -190,7 +190,7 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { if (isStateReversed) { - setTargetArmAngle(subtractFrom360Degrees(targetState.targetAngle), targetState.ignoreConstraints); + setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints); return; } setTargetArmAngle(targetState.targetAngle, targetState.ignoreConstraints); @@ -237,8 +237,8 @@ private double getElevatorHeightFromMinimumSafeZone() { return getCurrentElevatorPositionMeters() - ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } - private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); + private static Rotation2d subtractFrom180Degrees(Rotation2d angleToSubtract) { + return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() - angleToSubtract.getDegrees()); } private Pose3d calculateVisualizationPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index fcfaa18..4f843ba 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -335,26 +335,26 @@ private static void configureReverseLimitSensor() { } public enum ArmElevatorState { - PREPARE_SCORE_L1(Rotation2d.fromDegrees(110), 0.3, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.3, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.7, null, false, 1), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.2, null, false, 1), - REST(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), - REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), - EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), - SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1), - SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), - SCORE_L3(Rotation2d.fromDegrees(90), 0.7, PREPARE_SCORE_L3, false, 1), - SCORE_L4(Rotation2d.fromDegrees(100), 1.2, PREPARE_SCORE_L4, false, 1), - SCORE_NET(Rotation2d.fromDegrees(160), 1.382, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(60), 0.2, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(50), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); + PREPARE_SCORE_L1(Rotation2d.fromDegrees(20), 0.3, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(10), 0.3, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(10), 0.7, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(30), 1.2, null, false, 1), + REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, REST, true, 0.7), + EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), + SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), + SCORE_L2(Rotation2d.fromDegrees(0), 0.3, PREPARE_SCORE_L2, false, 1), + SCORE_L3(Rotation2d.fromDegrees(0), 0.7, PREPARE_SCORE_L3, false, 1), + SCORE_L4(Rotation2d.fromDegrees(10), 1.2, PREPARE_SCORE_L4, false, 1), + SCORE_NET(Rotation2d.fromDegrees(70), 1.382, null, false, 0.3), + SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.2, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-40), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); public final Rotation2d targetAngle; public final double targetPositionMeters; From 8c3a876a281e922ea10ff4a8921469b682ad76eb Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 03:39:33 +0300 Subject: [PATCH 06/53] changed arm position --- .../trigon/robot/subsystems/armelevator/ArmElevator.java | 8 ++++---- .../subsystems/armelevator/ArmElevatorConstants.java | 9 ++++----- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index b45b7f4..7fbd027 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -219,12 +219,12 @@ private Rotation2d calculateMinimumArmSafeAngle() { final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmElevatorConstants.ARM_LENGTH_METERS, 0, 1); final double acos = Math.acos(cosOfMinimumSafeAngle); return Double.isNaN(acos) - ? Rotation2d.fromRadians(0) - : Rotation2d.fromRadians(acos); + ? Rotation2d.fromDegrees(-90) + : Rotation2d.fromRadians(acos).minus(Rotation2d.kCCW_90deg); } private double calculateMinimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getCos(); + final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().plus(Rotation2d.kCCW_90deg).getCos(); final double elevatorHeightFromArm = armCos * ArmElevatorConstants.ARM_LENGTH_METERS; final double minimumElevatorSafeZone = ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; final double minimumSafeHeightMeters = (RobotContainer.ARM_ELEVATOR.isArmAboveSafeAngle() @@ -244,7 +244,7 @@ private static Rotation2d subtractFrom180Degrees(Rotation2d angleToSubtract) { private Pose3d calculateVisualizationPose() { final Transform3d armTransform = new Transform3d( new Translation3d(0, 0, getCurrentElevatorPositionMeters()), - new Rotation3d(0, getCurrentArmAngle().getRadians(), 0) + new Rotation3d(0, getCurrentArmAngle().getRadians() + Math.PI / 2, 0) ); return ArmElevatorConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 4f843ba..9ba22eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -19,7 +19,6 @@ import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.hardware.simulation.ElevatorSimulation; import lib.hardware.simulation.SingleJointedArmSimulation; -import lib.utilities.Conversions; import lib.utilities.mechanisms.ElevatorMechanism2d; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; @@ -56,7 +55,7 @@ public class ArmElevatorConstants { ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.059326171875; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : edu.wpi.first.math.util.Units.degreesToRotations(-23.56) - ANGLE_ENCODER_GRAVITY_OFFSET; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : edu.wpi.first.math.util.Units.degreesToRotations(-23.56) - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; @@ -83,8 +82,8 @@ public class ArmElevatorConstants { private static final double ARM_MASS_KILOGRAMS = 3.5; private static final Rotation2d - ARM_MINIMUM_ANGLE = Rotation2d.fromDegrees(0), - ARM_MAXIMUM_ANGLE = Rotation2d.fromDegrees(360); + ARM_MINIMUM_ANGLE = Rotation2d.fromDegrees(-90), + ARM_MAXIMUM_ANGLE = Rotation2d.fromDegrees(270); private static final boolean SHOULD_ARM_SIMULATE_GRAVITY = true; private static final boolean SHOULD_ELEVATOR_SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation ARM_SIMULATION = new SingleJointedArmSimulation( @@ -164,7 +163,7 @@ public class ArmElevatorConstants { /** * The highest point of the arms angular zone where the safety logic applies. */ - static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); + static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(0); /** * The lowest point in the Elevators zone where the safety logic applies. From 48875b7c92f5544659441759ca45f24f8a555a80 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 05:20:14 +0300 Subject: [PATCH 07/53] cleaned intakeCOmmands --- .../java/frc/trigon/robot/RobotContainer.java | 39 ++++++++++--------- .../CoralCollectionCommands.java | 25 ++++-------- .../subsystems/armelevator/ArmElevator.java | 2 + .../endeffector/EndEffectorConstants.java | 2 +- .../robot/subsystems/intake/Intake.java | 4 ++ 5 files changed, 34 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index e804f0d..dfd6383 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,29 +6,30 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.commands.CommandConstants; -import frc.trigon.robot.commands.commandfactories.*; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; +import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; -import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.endeffector.EndEffector; import frc.trigon.robot.subsystems.intake.Intake; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -72,10 +73,10 @@ private void bindDefaultCommands() { // ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); // CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); // END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); -// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); -// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); - configureSysIDBindings(ARM_ELEVATOR); - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); + INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); + TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); +// configureSysIDBindings(ARM_ELEVATOR); +// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); } /** @@ -90,18 +91,18 @@ private void initializeGeneralSystems() { } 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.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.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); +// +// OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); - +// OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); - OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); - - OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); +// OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); +// OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); +// +// OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index d8a2f92..96b69d0 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; @@ -19,41 +20,29 @@ public class CoralCollectionCommands { public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( - getIntakeSequenceCommand(), + getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral), + getCollectionConfirmationCommand(), new InstantCommand( () -> getLoadCoralCommand().schedule() ) - ); - // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); } public static Command getLoadCoralCommand() { return new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) - ).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); + ).until(RobotContainer.END_EFFECTOR::hasGamePiece).unless(RobotContainer.END_EFFECTOR::hasGamePiece); } - private static Command getIntakeSequenceCommand() { - return new SequentialCommandGroup( - getInitiateCollectionCommand().until(RobotContainer.INTAKE::hasCoral), - new InstantCommand(() -> getAlignCoralCommand().schedule()).alongWith(getCollectionConfirmationCommand()) - ).until(RobotContainer.INTAKE::hasCoral); - } - private static Command getInitiateCollectionCommand() { + private static Command getIntakeCoralCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.COLLECT) + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT) ); } - private static Command getAlignCoralCommand() { - return new SequentialCommandGroup( - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.ALIGN_CORAL).until(RobotContainer.TRANSPORTER::hasCoral), - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.HOLD_CORAL) - ); - } private static Command getCollectionConfirmationCommand() { return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 7fbd027..e32adc4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -50,7 +50,9 @@ public SysIdRoutine.Config getSysIDConfig() { @Override public void setBrake(boolean brake) { armMasterMotor.setBrake(brake); + ArmElevatorConstants.ARM_FOLLOWER_MOTOR.setBrake(brake); elevatorMasterMotor.setBrake(brake); + ArmElevatorConstants.ELEVATOR_FOLLOWER_MOTOR.setBrake(brake); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 9f6270a..536049a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -62,7 +62,7 @@ private static void configureEndEffectorMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 389592f..109fd0a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -100,6 +100,7 @@ public Translation3d calculateLinearIntakeVelocity() { void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; + System.out.println("Setting intake state to " + targetState.name()); setTargetState(targetState.targetAngle, targetState.targetVoltage); } @@ -110,10 +111,13 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { private void setTargetVoltage(double voltage) { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(voltage); + System.out.println("Setting intake voltage to " + voltage); intakeMotor.setControl(voltageRequest.withOutput(voltage)); } + private void setTargetAngle(Rotation2d targetAngle) { + System.out.println("Setting intake angle to " + targetAngle.getDegrees() + " degrees"); angleMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); } From f6c5a7c3bc8a89acc70e1a37755a0b8c130fa5e1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 06:11:40 +0300 Subject: [PATCH 08/53] tuned elevator --- .../java/frc/trigon/robot/RobotContainer.java | 21 ++++++------------- .../subsystems/armelevator/ArmElevator.java | 2 +- .../armelevator/ArmElevatorConstants.java | 14 ++++++------- 3 files changed, 14 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index dfd6383..0571444 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -9,9 +9,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; -import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands; -import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; @@ -21,15 +18,12 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.endeffector.EndEffector; import frc.trigon.robot.subsystems.intake.Intake; -import frc.trigon.robot.subsystems.intake.IntakeCommands; -import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; -import frc.trigon.robot.subsystems.transporter.TransporterCommands; -import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -73,10 +67,10 @@ private void bindDefaultCommands() { // ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); // CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); // END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); - INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); - TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); -// configureSysIDBindings(ARM_ELEVATOR); -// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); +// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); +// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); + configureSysIDBindings(ARM_ELEVATOR); + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); } /** @@ -93,12 +87,9 @@ private void initializeGeneralSystems() { 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.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); - OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); // - OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); // OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); // OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); // diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index e32adc4..86721b4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -17,7 +17,7 @@ import org.littletonrobotics.junction.Logger; public class ArmElevator extends MotorSubsystem { - private static final boolean SHOULD_CALIBRATE_ELEVATOR = false; + private static final boolean SHOULD_CALIBRATE_ELEVATOR = true; private final TalonFXMotor armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR, elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR; diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 9ba22eb..bce7c60 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -64,7 +64,7 @@ public class ArmElevatorConstants { ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 7, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, - ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; + ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; static final boolean FOC_ENABLED = true; public static final double @@ -261,13 +261,13 @@ private static void configureElevatorMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 12; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0.4; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0;//0.036238 + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0.41015625; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; From 53fc6e1a367372ad759f5cb84d7eff61e77b06b3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 07:35:58 +0300 Subject: [PATCH 09/53] fix --- .../java/frc/trigon/robot/RobotContainer.java | 25 ++++++++++++------- .../commandfactories/AutonomousCommands.java | 2 +- .../CoralCollectionCommands.java | 8 +++++- .../armelevator/ArmElevatorConstants.java | 5 +++- 4 files changed, 28 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index cb112b4..361bc59 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.*; @@ -18,16 +19,24 @@ import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; -import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.climber.Climber; +import frc.trigon.robot.subsystems.climber.ClimberCommands; +import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.endeffector.EndEffector; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -73,14 +82,12 @@ private void configureBindings() { } private void bindDefaultCommands() { -// SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); -// ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); -// CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); -// END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); -// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); -// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); - configureSysIDBindings(ARM_ELEVATOR); - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(true)); + SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); + ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); + CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); + END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); + INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); + TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } /** diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index bef5496..ce47b1a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -90,7 +90,7 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( - CoralCollectionCommands.getIntakeSequenceCommand(), + CoralCollectionCommands.getIntakeCoralCommand(), ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), getDriveToCoralCommand(isRight) ) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 97eabbc..5dca8d5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -37,8 +37,14 @@ public static Command getLoadCoralCommand() { ).until(RobotContainer.END_EFFECTOR::hasGamePiece).unless(RobotContainer.END_EFFECTOR::hasGamePiece); } + public static Command getUnloadCoralCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL), () -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL)) + ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); + } - private static Command getIntakeCoralCommand() { + public static Command getIntakeCoralCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 874a9f5..263a6dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -343,6 +343,7 @@ public enum ArmElevatorState { REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, REST, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(0), 0.3, PREPARE_SCORE_L2, false, 1), @@ -353,7 +354,9 @@ public enum ArmElevatorState { COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.2, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-40), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-40), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.29, null, false, 1); + public final Rotation2d targetAngle; public final double targetPositionMeters; From bfcc454aab91517dce2b1674287969932f7260e7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 3 Oct 2025 08:02:28 +0300 Subject: [PATCH 10/53] fixed intake sim --- .../trigon/robot/subsystems/intake/Intake.java | 4 ++-- .../robot/subsystems/intake/IntakeConstants.java | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3d46aa5..b2f11f9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -97,7 +97,7 @@ void prepareForState(IntakeConstants.IntakeState targetState) { targetState.targetVoltage ); } - + public Translation3d calculateLinearIntakeVelocity() { double velocityMetersPerSecond = intakeMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * IntakeConstants.WHEEL_RADIUS_METERS; return new Translation3d( @@ -133,7 +133,7 @@ private void setTargetAngle(Rotation2d targetAngle) { private Pose3d calculateVisualizationPose() { final Transform3d transform = new Transform3d( new Translation3d(), - new Rotation3d(0, getCurrentAngle().getRadians(), 0) + new Rotation3d(0, -getCurrentAngle().getRadians(), 0) ); return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 237acce..911eac4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -66,8 +66,8 @@ public class IntakeConstants { INTAKE_LENGTH_METERS = 0.365, INTAKE_MASS_KILOGRAMS = 3.26; private static final Rotation2d - MINIMUM_ANGLE = Rotation2d.fromDegrees(0), - MAXIMUM_ANGLE = Rotation2d.fromDegrees(-127); + MINIMUM_ANGLE = Rotation2d.fromDegrees(-127), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(0); private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final SimpleMotorSimulation INTAKE_SIMULATION = new SimpleMotorSimulation( INTAKE_GEARBOX, @@ -137,7 +137,7 @@ public class IntakeConstants { static { configureIntakeMotor(); configureAngleMotor(); - configureLimitSensor(REVERSE_LIMIT_SENSOR, REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER, REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT, MINIMUM_ANGLE); + configureLimitSensor(REVERSE_LIMIT_SENSOR, REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER, REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT, MAXIMUM_ANGLE); configureLimitSensor(FORWARD_LIMIT_SENSOR, FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER, FORWARD_LIMIT_SENSOR_BOOLEAN_EVENT, MAXIMUM_ANGLE); configureDistanceSensor(); } @@ -221,11 +221,11 @@ private static void configureDistanceSensor() { } public enum IntakeState { - REST(0, MINIMUM_ANGLE), - OPEN_REST(0, MAXIMUM_ANGLE), - REST_FOR_CLIMB(0, MAXIMUM_ANGLE), - COLLECT(-8, MAXIMUM_ANGLE), - EJECT(5, MINIMUM_ANGLE); + REST(0, MAXIMUM_ANGLE), + OPEN_REST(0, MINIMUM_ANGLE), + REST_FOR_CLIMB(0, MINIMUM_ANGLE), + COLLECT(-8, MINIMUM_ANGLE), + EJECT(5, MAXIMUM_ANGLE); public final double targetVoltage; public final Rotation2d targetAngle; From 5604fa054adc5e026bcac6b7d27678d40249a872 Mon Sep 17 00:00:00 2001 From: Shm Date: Fri, 3 Oct 2025 01:40:33 -0400 Subject: [PATCH 11/53] increased current limit to 120 Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/deploy/pathplanner/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a8ea08d..f3cb5fd 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -18,7 +18,7 @@ "driveGearing": 6.12, "maxDriveSpeed": 4.4, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 100.0, + "driveCurrentLimit": 120.0, "wheelCOF": 1.6, "flModuleX": 0.172, "flModuleY": 0.273, From 89a72a6da162f02d7e62a236a336765494eaecc8 Mon Sep 17 00:00:00 2001 From: Shm Date: Fri, 3 Oct 2025 03:04:04 -0400 Subject: [PATCH 12/53] stuff Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 22 +++++++------------ .../commandfactories/GeneralCommands.java | 2 +- .../subsystems/intake/IntakeConstants.java | 2 +- .../robot/subsystems/swerve/Swerve.java | 1 + .../subsystems/swerve/SwerveConstants.java | 22 +++++++++---------- .../swervemodule/SwerveModuleConstants.java | 6 ++--- 6 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 361bc59..646230e 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -23,20 +23,12 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; -import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; -import frc.trigon.robot.subsystems.climber.ClimberCommands; -import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.endeffector.EndEffector; -import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; -import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; -import frc.trigon.robot.subsystems.intake.IntakeCommands; -import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; -import frc.trigon.robot.subsystems.transporter.TransporterCommands; -import frc.trigon.robot.subsystems.transporter.TransporterConstants; +import lib.commands.WheelRadiusCharacterizationCommand; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -83,11 +75,12 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); - ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); - CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); - END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); - INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); - TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); +// ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); +// CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); +// END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); +// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); +// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); +// configureSysIDBindings(SWERVE); } /** @@ -102,6 +95,7 @@ private void initializeGeneralSystems() { } private void bindControllerCommands() { + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); 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()); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 2eb3150..c5a4a3c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -18,7 +18,7 @@ */ public class GeneralCommands { public static Command getFieldRelativeDriveCommand() { - return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + return SwerveCommands.getOpenLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 911eac4..eeb5a83 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -207,7 +207,7 @@ private static void configureAngleMotor() { ANGLE_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); ANGLE_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - OperatorConstants.DEBUGGING_TRIGGER.onTrue(new InstantCommand(() -> ANGLE_MOTOR.setPosition(0)).ignoringDisable(true)); +// OperatorConstants.DEBUGGING_TRIGGER.onTrue(new InstantCommand(() -> ANGLE_MOTOR.setPosition(0)).ignoringDisable(true)); } private static void configureLimitSensor(SimpleSensor limitSensor, DoubleSupplier simulationSupplier, BooleanEvent booleanEvent, Rotation2d resetPosition) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 8789cf5..9bae850 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.swerve; import com.pathplanner.lib.util.DriveFeedforwards; + import com.pathplanner.lib.util.swerve.SwerveSetpoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 4e2e430..9bb6316 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -28,15 +28,15 @@ public class SwerveConstants { REAR_LEFT_ID = 3, REAR_RIGHT_ID = 4; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = 0, - FRONT_RIGHT_STEER_ENCODER_OFFSET = 0, - REAR_LEFT_STEER_ENCODER_OFFSET = 0, - REAR_RIGHT_STEER_ENCODER_OFFSET = 0; + FRONT_LEFT_STEER_ENCODER_OFFSET = -0.044677734375, + FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.3525390625, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.30517578125, + REAR_RIGHT_STEER_ENCODER_OFFSET = -0.121826171875; private static final double - FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2, - FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2, - REAR_LEFT_WHEEL_DIAMETER = 0.05 * 2, - REAR_RIGHT_WHEEL_DIAMETER = 0.05 * 2; + FRONT_LEFT_WHEEL_DIAMETER = 0.038 * 2, + FRONT_RIGHT_WHEEL_DIAMETER = 0.038 * 2, + REAR_LEFT_WHEEL_DIAMETER = 0.038 * 2, + REAR_RIGHT_WHEEL_DIAMETER = 0.038 * 2; static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{ new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER), new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER), @@ -99,9 +99,9 @@ public class SwerveConstants { private static void configureGyro() { final Pigeon2Configuration config = new Pigeon2Configuration(); - config.MountPose.MountPoseYaw = 0; - config.MountPose.MountPosePitch = 0; - config.MountPose.MountPoseRoll = 0; + config.MountPose.MountPoseYaw = Units.degreesToRotations(-52.198792); + config.MountPose.MountPosePitch = Units.degreesToRadians(-0.087891); + config.MountPose.MountPoseRoll = Units.degreesToRadians(-0.659180); GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index b7432bf..440e94a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -31,7 +31,7 @@ public class SwerveModuleConstants { public static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1).per(Units.Second), - Units.Volts.of(8), + Units.Volts.of(2), Units.Second.of(1000) ); @@ -77,10 +77,10 @@ public static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 50; + 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.016046 : 5.25; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0; From a3a6d42f7014fa6cec83e3d5c51ffa581fdf70dd Mon Sep 17 00:00:00 2001 From: Shm Date: Fri, 3 Oct 2025 07:23:38 -0400 Subject: [PATCH 13/53] LOTS OF GOOD STUFF Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 17 ++++++--- .../CoralPlacingCommands.java | 4 +- .../commandfactories/GeneralCommands.java | 2 +- .../subsystems/armelevator/ArmElevator.java | 18 ++++++--- .../armelevator/ArmElevatorConstants.java | 37 ++++--------------- .../subsystems/endeffector/EndEffector.java | 1 + .../endeffector/EndEffectorCommands.java | 9 +++++ .../endeffector/EndEffectorConstants.java | 12 ++++-- .../robot/subsystems/intake/Intake.java | 2 - .../subsystems/intake/IntakeConstants.java | 31 +--------------- .../swerve/swervemodule/SwerveModule.java | 1 + .../swervemodule/SwerveModuleConstants.java | 8 ++-- .../transporter/TransporterConstants.java | 2 +- 13 files changed, 60 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 646230e..0208400 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -23,11 +23,18 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.endeffector.EndEffector; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.commands.WheelRadiusCharacterizationCommand; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -75,12 +82,11 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); -// ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); + ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); // CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); -// END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); -// INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); -// TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); -// configureSysIDBindings(SWERVE); + END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getDefaultCommand()); + INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); + TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } /** @@ -95,7 +101,6 @@ private void initializeGeneralSystems() { } private void bindControllerCommands() { - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); 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()); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index a7367cf..cc02403 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -18,7 +18,7 @@ import lib.utilities.flippable.FlippableTranslation2d; public class CoralPlacingCommands { - public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; + public static boolean SHOULD_SCORE_AUTONOMOUSLY = false; static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { @@ -44,7 +44,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), + getPrepareArmElevatorIfWontHitReef(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index c5a4a3c..2eb3150 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -18,7 +18,7 @@ */ public class GeneralCommands { public static Command getFieldRelativeDriveCommand() { - return SwerveCommands.getOpenLoopFieldRelativeDriveCommand( + return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 1319c9e..8a2b879 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; @@ -31,7 +30,7 @@ public class ArmElevator extends MotorSubsystem { ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION, ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_JERK ).withEnableFOC(ArmElevatorConstants.FOC_ENABLED); - private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + private final DynamicMotionMagicVoltage elevatorPositionRequest = new DynamicMotionMagicVoltage( 0, ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY, ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION, @@ -194,6 +193,7 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i } void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { + scaleArmPositionRequestSpeed(targetState.speedScalar); if (isStateReversed) { setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints); return; @@ -216,7 +216,7 @@ void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignore } void setTargetElevatorPositionRotations(double targetPositionRotations, boolean ignoreConstraints) { - elevatorMasterMotor.setControl(positionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); + elevatorMasterMotor.setControl(elevatorPositionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); } private Rotation2d calculateMinimumArmSafeAngle() { @@ -254,10 +254,16 @@ private Pose3d calculateVisualizationPose() { return ArmElevatorConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); } + private void scaleArmPositionRequestSpeed(double speedScalar) { + armPositionRequest.Velocity = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_VELOCITY * speedScalar; + armPositionRequest.Acceleration = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + armPositionRequest.Jerk = armPositionRequest.Acceleration * 10; + } + private void scaleElevatorPositionRequestSpeed(double speedScalar) { - positionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar; - positionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; - positionRequest.Jerk = positionRequest.Acceleration * 10; + elevatorPositionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar; + elevatorPositionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + elevatorPositionRequest.Jerk = elevatorPositionRequest.Acceleration * 10; } private Pose3d getFirstStageComponentPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 263a6dc..2063b0a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -7,12 +7,9 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lib.hardware.RobotHardwareStats; -import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -22,35 +19,29 @@ import lib.utilities.mechanisms.ElevatorMechanism2d; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; -import java.util.function.DoubleSupplier; - public class ArmElevatorConstants { private static final int ARM_MASTER_MOTOR_ID = 13, ARM_FOLLOWER_MOTOR_ID = 14, ANGLE_ENCODER_ID = 13, ELEVATOR_MASTER_MOTOR_ID = 16, - ELEVATOR_FOLLOWER_MOTOR_ID = 17, - REVERSE_LIMIT_SENSOR_CHANNEL = 1; + ELEVATOR_FOLLOWER_MOTOR_ID = 17; private static final String ARM_MASTER_MOTOR_NAME = "ArmMasterMotor", ARM_FOLLOWER_MOTOR_NAME = "ArmFollowerMotor", ANGLE_ENCODER_NAME = "ArmEncoder", ELEVATOR_MASTER_MOTOR_NAME = "ElevatorMasterMotor", - ELEVATOR_FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", - REVERSE_LIMIT_SENSOR_NAME = "ElevatorReverseLimitSensor"; + ELEVATOR_FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor"; static final TalonFXMotor ARM_MASTER_MOTOR = new TalonFXMotor(ARM_MASTER_MOTOR_ID, ARM_MASTER_MOTOR_NAME), ARM_FOLLOWER_MOTOR = new TalonFXMotor(ARM_FOLLOWER_MOTOR_ID, ARM_FOLLOWER_MOTOR_NAME), ELEVATOR_MASTER_MOTOR = new TalonFXMotor(ELEVATOR_MASTER_MOTOR_ID, ELEVATOR_MASTER_MOTOR_NAME), ELEVATOR_FOLLOWER_MOTOR = new TalonFXMotor(ELEVATOR_FOLLOWER_MOTOR_ID, ELEVATOR_FOLLOWER_MOTOR_NAME); static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME); - private static final SimpleSensor REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); static final double ARM_GEAR_RATIO = 42, ELEVATOR_GEAR_RATIO = 4; - private static final double ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; @@ -152,11 +143,6 @@ public class ArmElevatorConstants { ); static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.593; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; - private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; - private static final BooleanEvent REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); @@ -169,8 +155,6 @@ public class ArmElevatorConstants { * The lowest point in the Elevators zone where the safety logic applies. */ public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; - - private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; static { @@ -179,7 +163,6 @@ public class ArmElevatorConstants { configureElevatorMasterMotor(); configureElevatorFollowerMotor(); configureAngleEncoder(); - configureReverseLimitSensor(); } private static void configureArmMasterMotor() { @@ -294,6 +277,7 @@ private static void configureElevatorMasterMotor() { ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + ELEVATOR_MASTER_MOTOR.setPosition(0); } private static void configureElevatorFollowerMotor() { @@ -328,27 +312,22 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); } - private static void configureReverseLimitSensor() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> ELEVATOR_MASTER_MOTOR.setPosition(ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); - } - public enum ArmElevatorState { PREPARE_SCORE_L1(Rotation2d.fromDegrees(20), 0.3, null, false, 1), PREPARE_SCORE_L2(Rotation2d.fromDegrees(10), 0.3, null, false, 1), PREPARE_SCORE_L3(Rotation2d.fromDegrees(10), 0.7, null, false, 1), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(30), 1.2, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(48), 1.4, null, false, 1), REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.02), + REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.02), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, REST, true, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.42, REST, true, 0.7), UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(0), 0.3, PREPARE_SCORE_L2, false, 1), SCORE_L3(Rotation2d.fromDegrees(0), 0.7, PREPARE_SCORE_L3, false, 1), - SCORE_L4(Rotation2d.fromDegrees(10), 1.2, PREPARE_SCORE_L4, false, 1), + SCORE_L4(Rotation2d.fromDegrees(40), 1.4, PREPARE_SCORE_L4, false, 1), SCORE_NET(Rotation2d.fromDegrees(70), 1.382, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index ff915dd..9c0acd5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -31,6 +31,7 @@ public void updatePeriodically() { EndEffectorConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("EndEffector/isHoldingAlgae", AlgaeManipulationCommands.isHoldingAlgae()); + Logger.recordOutput("EndEffector/distanceSensorCM", EndEffectorConstants.DISTANCE_SENSOR.getScaledValue()); } @AutoLogOutput(key = "EndEffector/HasCoral") diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java index 9345535..4147324 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.endeffector; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import lib.commands.NetworkTablesCommand; @@ -24,4 +25,12 @@ public static Command getSetTargetStateCommand(EndEffectorConstants.EndEffectorS RobotContainer.END_EFFECTOR ); } + + public static Command getDefaultCommand() { + return new ConditionalCommand( + getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL), + getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST), + RobotContainer.END_EFFECTOR::hasGamePiece + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 31392ba..2c1f289 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -23,7 +23,7 @@ public class EndEffectorConstants { END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME); - static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); + static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDutyCycleSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); static final boolean FOC_ENABLED = true; private static final double END_EFFECTOR_GEAR_RATIO = 12.82; @@ -46,9 +46,13 @@ public class EndEffectorConstants { ); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; + private static final double + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 3; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - DISTANCE_SENSOR::getBinaryValue + () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); @@ -63,7 +67,7 @@ private static void configureEndEffectorMotor() { config.Audio.BeepOnConfig = false; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -80,6 +84,7 @@ private static void configureEndEffectorMotor() { } private static void configureDistanceSensor() { + DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); } @@ -91,6 +96,7 @@ public enum EndEffectorState { SCORE_CORAL(4), COLLECT_ALGAE(-4), SCORE_ALGAE(4), + HOLD_CORAL(-0.5), HOLD_ALGAE(-4); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index b2f11f9..3e708f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -51,8 +51,6 @@ public void setBrake(boolean brake) { public void updatePeriodically() { intakeMotor.update(); angleMotor.update(); - IntakeConstants.FORWARD_LIMIT_SENSOR.updateSensor(); - IntakeConstants.REVERSE_LIMIT_SENSOR.updateSensor(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index eeb5a83..577a387 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -14,9 +14,7 @@ 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.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.simplesensor.SimpleSensor; @@ -33,21 +31,15 @@ public class IntakeConstants { private static final int INTAKE_MOTOR_ID = 9, ANGLE_MOTOR_ID = 10, - REVERSE_LIMIT_SENSOR_CHANNEL = 3, - FORWARD_LIMIT_CHANNEL = 4, DISTANCE_SENSOR_CHANNEL = 5; private static final String INTAKE_MOTOR_NAME = "IntakeMotor", ANGLE_MOTOR_NAME = "IntakeAngleMotor", - REVERSE_LIMIT_SWITCH_NAME = "IntakeReverseLimitSwitch", - FORWARD_LIMIT_SWITCH_NAME = "IntakeForwardLimitSwitch", DISTANCE_SENSOR_NAME = "IntakeDistanceSensor"; static final TalonFXMotor INTAKE_MOTOR = new TalonFXMotor(INTAKE_MOTOR_ID, INTAKE_MOTOR_NAME), ANGLE_MOTOR = new TalonFXMotor(ANGLE_MOTOR_ID, ANGLE_MOTOR_NAME); static final SimpleSensor - REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SWITCH_NAME), - FORWARD_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(FORWARD_LIMIT_CHANNEL, FORWARD_LIMIT_SWITCH_NAME), DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); private static final double @@ -84,8 +76,6 @@ public class IntakeConstants { SHOULD_SIMULATE_GRAVITY ); private static final DoubleSupplier - REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, - FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( @@ -112,22 +102,11 @@ public class IntakeConstants { static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.5); private static final double - COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2, - REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1, - FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; + COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), DISTANCE_SENSOR::getBinaryValue ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); - 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), - FORWARD_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - FORWARD_LIMIT_SENSOR::getBinaryValue - ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( new Translation3d(0.6827, 0, 0), new Rotation3d() @@ -137,8 +116,6 @@ public class IntakeConstants { static { configureIntakeMotor(); configureAngleMotor(); - configureLimitSensor(REVERSE_LIMIT_SENSOR, REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER, REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT, MAXIMUM_ANGLE); - configureLimitSensor(FORWARD_LIMIT_SENSOR, FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER, FORWARD_LIMIT_SENSOR_BOOLEAN_EVENT, MAXIMUM_ANGLE); configureDistanceSensor(); } @@ -210,12 +187,6 @@ private static void configureAngleMotor() { // OperatorConstants.DEBUGGING_TRIGGER.onTrue(new InstantCommand(() -> ANGLE_MOTOR.setPosition(0)).ignoringDisable(true)); } - private static void configureLimitSensor(SimpleSensor limitSensor, DoubleSupplier simulationSupplier, BooleanEvent booleanEvent, Rotation2d resetPosition) { - limitSensor.setSimulationSupplier(simulationSupplier); - -// booleanEvent.ifHigh(() -> ANGLE_MOTOR.setPosition(resetPosition.getRotations())); - } - private static void configureDistanceSensor() { // DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index f53c9a6..549cbb5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -80,6 +80,7 @@ public void updatePeriodically() { steerMotor.update(); steerEncoder.update(); + latestOdometryDrivePositions = driveMotor.getThreadedSignal(TalonFXSignal.POSITION); latestOdometrySteerPositions = steerMotor.getThreadedSignal(TalonFXSignal.POSITION); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 440e94a..66f03c0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -77,12 +77,12 @@ public static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 2; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0.24025; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0.91963; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0.069151; config.Feedback.VelocityFilterTimeConstant = 0; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index b891544..09cd007 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -22,7 +22,7 @@ public class TransporterConstants { private static final int RIGHT_MOTOR_ID = 11, LEFT_MOTOR_ID = 12, - BEAM_BREAK_CHANNEL = 6; + BEAM_BREAK_CHANNEL = 1; private static final String RIGHT_MOTOR_NAME = "TransporterRightMotor", LEFT_MOTOR_NAME = "TransporterLeftMotor", From 456e939bc52d6146522869891877bd9d001e1208 Mon Sep 17 00:00:00 2001 From: Shm Date: Sun, 5 Oct 2025 11:25:10 -0400 Subject: [PATCH 14/53] canus on canivore Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../armelevator/ArmElevatorConstants.java | 15 ++++++++------- .../endeffector/EndEffectorConstants.java | 3 ++- .../swerve/swervemodule/SwerveModule.java | 1 - 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 2063b0a..c11db18 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -9,6 +9,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.constants.RobotConstants; import lib.hardware.RobotHardwareStats; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -33,11 +34,11 @@ public class ArmElevatorConstants { ELEVATOR_MASTER_MOTOR_NAME = "ElevatorMasterMotor", ELEVATOR_FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor"; static final TalonFXMotor - ARM_MASTER_MOTOR = new TalonFXMotor(ARM_MASTER_MOTOR_ID, ARM_MASTER_MOTOR_NAME), - ARM_FOLLOWER_MOTOR = new TalonFXMotor(ARM_FOLLOWER_MOTOR_ID, ARM_FOLLOWER_MOTOR_NAME), - ELEVATOR_MASTER_MOTOR = new TalonFXMotor(ELEVATOR_MASTER_MOTOR_ID, ELEVATOR_MASTER_MOTOR_NAME), - ELEVATOR_FOLLOWER_MOTOR = new TalonFXMotor(ELEVATOR_FOLLOWER_MOTOR_ID, ELEVATOR_FOLLOWER_MOTOR_NAME); - static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME); + ARM_MASTER_MOTOR = new TalonFXMotor(ARM_MASTER_MOTOR_ID, ARM_MASTER_MOTOR_NAME, RobotConstants.CANIVORE_NAME), + ARM_FOLLOWER_MOTOR = new TalonFXMotor(ARM_FOLLOWER_MOTOR_ID, ARM_FOLLOWER_MOTOR_NAME, RobotConstants.CANIVORE_NAME), + ELEVATOR_MASTER_MOTOR = new TalonFXMotor(ELEVATOR_MASTER_MOTOR_ID, ELEVATOR_MASTER_MOTOR_NAME, RobotConstants.CANIVORE_NAME), + ELEVATOR_FOLLOWER_MOTOR = new TalonFXMotor(ELEVATOR_FOLLOWER_MOTOR_ID, ELEVATOR_FOLLOWER_MOTOR_NAME, RobotConstants.CANIVORE_NAME); + static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME, RobotConstants.CANIVORE_NAME); static final double ARM_GEAR_RATIO = 42, @@ -318,8 +319,8 @@ public enum ArmElevatorState { PREPARE_SCORE_L3(Rotation2d.fromDegrees(10), 0.7, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(48), 1.4, null, false, 1), REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.02), - REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.02), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.4), + REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.4), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.42, REST, true, 0.7), UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, null, false, 0.7), diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 2c1f289..865f4f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -22,7 +23,7 @@ public class EndEffectorConstants { private static final String END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; - static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME); + static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME, RobotConstants.CANIVORE_NAME); static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDutyCycleSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); static final boolean FOC_ENABLED = true; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index 549cbb5..f53c9a6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -80,7 +80,6 @@ public void updatePeriodically() { steerMotor.update(); steerEncoder.update(); - latestOdometryDrivePositions = driveMotor.getThreadedSignal(TalonFXSignal.POSITION); latestOdometrySteerPositions = steerMotor.getThreadedSignal(TalonFXSignal.POSITION); } From 3f4d6464d5d408f7a4fc19d8b420141c725d5076 Mon Sep 17 00:00:00 2001 From: Shm Date: Sun, 5 Oct 2025 13:44:53 -0400 Subject: [PATCH 15/53] push Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 2 + .../AlgaeManipulationCommands.java | 2 +- .../CoralCollectionCommands.java | 4 +- .../subsystems/armelevator/ArmElevator.java | 2 +- .../armelevator/ArmElevatorConstants.java | 38 +++++++++---------- .../endeffector/EndEffectorCommands.java | 2 +- .../endeffector/EndEffectorConstants.java | 8 ++-- .../transporter/TransporterConstants.java | 10 ++--- 8 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0208400..a3e9aef 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -78,6 +78,7 @@ public Command getAutonomousCommand() { private void configureBindings() { bindDefaultCommands(); bindControllerCommands(); +// configureSysIDBindings(ARM_ELEVATOR); } private void bindDefaultCommands() { @@ -117,6 +118,7 @@ private void bindControllerCommands() { OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 41268ba..e22d68c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.5))), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 5dca8d5..e74f7e8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -34,7 +34,9 @@ public static Command getLoadCoralCommand() { return new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) - ).until(RobotContainer.END_EFFECTOR::hasGamePiece).unless(RobotContainer.END_EFFECTOR::hasGamePiece); + ).until(RobotContainer.END_EFFECTOR::hasGamePiece).andThen( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)) + ).unless(RobotContainer.END_EFFECTOR::hasGamePiece); } public static Command getUnloadCoralCommand() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 8a2b879..7b1311a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -18,7 +18,7 @@ import org.littletonrobotics.junction.Logger; public class ArmElevator extends MotorSubsystem { - private static final boolean SHOULD_CALIBRATE_ELEVATOR = true; + private static final boolean SHOULD_CALIBRATE_ELEVATOR = false; private final TalonFXMotor armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR, elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR; diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index c11db18..cd00200 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -46,14 +46,14 @@ public class ArmElevatorConstants { private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.059326171875; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.0625; static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : edu.wpi.first.math.util.Units.degreesToRotations(-23.56) - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2.5, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 7, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 4, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; @@ -145,7 +145,7 @@ public class ArmElevatorConstants { static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.593; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2); /** * The highest point of the arms angular zone where the safety logic applies. @@ -177,18 +177,18 @@ private static void configureArmMasterMotor() { config.Feedback.RotorToSensorRatio = ARM_GEAR_RATIO; config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 70; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 30; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.02; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0.1; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 2; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.39; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.35; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; // config.Feedback.VelocityFilterTimeConstant = 0.2; @@ -278,7 +278,7 @@ private static void configureElevatorMasterMotor() { ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); - ELEVATOR_MASTER_MOTOR.setPosition(0); +// ELEVATOR_MASTER_MOTOR.setPosition(0); } private static void configureElevatorFollowerMotor() { @@ -317,25 +317,25 @@ public enum ArmElevatorState { PREPARE_SCORE_L1(Rotation2d.fromDegrees(20), 0.3, null, false, 1), PREPARE_SCORE_L2(Rotation2d.fromDegrees(10), 0.3, null, false, 1), PREPARE_SCORE_L3(Rotation2d.fromDegrees(10), 0.7, null, false, 1), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(48), 1.4, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.41, null, false, 1), REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.4), - REST_WITH_ALGAE(Rotation2d.fromDegrees(0), 0.603, null, false, 0.4), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.6), + REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.42, REST, true, 0.7), - UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5519, null, false, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(0), 0.3, PREPARE_SCORE_L2, false, 1), SCORE_L3(Rotation2d.fromDegrees(0), 0.7, PREPARE_SCORE_L3, false, 1), - SCORE_L4(Rotation2d.fromDegrees(40), 1.4, PREPARE_SCORE_L4, false, 1), + SCORE_L4(Rotation2d.fromDegrees(25), 1.41, PREPARE_SCORE_L4, false, 1), SCORE_NET(Rotation2d.fromDegrees(70), 1.382, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.2, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-40), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), - COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.29, null, false, 1); + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.4, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.12, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.29, null, false, 1); public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java index 4147324..295d004 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java @@ -21,7 +21,7 @@ public static Command getDebuggingCommand() { public static Command getSetTargetStateCommand(EndEffectorConstants.EndEffectorState targetState) { return new StartEndCommand( () -> RobotContainer.END_EFFECTOR.setTargetState(targetState), - RobotContainer.END_EFFECTOR::stop, + () -> {}, RobotContainer.END_EFFECTOR ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 865f4f2..c7342d8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -46,11 +46,11 @@ public class EndEffectorConstants { END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY ); - private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.1; private static final double DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; - private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 3; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 6; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES @@ -95,10 +95,10 @@ public enum EndEffectorState { LOAD_CORAL(-4), UNLOAD_CORAL(4), SCORE_CORAL(4), - COLLECT_ALGAE(-4), + COLLECT_ALGAE(-7), SCORE_ALGAE(4), HOLD_CORAL(-0.5), - HOLD_ALGAE(-4); + HOLD_ALGAE(-7); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 09cd007..c00d5c0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -32,10 +32,10 @@ public class TransporterConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); static final SimpleSensor BEAM_BREAK = SimpleSensor.createDigitalSensor(BEAM_BREAK_CHANNEL, BEAM_BREAK_NAME); - private static double GEAR_RATIO = 3; - static boolean FOC_ENABLED = true; - private static int MOTOR_AMOUNT = 1; - private static DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); + private final static double GEAR_RATIO = 3; + static final boolean FOC_ENABLED = true; + private final static int MOTOR_AMOUNT = 1; + private final static DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); private static final double MOMENT_OF_INERTIA = 0.003; private static final SimpleMotorSimulation RIGHT_MOTOR_SIMULATION = new SimpleMotorSimulation( @@ -63,7 +63,7 @@ public class TransporterConstants { static final double PULSE_VOLTAGE_APPLIED_TIME_SECONDS = 0.08; static final double PULSE_WAIT_TIME_SECONDS = 0.03; - private static final double HAS_CORAL_DEBOUNCE_TIME_SECONDS = 0.2; + private static final double HAS_CORAL_DEBOUNCE_TIME_SECONDS = 0.08; static final BooleanEvent HAS_CORAL_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), BEAM_BREAK::getBinaryValue From 204470b8e9bc26763f7623ce9116cff9738637da Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Sun, 5 Oct 2025 22:17:47 +0300 Subject: [PATCH 16/53] fixed algae --- .../commandfactories/AlgaeManipulationCommands.java | 9 ++++++++- .../subsystems/endeffector/EndEffectorConstants.java | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index e22d68c..0db1a0e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -95,7 +95,7 @@ private static Command getScoreAlgaeCommand() { return new SelectCommand<>( Map.of( 0, getHoldAlgaeCommand(), - 1, getScoreInNetCommand(), + 1, CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY ? getAtonomouslyScoreInNetCommand() : getScoreInNetCommand(), 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector @@ -110,6 +110,13 @@ private static Command getHoldAlgaeCommand() { } private static Command getScoreInNetCommand() { + return new ParallelRaceGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER) + ); + } + + private static Command getAtonomouslyScoreInNetCommand() { return new ParallelRaceGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index c7342d8..0b527d7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -39,7 +39,7 @@ public class EndEffectorConstants { END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 10; static final SpeedMechanism2d END_EFFECTOR_MECHANISM = new SpeedMechanism2d( "EndEffectorMechanism", From 779c47081ed535e4f92f15e547b67e18c9f042d6 Mon Sep 17 00:00:00 2001 From: Shm Date: Sun, 5 Oct 2025 15:18:08 -0400 Subject: [PATCH 17/53] FIX Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/CoralCollectionCommands.java | 7 +++++-- .../commandfactories/CoralEjectionCommands.java | 2 +- .../robot/subsystems/armelevator/ArmElevator.java | 7 +++++++ .../subsystems/armelevator/ArmElevatorCommands.java | 3 ++- .../subsystems/armelevator/ArmElevatorConstants.java | 11 ++++++----- .../subsystems/endeffector/EndEffectorCommands.java | 3 ++- .../subsystems/endeffector/EndEffectorConstants.java | 2 +- .../subsystems/transporter/TransporterConstants.java | 2 +- 8 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index e74f7e8..1ac5588 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -27,7 +27,7 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)).unless(CoralCollectionCommands::hasCoral); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); } public static Command getLoadCoralCommand() { @@ -35,7 +35,10 @@ public static Command getLoadCoralCommand() { ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) ).until(RobotContainer.END_EFFECTOR::hasGamePiece).andThen( - ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)) + new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL) + ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING)) ).unless(RobotContainer.END_EFFECTOR::hasGamePiece); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index 2287965..2c6030e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -20,7 +20,7 @@ public static Command getCoralEjectionCommand() { getEjectCoralFromIntakeCommand(), getEjectCoralFromEndEffectorCommand(), () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() - ).onlyIf(SimulationFieldHandler::isHoldingCoral); + ); } private static Command getEjectCoralFromIntakeCommand() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 7b1311a..4b8287c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -207,6 +208,12 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { } void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { + final Rotation2d minimumSafeAngle = calculateMinimumArmSafeAngle(); + final Rotation2d currentAngle = getCurrentArmAngle(); + if (minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { + armMasterMotor.setControl(armPositionRequest.withPosition(currentAngle.getRotations() - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); + return; + } final double targetPosition = ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()); armMasterMotor.setControl(armPositionRequest.withPosition(targetPosition - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 1363f51..aee7926 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; import lib.commands.NetworkTablesCommand; @@ -69,7 +70,7 @@ public static Command getPrepareForStateCommand(Supplier Date: Sun, 5 Oct 2025 22:36:37 +0300 Subject: [PATCH 18/53] proccessor works --- .../commandfactories/AlgaeManipulationCommands.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 0db1a0e..d6467f6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -96,7 +96,7 @@ private static Command getScoreAlgaeCommand() { Map.of( 0, getHoldAlgaeCommand(), 1, CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY ? getAtonomouslyScoreInNetCommand() : getScoreInNetCommand(), - 2, getScoreInProcessorCommand() + 2, CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY ? getAtonomouslyScoreInProcessorCommand() : getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); @@ -126,7 +126,14 @@ private static Command getAtonomouslyScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, false), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER) + ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); + } + + private static Command getAtonomouslyScoreInProcessorCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), getDriveToProcessorCommand() ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); From d18c2eec958d02d8b4c28cf9533764a2853997a3 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Sun, 5 Oct 2025 22:50:10 +0300 Subject: [PATCH 19/53] fixec coral and lolipop intake --- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 1ac5588..0906235 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -27,7 +27,7 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).asProxy()); } public static Command getLoadCoralCommand() { From 8780d1e44dab408c6d667649e675e2ab5878191b Mon Sep 17 00:00:00 2001 From: Shm Date: Sun, 5 Oct 2025 18:45:18 -0400 Subject: [PATCH 20/53] all good Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 9 ++++--- .../robot/constants/CameraConstants.java | 16 ++++++------ .../robot/constants/OperatorConstants.java | 11 +++++--- .../subsystems/armelevator/ArmElevator.java | 7 +++-- .../armelevator/ArmElevatorConstants.java | 26 +++++++++---------- .../robot/subsystems/climber/Climber.java | 15 +++++++---- .../subsystems/climber/ClimberConstants.java | 6 ++--- .../endeffector/EndEffectorConstants.java | 4 +-- .../robot/subsystems/intake/Intake.java | 4 +++ .../java/lib/hardware/misc/servo/Servo.java | 12 +++++++++ .../java/lib/hardware/misc/servo/ServoIO.java | 3 +++ .../hardware/misc/servo/io/RealServoIO.java | 5 ++++ .../misc/servo/io/SimulationServoIO.java | 5 ++++ 13 files changed, 83 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index a3e9aef..2999877 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -27,7 +27,6 @@ import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.endeffector.EndEffector; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; -import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -35,7 +34,6 @@ import frc.trigon.robot.subsystems.transporter.Transporter; import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; -import lib.commands.WheelRadiusCharacterizationCommand; import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -43,7 +41,7 @@ public class RobotContainer { public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA, + CameraConstants.FRONT_REEF_TAG_CAMERA, CameraConstants.LEFT_REEF_TAG_CAMERA, CameraConstants.RIGHT_REEF_TAG_CAMERA ); @@ -118,7 +116,10 @@ private void bindControllerCommands() { OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); +// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); + + OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.onTrue(new InstantCommand(ARM_ELEVATOR::resetElevatorPosition).ignoringDisable(true)); + OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.onTrue(new InstantCommand(INTAKE::resetIntakePosition).ignoringDisable(true)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 418efb0..0682185 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,17 +20,17 @@ public class CameraConstants { new Translation3d(0.2015, 0.195, 0.7156), new Rotation3d(0, Units.degreesToRadians(30), 0) ), - ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA = new Transform3d( + ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA = new Transform3d( new Translation3d(0.2247, 0.195, 0.7498), new Rotation3d(0, Units.degreesToRadians(90 - 51), 0) ), ROBOT_CENTER_TO_LEFT_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(-0.129, 0.2032, 0.1258), - new Rotation3d(0, Units.degreesToRadians(60), 0) + new Translation3d(-0.2032, 0.129, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), Units.degreesToRadians(180)) ), ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(-0.129, -0.2032, 0.1258), - new Rotation3d(0, Units.degreesToRadians(60), 0) + new Translation3d(-0.2032, -0.129, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), Units.degreesToRadians(180)) ); public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 1; @@ -39,10 +39,10 @@ public class CameraConstants { ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); public static final AprilTagCamera - INTAKE_SIDE_REEF_TAG_CAMERA = new AprilTagCamera( + FRONT_REEF_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "IntakeSideReefTagCameraCamera", - ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA, + "FrontReefTagCamera", + ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA, REEF_TAG_CAMERA_STANDARD_DEVIATIONS ), LEFT_REEF_TAG_CAMERA = new AprilTagCamera( diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index a6ef492..316df92 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; +import frc.trigon.robot.commands.commandfactories.ClimbCommands; import frc.trigon.robot.misc.ReefChooser; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -18,7 +19,7 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT = 0, REEF_CHOOSER_PORT = 1; private static final int - DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1, + DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 2, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2; public static final XboxController DRIVER_CONTROLLER = new XboxController( DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND @@ -63,7 +64,9 @@ public class OperatorConstants { FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); - + public static final Trigger + RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.m(), + RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.n(); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), @@ -79,7 +82,7 @@ public class OperatorConstants { SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeCommand) { - final Trigger scoreTrigger; + Trigger scoreTrigger; if (isRight) scoreTrigger = DRIVER_CONTROLLER.rightStick() @@ -92,6 +95,8 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + scoreTrigger = scoreTrigger.and(() -> !ClimbCommands.isClimbing()); + if (isAlgaeCommand) return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 4b8287c..3cb4a1e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; -import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -113,6 +112,10 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public void resetElevatorPosition() { + elevatorMasterMotor.setPosition(0); + } + public Pose3d calculateGamePieceCollectionPose() { return calculateVisualizationPose() .transformBy(ArmElevatorConstants.ARM_TO_HELD_GAME_PIECE); @@ -210,7 +213,7 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { final Rotation2d minimumSafeAngle = calculateMinimumArmSafeAngle(); final Rotation2d currentAngle = getCurrentArmAngle(); - if (minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { + if (!targetState.ignoreConstraints && minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { armMasterMotor.setControl(armPositionRequest.withPosition(currentAngle.getRotations() - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); return; } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 76fe574..a2fc771 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -179,7 +179,7 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 37; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 45; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0.1; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; @@ -315,28 +315,28 @@ private static void configureAngleEncoder() { public enum ArmElevatorState { PREPARE_SCORE_L1(Rotation2d.fromDegrees(20), 0.3, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(10), 0.3, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(10), 0.7, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(52), 0.17, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(52), 0.6, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), - REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), + REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.6), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.49, REST, true, 0.7), - UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-91), 0.51, REST, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(-91), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), - SCORE_L2(Rotation2d.fromDegrees(0), 0.3, PREPARE_SCORE_L2, false, 1), - SCORE_L3(Rotation2d.fromDegrees(0), 0.7, PREPARE_SCORE_L3, false, 1), - SCORE_L4(Rotation2d.fromDegrees(20), 1.5, PREPARE_SCORE_L4, false, 1), - SCORE_NET(Rotation2d.fromDegrees(70), 1.382, null, false, 0.3), + SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), + SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), + SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), + SCORE_NET(Rotation2d.fromDegrees(70), 1.636, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.4, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.12, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), - COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.29, null, false, 1); + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.13, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.20, null, false, 1); public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 0d0c72a..5ead62f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -28,7 +28,7 @@ public Climber() { @Override public void stop() { motor.stopMotor(); - setServoPowers(0); + stopServos(); } @Override @@ -45,7 +45,7 @@ public void updateMechanism() { public void updatePeriodically() { motor.update(); rightServo.update(); - leftServo.update(); + // leftServo.update(); } @Override @@ -100,9 +100,14 @@ void setTargetVoltage(double targetVoltage) { motor.setControl(voltageRequest.withOutput(targetVoltage)); } - private void setServoPowers(double power) { -// rightServo.setTargetSpeed(power); -// leftServo.setTargetSpeed(-power); + public void setServoPowers(double power) { + rightServo.setTargetSpeed(power); + leftServo.setTargetSpeed(-power); + } + + private void stopServos() { + rightServo.stop(); + leftServo.stop(); } private Pose3d calculateVisualizationPose() { 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 dbaf510..3eb9703 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -27,8 +27,8 @@ public class ClimberConstants { private static final int MOTOR_ID = 18, REVERSE_LIMIT_SENSOR_CHANNEL = 8, - RIGHT_SERVO_CHANNEL = 7, - LEFT_SERVO_CHANNEL = 8, + RIGHT_SERVO_CHANNEL = 0, + LEFT_SERVO_CHANNEL = 1, CAGE_SENSOR_CHANNEL = 2; private static final String MOTOR_NAME = "ClimberMotor", @@ -110,7 +110,7 @@ public class ClimberConstants { private static void configureMotor() { final TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 7e401e6..bbd31e5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -94,9 +94,9 @@ public enum EndEffectorState { EJECT(4), LOAD_CORAL(-4), UNLOAD_CORAL(4), - SCORE_CORAL(1), + SCORE_CORAL(1.5), COLLECT_ALGAE(-7), - SCORE_ALGAE(4), + SCORE_ALGAE(10), HOLD_CORAL(-0.5), HOLD_ALGAE(-7); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3e708f2..2a052bd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -105,6 +105,10 @@ public Translation3d calculateLinearIntakeVelocity() { ); } + public void resetIntakePosition() { + intakeMotor.setPosition(0); + } + void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; System.out.println("Setting intake state to " + targetState.name()); diff --git a/src/main/java/lib/hardware/misc/servo/Servo.java b/src/main/java/lib/hardware/misc/servo/Servo.java index 91533c2..3534ac0 100644 --- a/src/main/java/lib/hardware/misc/servo/Servo.java +++ b/src/main/java/lib/hardware/misc/servo/Servo.java @@ -38,6 +38,10 @@ public void update() { * @param targetSpeed the target speed of the servo, from -1.0 to 1.0. */ public void setTargetSpeed(double targetSpeed) { + if (targetSpeed == 0) { + stop(); + return; + } servoIO.setTargetSpeed(targetSpeed); } @@ -51,9 +55,17 @@ public void setTargetSpeed(double targetSpeed) { * @param value the target position/speed of the servo on a scale from 0 to 1 */ public void set(double value) { + if (value == 0) { + stop(); + return; + } servoIO.set(value); } + public void stop() { + servoIO.stop(); + } + /** * Set the target angle of the servo. * This method is used for angle control servos, where the PWM pulse's width dictates the angle of the servo. diff --git a/src/main/java/lib/hardware/misc/servo/ServoIO.java b/src/main/java/lib/hardware/misc/servo/ServoIO.java index 89a4184..971c28a 100644 --- a/src/main/java/lib/hardware/misc/servo/ServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/ServoIO.java @@ -27,6 +27,9 @@ protected void setTargetAngle(Rotation2d targetAngle) { protected void set(double value) { } + protected void stop() { + } + protected void setPWMBoundaries(int maximumPulseWidthMicroseconds, int maximumDeadbandRangeMicroseconds, int centerPulseMicroseconds, int minimumDeadbandRangeMicroseconds, int minimumPulseWidthMicroseconds) { diff --git a/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java b/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java index b6fa616..b9e4791 100644 --- a/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java @@ -36,6 +36,11 @@ protected void set(double value) { servo.set(MathUtil.clamp(value, 0, 1)); } + @Override + protected void stop() { + servo.setDisabled(); + } + @Override protected void setPWMBoundaries(int maximumPulseWidthMicroseconds, int maximumDeadbandRangeMicroseconds, int centerPulseMicroseconds, int minimumDeadbandRangeMicroseconds, diff --git a/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java b/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java index cc2cbe3..3008102 100644 --- a/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java @@ -27,6 +27,11 @@ protected void set(double value) { this.targetSpeed = MathUtil.clamp(value, 0, 1); } + @Override + protected void stop() { + this.targetSpeed = 0; + } + @Override protected void setTargetAngle(Rotation2d targetAngle) { final Rotation2d clampedAngle = clampAngleToServoRange(targetAngle); From c09aefc7a1300f9f1276ff7722a1c76b41b4715d Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Mon, 6 Oct 2025 02:06:00 +0300 Subject: [PATCH 21/53] Update AlgaeManipulationCommands.java --- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index d6467f6..530271a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -195,7 +195,7 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() { .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) .repeatedly() - ); + ).finallyDo(() -> SHOULD_COLLECT_FROM_LOLLIPOP = false); } private static Command getCollectAlgaeFromFloorSequenceCommand() { From 5fe75090e6d78cea612a9999b5a7129c3cfb2121 Mon Sep 17 00:00:00 2001 From: Shm Date: Sun, 5 Oct 2025 19:18:16 -0400 Subject: [PATCH 22/53] more changes Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AlgaeManipulationCommands.java | 2 +- .../commandfactories/CoralEjectionCommands.java | 4 ++-- .../frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- .../subsystems/armelevator/ArmElevatorConstants.java | 10 +++++----- .../subsystems/endeffector/EndEffectorConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/Intake.java | 2 +- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index d6467f6..a5518e5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.5))), + getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index 2c6030e..111f5d4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -17,8 +17,8 @@ public class CoralEjectionCommands { public static Command getCoralEjectionCommand() { return GeneralCommands.getContinuousConditionalCommand( - getEjectCoralFromIntakeCommand(), - getEjectCoralFromEndEffectorCommand(), + getEjectCoralFromIntakeCommand().asProxy(), + getEjectCoralFromEndEffectorCommand().asProxy(), () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 316df92..d1a4338 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -65,8 +65,8 @@ public class OperatorConstants { LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); public static final Trigger - RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.m(), - RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.n(); + RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.m()), + RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.n()); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index a2fc771..e0f236d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -314,9 +314,9 @@ private static void configureAngleEncoder() { } public enum ArmElevatorState { - PREPARE_SCORE_L1(Rotation2d.fromDegrees(20), 0.3, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(52), 0.17, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(52), 0.6, null, false, 1), + PREPARE_SCORE_L1(Rotation2d.fromDegrees(-10), 0.33, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.1, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.53, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), @@ -326,7 +326,7 @@ public enum ArmElevatorState { LOAD_CORAL(Rotation2d.fromDegrees(-91), 0.51, REST, true, 0.7), UNLOAD_CORAL(Rotation2d.fromDegrees(-91), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), - SCORE_L1(Rotation2d.fromDegrees(-20), 0.4, null, false, 1), + SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), @@ -336,7 +336,7 @@ public enum ArmElevatorState { COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.4, null, false, 1), COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.13, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), - COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.20, null, false, 1); + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index bbd31e5..266d95a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -95,7 +95,7 @@ public enum EndEffectorState { LOAD_CORAL(-4), UNLOAD_CORAL(4), SCORE_CORAL(1.5), - COLLECT_ALGAE(-7), + COLLECT_ALGAE(-11), SCORE_ALGAE(10), HOLD_CORAL(-0.5), HOLD_ALGAE(-7); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 2a052bd..0867382 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -106,7 +106,7 @@ public Translation3d calculateLinearIntakeVelocity() { } public void resetIntakePosition() { - intakeMotor.setPosition(0); + angleMotor.setPosition(0); } void setTargetState(IntakeConstants.IntakeState targetState) { From 98f9216e66bfa31d550fd377bc312abe96a10982 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Mon, 6 Oct 2025 03:35:06 +0300 Subject: [PATCH 23/53] load coral while driving to pose --- .../commandfactories/CoralPlacingCommands.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index cc02403..1f5ffe0 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -18,17 +18,14 @@ import lib.utilities.flippable.FlippableTranslation2d; public class CoralPlacingCommands { - public static boolean SHOULD_SCORE_AUTONOMOUSLY = false; + public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { - return new SequentialCommandGroup( - CoralCollectionCommands.getLoadCoralCommand(), - new ConditionalCommand( - getAutonomouslyScoreCommand(shouldScoreRight), - getScoreCommand(shouldScoreRight), - () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 - ) + return new ConditionalCommand( + getAutonomouslyScoreCommand(shouldScoreRight), + getScoreCommand(shouldScoreRight), + () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ).onlyIf(CoralCollectionCommands::hasCoral); } @@ -44,6 +41,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( + CoralCollectionCommands.getLoadCoralCommand(), getPrepareArmElevatorIfWontHitReef(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), @@ -54,7 +52,9 @@ private static Command getScoreCommand(boolean shouldScoreRight) { private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - getPrepareArmElevatorIfWontHitReef(shouldScoreRight), + new SequentialCommandGroup( + CoralCollectionCommands.getLoadCoralCommand(), + getPrepareArmElevatorIfWontHitReef(shouldScoreRight)), new SequentialCommandGroup( getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), new WaitUntilCommand(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), From 247c1987ab95ba5a680193fbb0acfe6742458136 Mon Sep 17 00:00:00 2001 From: Shm Date: Mon, 6 Oct 2025 01:38:05 -0400 Subject: [PATCH 24/53] PUSH Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../pathplanner/paths/Example Path.path | 54 +++++++++++++++++++ .../commandclasses/IntakeAssistCommand.java | 40 ++++++++------ .../commandfactories/AutonomousCommands.java | 4 +- .../CoralCollectionCommands.java | 18 ++++--- .../CoralPlacingCommands.java | 2 +- .../robot/constants/AutonomousConstants.java | 2 +- .../robot/constants/CameraConstants.java | 4 +- .../robot/constants/FieldConstants.java | 2 +- .../robot/constants/OperatorConstants.java | 2 +- .../ObjectPoseEstimator.java | 7 +-- .../subsystems/armelevator/ArmElevator.java | 2 +- .../armelevator/ArmElevatorConstants.java | 22 +++++--- .../subsystems/climber/ClimberConstants.java | 2 +- .../endeffector/EndEffectorConstants.java | 6 +-- .../robot/subsystems/intake/Intake.java | 4 +- .../subsystems/intake/IntakeConstants.java | 12 +++-- .../robot/subsystems/swerve/Swerve.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 2 +- .../swervemodule/SwerveModuleConstants.java | 6 +-- 19 files changed, 136 insertions(+), 57 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..91491b7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index ba2d2ce..2dc325e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -23,13 +23,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup { static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.8, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.2, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedGamePiece; /** @@ -96,8 +96,8 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getX())); final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getY())); - if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) - return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + if (assistMode.isAlternate) + return calculateAlternateAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar); } @@ -115,14 +115,14 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d return pow; } - private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + private static Translation2d calculateAlternateAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { final double pidScalar = Math.cbrt(joystickValue.getNorm()); final double - xJoystickPower = Math.cbrt(joystickValue.getX()), - yJoystickPower = Math.cbrt(joystickValue.getY()); + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); final double - xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower), - yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower); + xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, pidScalar, Math.cbrt(xJoystickPower)) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, pidScalar, Math.cbrt(yJoystickPower)) : yJoystickPower; return new Translation2d(xPower, yPower); } @@ -143,7 +143,7 @@ private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2 pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); - if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + if (assistMode.isAlternate) return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar); } @@ -173,29 +173,35 @@ public enum AssistMode { /** * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. */ - ALTERNATE_ASSIST(true, true, true), + ALTERNATE_ASSIST(true, true, true, true), + /** + * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. + */ + ALTERNATE_ALIGN(false, true, true, true), /** * Applies pid values to autonomously drive to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - FULL_ASSIST(true, true, true), + FULL_ASSIST(true, true, true, false), /** * Applies pid values to align to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - ALIGN_ASSIST(false, true, true), + ALIGN_ASSIST(false, true, true, false), /** * Applies pid values to face the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - ASSIST_ROTATION(false, false, true); + ASSIST_ROTATION(false, false, true, false); final boolean shouldAssistX, shouldAssistY, - shouldAssistTheta; + shouldAssistTheta, + isAlternate; - AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { + AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta, boolean isAlternate) { this.shouldAssistX = shouldAssistX; this.shouldAssistY = shouldAssistY; this.shouldAssistTheta = shouldAssistTheta; + this.isAlternate = isAlternate; } } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index ce47b1a..c68b713 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -91,10 +91,10 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( CoralCollectionCommands.getIntakeCoralCommand(), - ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + ArmElevatorCommands.getSetTargetStateCommand(() -> ArmElevatorConstants.ArmElevatorState.REST), getDriveToCoralCommand(isRight) ) - .until(RobotContainer.INTAKE::hasCoral) + .until(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.TRANSPORTER.hasCoral()) .unless(() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.END_EFFECTOR.hasGamePiece()); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 0906235..10c7fc4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -19,7 +19,7 @@ public class CoralCollectionCommands { public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( - getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral), + getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)), getCollectionConfirmationCommand(), new InstantCommand( () -> { @@ -27,14 +27,20 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).asProxy()); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy()); } public static Command getLoadCoralCommand() { - return new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), - EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) - ).until(RobotContainer.END_EFFECTOR::hasGamePiece).andThen( + return new SequentialCommandGroup( + new ParallelCommandGroup( + getIntakeCoralCommand(), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST) + ).until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), + new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) + ).until(RobotContainer.END_EFFECTOR::hasGamePiece), new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index cc02403..cdc5102 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -18,7 +18,7 @@ import lib.utilities.flippable.FlippableTranslation2d; public class CoralPlacingCommands { - public static boolean SHOULD_SCORE_AUTONOMOUSLY = false; + public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index a13abb4..dc32817 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -28,7 +28,7 @@ public class AutonomousConstants { public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 3, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 0682185..c7f2450 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -17,8 +17,8 @@ public class CameraConstants { ); private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( - new Translation3d(0.2015, 0.195, 0.7156), - new Rotation3d(0, Units.degreesToRadians(30), 0) + new Translation3d(0.2015, -0.195, 0.62), + new Rotation3d(0, Units.degreesToRadians(29.2), Units.degreesToRadians(15)) ), ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA = new Transform3d( new Translation3d(0.2247, 0.195, 0.7498), diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 8c71b29..bb7d379 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -28,7 +28,7 @@ public class FieldConstants { public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); public static final double - REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.35, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3; diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d1a4338..f3842a9 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -37,7 +37,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 64ea2cf..0a27dd5 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -22,9 +22,10 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. * - * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed - * @param gamePieceType the type of game piece to track - * @param camera the camera used for detecting objects + * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed + * @param distanceCalculationMethod the method used to calculate the distance from the game piece + * @param gamePieceType the type of game piece to track + * @param camera the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 3cb4a1e..9a6eb9e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -234,7 +234,7 @@ private Rotation2d calculateMinimumArmSafeAngle() { final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmElevatorConstants.ARM_LENGTH_METERS, 0, 1); final double acos = Math.acos(cosOfMinimumSafeAngle); return Double.isNaN(acos) - ? Rotation2d.fromDegrees(-90) + ? Rotation2d.fromDegrees(-100) : Rotation2d.fromRadians(acos).minus(Rotation2d.kCCW_90deg); } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index e0f236d..e499bf2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -7,7 +7,11 @@ import edu.wpi.first.math.geometry.*; 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.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; import lib.hardware.RobotHardwareStats; @@ -158,12 +162,17 @@ public class ArmElevatorConstants { public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; + static final Trigger IS_BREAKING_ARM_EVENT = new Trigger( + () -> Math.abs(ARM_MASTER_MOTOR.getSignal(TalonFXSignal.STATOR_CURRENT)) > 70 + ).debounce(0.1); + static { configureArmMasterMotor(); configureArmFollowerMotor(); configureElevatorMasterMotor(); configureElevatorFollowerMotor(); configureAngleEncoder(); + IS_BREAKING_ARM_EVENT.onTrue(new InstantCommand(ARM_MASTER_MOTOR::stopMotor)); } private static void configureArmMasterMotor() { @@ -314,23 +323,24 @@ private static void configureAngleEncoder() { } public enum ArmElevatorState { - PREPARE_SCORE_L1(Rotation2d.fromDegrees(-10), 0.33, null, false, 1), + PREPARE_SCORE_L1(Rotation2d.fromDegrees(-8), 0.5, null, false, 1), PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.1, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.53, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.6, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), - REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), + PREPARE_REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), + REST(Rotation2d.fromDegrees(-90), 0.534, PREPARE_REST, true, 0.8), REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(-91), 0.51, REST, true, 0.7), - UNLOAD_CORAL(Rotation2d.fromDegrees(-91), 0.603, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5, null, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), - SCORE_NET(Rotation2d.fromDegrees(70), 1.636, null, false, 0.3), + SCORE_NET(Rotation2d.fromDegrees(70), 1.644, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), 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 3eb9703..57ddef8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -29,7 +29,7 @@ public class ClimberConstants { REVERSE_LIMIT_SENSOR_CHANNEL = 8, RIGHT_SERVO_CHANNEL = 0, LEFT_SERVO_CHANNEL = 1, - CAGE_SENSOR_CHANNEL = 2; + CAGE_SENSOR_CHANNEL = 4; private static final String MOTOR_NAME = "ClimberMotor", REVERSE_LIMIT_SENSOR_NAME = "ClimberReverseLimitSensor", diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 266d95a..600b68d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -46,11 +46,11 @@ public class EndEffectorConstants { END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY ); - private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.1; + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.16; private static final double DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; - private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 6; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 5; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES @@ -98,7 +98,7 @@ public enum EndEffectorState { COLLECT_ALGAE(-11), SCORE_ALGAE(10), HOLD_CORAL(-0.5), - HOLD_ALGAE(-7); + HOLD_ALGAE(-6); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 0867382..6d5ed37 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -52,6 +52,7 @@ public void updatePeriodically() { intakeMotor.update(); angleMotor.update(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); + Logger.recordOutput("Intake/distanceSensorCM", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } @Override @@ -111,7 +112,6 @@ public void resetIntakePosition() { void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; - System.out.println("Setting intake state to " + targetState.name()); setTargetState(targetState.targetAngle, targetState.targetVoltage); } @@ -122,13 +122,11 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { private void setTargetVoltage(double voltage) { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(voltage); - System.out.println("Setting intake voltage to " + voltage); intakeMotor.setControl(voltageRequest.withOutput(voltage)); } private void setTargetAngle(Rotation2d targetAngle) { - System.out.println("Setting intake angle to " + targetAngle.getDegrees() + " degrees"); angleMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 577a387..ff36937 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -31,7 +31,7 @@ public class IntakeConstants { private static final int INTAKE_MOTOR_ID = 9, ANGLE_MOTOR_ID = 10, - DISTANCE_SENSOR_CHANNEL = 5; + DISTANCE_SENSOR_CHANNEL = 3; private static final String INTAKE_MOTOR_NAME = "IntakeMotor", ANGLE_MOTOR_NAME = "IntakeAngleMotor", @@ -40,7 +40,7 @@ public class IntakeConstants { INTAKE_MOTOR = new TalonFXMotor(INTAKE_MOTOR_ID, INTAKE_MOTOR_NAME), ANGLE_MOTOR = new TalonFXMotor(ANGLE_MOTOR_ID, ANGLE_MOTOR_NAME); static final SimpleSensor - DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); + DISTANCE_SENSOR = SimpleSensor.createDutyCycleSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); private static final double INTAKE_MOTOR_GEAR_RATIO = 4, @@ -101,11 +101,14 @@ public class IntakeConstants { ); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.5); + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.04; private static final double - COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 35; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - DISTANCE_SENSOR::getBinaryValue + () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( new Translation3d(0.6827, 0, 0), @@ -189,6 +192,7 @@ private static void configureAngleMotor() { private static void configureDistanceSensor() { // DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); + DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); } public enum IntakeState { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 9bae850..338e676 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -305,7 +305,7 @@ private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaP } private ChassisSpeeds calculateSelfRelativePIDToPoseSpeeds(FlippablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotFuturePose(0.2); final Pose2d flippedTargetPose = targetPose.get(); final double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); final double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 9bb6316..8a0b7bc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -61,7 +61,7 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(4.2, 0, 0), + new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : new PIDConstants(13, 0, 0.25); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 66f03c0..65420af 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -77,7 +77,7 @@ public static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 2; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 1; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0.24025; @@ -98,14 +98,14 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 30; + config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 50; config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.RotorToSensorRatio = REAR_STEER_MOTOR_GEAR_RATIO; config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; config.Feedback.FeedbackRemoteSensorID = feedbackRemoteSensorID; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 65; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 40; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; config.ClosedLoopGeneral.ContinuousWrap = true; From a88fb13b2ec65397ba19a5b68b017aecfbdf8549 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Mon, 6 Oct 2025 11:35:19 +0300 Subject: [PATCH 25/53] fix some sim stuff and alternate align Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commands/commandclasses/IntakeAssistCommand.java | 9 ++++----- .../frc/trigon/robot/constants/OperatorConstants.java | 2 +- .../subsystems/armelevator/ArmElevatorConstants.java | 2 +- .../trigon/robot/subsystems/endeffector/EndEffector.java | 3 ++- .../java/frc/trigon/robot/subsystems/intake/Intake.java | 4 ++-- .../trigon/robot/subsystems/intake/IntakeConstants.java | 5 ++--- 6 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 2dc325e..23210e9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -116,13 +116,12 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d } private static Translation2d calculateAlternateAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { - final double pidScalar = Math.cbrt(joystickValue.getNorm()); final double - xJoystickPower = joystickValue.getX(), - yJoystickPower = joystickValue.getY(); + xJoystickPower = Math.cbrt(joystickValue.getX()), + yJoystickPower = Math.cbrt(joystickValue.getY()); final double - xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, pidScalar, Math.cbrt(xJoystickPower)) : xJoystickPower, - yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, pidScalar, Math.cbrt(yJoystickPower)) : yJoystickPower; + xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, Math.cbrt(joystickValue.getX()), xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, Math.cbrt(joystickValue.getY()), yJoystickPower) : yJoystickPower; return new Translation2d(xPower, yPower); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f3842a9..d1a4338 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -37,7 +37,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index e499bf2..41763dd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -68,7 +68,7 @@ public class ArmElevatorConstants { ELEVATOR_MASS_KILOGRAMS = 7, DRUM_RADIUS_METERS = 0.04, MINIMUM_ELEVATOR_HEIGHT_METERS = 0, - MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.382; + MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.644; private static final int ARM_MOTOR_AMOUNT = 2, ELEVATOR_MOTOR_AMOUNT = 2; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index 9c0acd5..b7bdffc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Translation3d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; +import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -57,7 +58,7 @@ public Translation3d calculateLinearEndEffectorVelocity() { @AutoLogOutput(key = "EndEffector/IsEjecting") public boolean isEjecting() { - return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; + return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 1; } void setTargetState(EndEffectorConstants.EndEffectorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 6d5ed37..1e0fc4d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -77,13 +77,13 @@ public boolean atState(IntakeConstants.IntakeState targetState) { return targetState == this.targetState && atTargetAngle(); } - @AutoLogOutput(key = "CoralIntake/AtTargetAngle") + @AutoLogOutput(key = "Intake/AtTargetAngle") public boolean atTargetAngle() { final double angleDifferenceFromTargetAngleDegrees = Math.abs(getCurrentAngle().minus(targetState.targetAngle).getDegrees()); return angleDifferenceFromTargetAngleDegrees < IntakeConstants.ANGLE_TOLERANCE.getDegrees(); } - @AutoLogOutput(key = "CoralIntake/HasCoral") + @AutoLogOutput(key = "Intake/HasCoral") public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index ff36937..c685d93 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -75,8 +75,7 @@ public class IntakeConstants { MAXIMUM_ANGLE, SHOULD_SIMULATE_GRAVITY ); - private static final DoubleSupplier - DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 1000; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.2).per(Units.Second), @@ -191,7 +190,7 @@ private static void configureAngleMotor() { } private static void configureDistanceSensor() { -// DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); + DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); } From 0f718f0598ab1494f06eba7e10a70a9f200c3b0f Mon Sep 17 00:00:00 2001 From: Shm Date: Mon, 6 Oct 2025 07:05:05 -0400 Subject: [PATCH 26/53] ji Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 2 -- .../frc/trigon/robot/subsystems/swerve/SwerveConstants.java | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 41763dd..363faf8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -7,9 +7,7 @@ import edu.wpi.first.math.geometry.*; 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.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 8a0b7bc..030f862 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -61,7 +61,7 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(5, 0, 0), + new PIDConstants(4.5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : new PIDConstants(13, 0, 0.25); From 0399a40d106523a9dfdb295e5733398a3a5ab9d9 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Tue, 7 Oct 2025 21:55:23 +0300 Subject: [PATCH 27/53] added more conditions for load coral and added continue trigger for atounomouslu scoring --- .../commands/commandfactories/CoralCollectionCommands.java | 5 ++++- .../commands/commandfactories/CoralPlacingCommands.java | 2 +- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 10c7fc4..ce36fd6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -45,7 +45,10 @@ public static Command getLoadCoralCommand() { ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL) ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING)) - ).unless(RobotContainer.END_EFFECTOR::hasGamePiece); + ).unless(() -> RobotContainer.END_EFFECTOR.hasGamePiece() + && (RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.PREPARE_REST))); } public static Command getUnloadCoralCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 1f5ffe0..446f0f4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -31,7 +31,7 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight) || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 363faf8..7c164f1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -147,7 +147,7 @@ public class ArmElevatorConstants { static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.593; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.2); /** * The highest point of the arms angular zone where the safety logic applies. From 937117c4ded637df08d55b9e5c818d9d4f6f6068 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Tue, 7 Oct 2025 22:12:12 +0300 Subject: [PATCH 28/53] fix --- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index ce36fd6..94d8b0b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -46,7 +46,7 @@ public static Command getLoadCoralCommand() { EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL) ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING)) ).unless(() -> RobotContainer.END_EFFECTOR.hasGamePiece() - && (RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL) + && !(RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.PREPARE_REST))); } From 8f463beb63e5242d0fd53b325b7a8756f46b94ab Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Tue, 7 Oct 2025 22:48:29 +0300 Subject: [PATCH 29/53] good changes --- .../java/frc/trigon/robot/RobotContainer.java | 3 +- .../AlgaeManipulationCommands.java | 4 +-- .../commandfactories/ClimbCommands.java | 2 +- .../robot/constants/CameraConstants.java | 8 ++--- .../robot/subsystems/climber/Climber.java | 4 --- .../subsystems/climber/ClimberCommands.java | 8 +++++ .../subsystems/climber/ClimberConstants.java | 34 +++++++------------ 7 files changed, 29 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2999877..ac7039f 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.trigon.robot.subsystems.armelevator.ArmElevator; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; +import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.endeffector.EndEffector; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.intake.Intake; @@ -82,7 +83,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); -// CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); + CLIMBER.setDefaultCommand(ClimberCommands.getDefaultCommand()); END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getDefaultCommand()); INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 2093631..483eda7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -95,8 +95,8 @@ private static Command getScoreAlgaeCommand() { return new SelectCommand<>( Map.of( 0, getHoldAlgaeCommand(), - 1, CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY ? getAtonomouslyScoreInNetCommand() : getScoreInNetCommand(), - 2, CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY ? getAtonomouslyScoreInProcessorCommand() : getScoreInProcessorCommand() + 1, getScoreInNetCommand(), + 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index ff9cd03..490f3f1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -21,7 +21,7 @@ public static Command getClimbCommand() { return new SequentialCommandGroup( new InstantCommand(() -> IS_CLIMBING = true), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB) - .until(() -> RobotContainer.CLIMBER.hasCage() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), + .until(OperatorConstants.CONTINUE_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) .until(RobotContainer.CLIMBER::atTargetState), getAdjustClimbManuallyCommand() diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index c7f2450..ab6f359 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -17,12 +17,12 @@ public class CameraConstants { ); private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( - new Translation3d(0.2015, -0.195, 0.62), - new Rotation3d(0, Units.degreesToRadians(29.2), Units.degreesToRadians(15)) + new Translation3d(0.204, -0.170, 0.972), + new Rotation3d(0, Units.degreesToRadians(42), Units.degreesToRadians(15)) ), ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(0.2247, 0.195, 0.7498), - new Rotation3d(0, Units.degreesToRadians(90 - 51), 0) + new Translation3d(0.221, -0.165, 1.017), + new Rotation3d(0, Units.degreesToRadians(42), Units.degreesToRadians(15)) ), ROBOT_CENTER_TO_LEFT_REEF_TAG_CAMERA = new Transform3d( new Translation3d(-0.2032, 0.129, 0.1258), diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 5ead62f..fc50d23 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -79,10 +79,6 @@ public boolean atTargetState() { return Math.abs(getPositionRotations() - targetState.targetPositionRotations) < ClimberConstants.CLIMBER_TOLERANCE_ROTATIONS; } - public boolean hasCage() { - return ClimberConstants.HAS_CAGE_BOOLEAN_EVENT.getAsBoolean(); - } - void setTargetState(ClimberConstants.ClimberState targetState) { this.targetState = targetState; setTargetState(targetState.targetPositionRotations, targetState.targetServoPower, targetState.isAffectedByRobotWeight); diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 34b3970..4ae0c71 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -43,4 +43,12 @@ public static Command getSetTargetSpeedCommand(DoubleSupplier targetSpeed) { RobotContainer.CLIMBER ); } + + public static Command getDefaultCommand() { + return new StartEndCommand( + RobotContainer.CLIMBER::stop, + () -> {}, + RobotContainer.CLIMBER + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 57ddef8..70ba99d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -28,21 +29,18 @@ public class ClimberConstants { MOTOR_ID = 18, REVERSE_LIMIT_SENSOR_CHANNEL = 8, RIGHT_SERVO_CHANNEL = 0, - LEFT_SERVO_CHANNEL = 1, - CAGE_SENSOR_CHANNEL = 4; + LEFT_SERVO_CHANNEL = 1; private static final String MOTOR_NAME = "ClimberMotor", REVERSE_LIMIT_SENSOR_NAME = "ClimberReverseLimitSensor", RIGHT_SERVO_NAME = "ClimberRightServo", - LEFT_SERVO_NAME = "ClimberLeftServo", - CAGE_SENSOR_NAME = "ClimberCageSensor"; + LEFT_SERVO_NAME = "ClimberLeftServo"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); static final Servo RIGHT_SERVO = new Servo(RIGHT_SERVO_CHANNEL, RIGHT_SERVO_NAME), LEFT_SERVO = new Servo(LEFT_SERVO_CHANNEL, LEFT_SERVO_NAME); private static final SimpleSensor - REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME), - CAGE_SENSOR = SimpleSensor.createDigitalSensor(CAGE_SENSOR_CHANNEL, CAGE_SENSOR_NAME); + REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); static final int GROUNDED_PID_SLOT = 0, @@ -90,10 +88,6 @@ public class ClimberConstants { HAS_CAGE_DEBOUNCE_TIME_SECONDS = 0.5, REVERSE_LIMIT_DEBOUNCE_TIME_SECONDS = 0.1; static final BooleanEvent - HAS_CAGE_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - CAGE_SENSOR::getBinaryValue - ).debounce(HAS_CAGE_DEBOUNCE_TIME_SECONDS), REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), REVERSE_LIMIT_SENSOR::getBinaryValue @@ -113,31 +107,27 @@ private static void configureMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 20; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0.5; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : 0.2; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot1.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; config.Slot1.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot1.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot1.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; config.Slot1.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0; + config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : 0.5; config.Slot1.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; - config.Slot1.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot1.GravityType = GravityTypeValue.Elevator_Static; config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 2 : 2; - config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 10 : 10; - config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; + config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 10 : 3; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -177,7 +167,7 @@ private static void configureReverseLimitSensor() { public enum ClimberState { REST(0, 0, false), - PREPARE_FOR_CLIMB(3, 1, false), + PREPARE_FOR_CLIMB(-0.2, 1, false), CLIMB(0, 0, true); public final double targetPositionRotations; From 1c60f7dfd6f99aeedb23cde7f3a48a22d7654cce Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Tue, 7 Oct 2025 23:03:39 +0300 Subject: [PATCH 30/53] added no loading toggle --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 + .../commands/commandfactories/CoralCollectionCommands.java | 7 +++++-- .../robot/commands/commandfactories/GeneralCommands.java | 7 +++++++ .../java/frc/trigon/robot/constants/OperatorConstants.java | 3 ++- 4 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index ac7039f..2365a53 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -116,6 +116,7 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))); OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); + OperatorConstants.SHOULD_LOAD_CORAL_TOGGLE_TRIGGER.onTrue(GeneralCommands.getToggleShouldLoadCoralCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); // OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 94d8b0b..6b50397 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -17,17 +17,20 @@ import frc.trigon.robot.subsystems.transporter.TransporterConstants; public class CoralCollectionCommands { + public static boolean SHOULD_LOAD_CORAL = true; + public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)), getCollectionConfirmationCommand(), new InstantCommand( () -> { - if (!AlgaeManipulationCommands.isHoldingAlgae()) + if (!AlgaeManipulationCommands.isHoldingAlgae() && SHOULD_LOAD_CORAL) getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy()); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy() + ).finallyDo(() -> SHOULD_LOAD_CORAL = true); } public static Command getLoadCoralCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 2eb3150..9f0e9d8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -32,6 +32,13 @@ public static Command getToggleBrakeCommand() { }).ignoringDisable(true); } + public static Command getToggleShouldLoadCoralCommand() { + return new InstantCommand(() -> { + CoralCollectionCommands.SHOULD_LOAD_CORAL = !CoralCollectionCommands.SHOULD_LOAD_CORAL; + } + ); + } + public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { return new WaitCommand(delaySeconds).andThen(toRun).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d1a4338..6294064 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -58,7 +58,8 @@ public class OperatorConstants { CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(); + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(), + SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.povDown(); public static final Trigger SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), From 8738ba1a1cc165d0f9bb34a11494bb92da8d10c7 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Tue, 7 Oct 2025 23:44:49 +0300 Subject: [PATCH 31/53] fix load --- .../java/frc/trigon/robot/RobotContainer.java | 3 +++ .../commandclasses/IntakeAssistCommand.java | 14 +++++----- .../CoralCollectionCommands.java | 3 ++- .../commandfactories/GeneralCommands.java | 27 +++++++++++++++++++ .../robot/constants/OperatorConstants.java | 7 +++-- .../subsystems/armelevator/ArmElevator.java | 4 +++ 6 files changed, 48 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2365a53..4be4cc0 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -117,6 +117,9 @@ private void bindControllerCommands() { OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); OperatorConstants.SHOULD_LOAD_CORAL_TOGGLE_TRIGGER.onTrue(GeneralCommands.getToggleShouldLoadCoralCommand()); + OperatorConstants.SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldManipulateCoralAtonomouslyCommand()); + OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand()); + OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); // OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 23210e9..06984b7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -117,11 +117,11 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d private static Translation2d calculateAlternateAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { final double - xJoystickPower = Math.cbrt(joystickValue.getX()), - yJoystickPower = Math.cbrt(joystickValue.getY()); + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); final double - xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, Math.cbrt(joystickValue.getX()), xJoystickPower) : xJoystickPower, - yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, Math.cbrt(joystickValue.getY()), yJoystickPower) : yJoystickPower; + xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; return new Translation2d(xPower, yPower); } @@ -143,7 +143,7 @@ private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2 joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.isAlternate) - return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); + return calculateAlternateAssistPower(pidOutput, joystickValue); return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar); } @@ -151,8 +151,8 @@ private static double clampToOutputRange(double value) { return MathUtil.clamp(value, -1, 1); } - private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { - return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower; + private static double calculateAlternateAssistPower(double pidOutput, double joystickPower) { + return pidOutput * (1 - Math.abs(joystickPower)) + joystickPower; } private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 6b50397..6ea27c9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -18,6 +18,7 @@ public class CoralCollectionCommands { public static boolean SHOULD_LOAD_CORAL = true; + public static boolean SHOULD_USE_INTAKE_ASSIST = true; public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( @@ -29,7 +30,7 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy() + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy().onlyIf(() -> CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST) ).finallyDo(() -> SHOULD_LOAD_CORAL = true); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 9f0e9d8..5990100 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -39,6 +39,33 @@ public static Command getToggleShouldLoadCoralCommand() { ); } + public static Command getToggleShouldManipulateCoralAtonomouslyCommand() { + return new InstantCommand(() -> { + if (CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST || CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = false; + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = false; + } else { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = true; + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true; + } + } + ); + } + + public static Command getToggleShouldCollectCoralAtonomouslyCommand() { + return new InstantCommand(() -> { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = !CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST; + } + ); + } + + public static Command getToggleShouldScoreCoralAtonomouslyCommand() { + return new InstantCommand(() -> { + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = !CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY; + } + ); + } + public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { return new WaitCommand(delaySeconds).andThen(toRun).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 6294064..d91fd23 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -37,7 +37,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ALIGN; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), @@ -59,7 +59,10 @@ public class OperatorConstants { SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(), - SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.povDown(); + SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.povDown(), + SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER = DRIVER_CONTROLLER.povRight(), + SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.y(), + SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.t(); public static final Trigger SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 9a6eb9e..90bfd0e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -213,6 +213,10 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { final Rotation2d minimumSafeAngle = calculateMinimumArmSafeAngle(); final Rotation2d currentAngle = getCurrentArmAngle(); + if (getElevatorPositionRotations() < metersToRotations(ArmElevatorConstants.ArmElevatorState.REST.targetPositionMeters) && currentAngle.getDegrees() < -80) { + armMasterMotor.setControl(armPositionRequest.withPosition(-0.25 - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); + return; + } if (!targetState.ignoreConstraints && minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { armMasterMotor.setControl(armPositionRequest.withPosition(currentAngle.getRotations() - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); return; From 1e5ba976e955053abaf8644bb903d4e02e6158af Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Wed, 8 Oct 2025 00:36:43 +0300 Subject: [PATCH 32/53] added reset position commands --- src/main/java/frc/trigon/robot/RobotContainer.java | 8 ++++---- .../trigon/robot/constants/OperatorConstants.java | 5 +++-- .../robot/subsystems/armelevator/ArmElevator.java | 5 +++++ .../armelevator/ArmElevatorCommands.java | 14 ++++++++++++++ .../trigon/robot/subsystems/climber/Climber.java | 4 ++++ .../robot/subsystems/climber/ClimberCommands.java | 14 +++++++++++++- .../frc/trigon/robot/subsystems/intake/Intake.java | 4 +++- .../robot/subsystems/intake/IntakeCommands.java | 13 +++++++++++++ 8 files changed, 59 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 4be4cc0..8465079 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -121,10 +121,10 @@ private void bindControllerCommands() { OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand()); OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); -// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false)); - - OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.onTrue(new InstantCommand(ARM_ELEVATOR::resetElevatorPosition).ignoringDisable(true)); - OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.onTrue(new InstantCommand(INTAKE::resetIntakePosition).ignoringDisable(true)); +// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false));\ + OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.toggleOnTrue(ClimberCommands.resetClimberPositionCommand()); + OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.toggleOnTrue(IntakeCommands.resetIntakePositionCommand()); + OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.toggleOnTrue(ArmElevatorCommands.resetElevatorPositionCommand()); } 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 d91fd23..9d36bd7 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -69,8 +69,9 @@ public class OperatorConstants { LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); public static final Trigger - RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.m()), - RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.n()); + RESET_CLIMBER_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.b()), + RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.n()), + RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.m()); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 90bfd0e..8f69119 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -280,6 +281,10 @@ private void scaleElevatorPositionRequestSpeed(double speedScalar) { elevatorPositionRequest.Jerk = elevatorPositionRequest.Acceleration * 10; } + void setElevatorVoltage(double voltage) { + elevatorMasterMotor.setControl(voltageRequest.withOutput(voltage)); + } + private Pose3d getFirstStageComponentPose() { return calculateComponentPose(ArmElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight()); } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index aee7926..f033c68 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -3,14 +3,18 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; import lib.commands.NetworkTablesCommand; import java.util.Set; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; public class ArmElevatorCommands { @@ -36,6 +40,16 @@ public static Command getArmGearRatioCalulationCommand() { ); } + public static Command resetElevatorPositionCommand() { + return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, true)), + new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + RobotContainer.ARM_ELEVATOR::resetElevatorPosition, + RobotContainer.ARM_ELEVATOR + )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + } + public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { return getSetTargetStateCommand(() -> targetState); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index fc50d23..3117b5c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -96,6 +96,10 @@ void setTargetVoltage(double targetVoltage) { motor.setControl(voltageRequest.withOutput(targetVoltage)); } + void resetClimberPosition() { + motor.setPosition(0); + } + public void setServoPowers(double power) { rightServo.setTargetSpeed(power); leftServo.setTargetSpeed(-power); diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 4ae0c71..ffabd74 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; @@ -20,6 +22,15 @@ public static Command getDebuggingCommand() { ); } + public static Command resetClimberPositionCommand() { + return new ExecuteEndCommand( + () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + RobotContainer.CLIMBER::resetClimberPosition, + RobotContainer.CLIMBER + ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + } + + public static Command getSetTargetStateCommand(ClimberConstants.ClimberState targetState) { return new StartEndCommand( () -> RobotContainer.CLIMBER.setTargetState(targetState), @@ -47,7 +58,8 @@ public static Command getSetTargetSpeedCommand(DoubleSupplier targetSpeed) { public static Command getDefaultCommand() { return new StartEndCommand( RobotContainer.CLIMBER::stop, - () -> {}, + () -> { + }, RobotContainer.CLIMBER ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 1e0fc4d..3d3e8e1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -119,7 +119,9 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); setTargetAngle(targetAngle); } - + void setAngleMotorVoltage(double voltage) { + angleMotor.setControl(voltageRequest.withOutput(voltage)); + } private void setTargetVoltage(double voltage) { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(voltage); intakeMotor.setControl(voltageRequest.withOutput(voltage)); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 9194dbc..b89e08d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -2,8 +2,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; import java.util.Set; @@ -19,6 +24,14 @@ public static Command getDebuggingCommand() { ); } + public static Command resetIntakePositionCommand() { + return new ExecuteEndCommand( + () -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + RobotContainer.INTAKE::resetIntakePosition, + RobotContainer.INTAKE + ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + } + public static Command getPrepareForStateCommand(IntakeConstants.IntakeState targetState) { return new StartEndCommand( () -> RobotContainer.INTAKE.prepareForState(targetState), From b3f844cddc45b6b337ea7421424245398f6ba91b Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Wed, 8 Oct 2025 01:20:42 +0300 Subject: [PATCH 33/53] AHAHAHAHAHAH --- .../commandclasses/IntakeAssistCommand.java | 2 +- .../commandfactories/AutonomousCommands.java | 2 +- .../robot/constants/AutonomousConstants.java | 4 ++-- .../armelevator/ArmElevatorConstants.java | 20 +++++++++---------- .../robot/subsystems/swerve/Swerve.java | 9 ++++----- .../subsystems/swerve/SwerveConstants.java | 12 ++++++----- 6 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 06984b7..b7c9859 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -23,7 +23,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup { static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.8, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.2, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index c68b713..70f0d18 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -95,7 +95,7 @@ public static Command getCollectCoralCommand(boolean isRight) { getDriveToCoralCommand(isRight) ) .until(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.TRANSPORTER.hasCoral()) - .unless(() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.END_EFFECTOR.hasGamePiece()); + .unless(RobotContainer.TRANSPORTER::hasCoral); } public static Command getDriveToReefCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index dc32817..118abb0 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -28,12 +28,12 @@ public class AutonomousConstants { public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 3, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; - + private static final double AUTO_FIND_CORAL_POSE_X = 3.3, AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5, diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 7c164f1..92fd94c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -43,13 +43,13 @@ public class ArmElevatorConstants { static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME, RobotConstants.CANIVORE_NAME); static final double - ARM_GEAR_RATIO = 42, + ARM_GEAR_RATIO = 55.273, ELEVATOR_GEAR_RATIO = 4; private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.0625; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : edu.wpi.first.math.util.Units.degreesToRotations(-23.56) - ANGLE_ENCODER_GRAVITY_OFFSET; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.061279296875; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.241455078125 - 0.25 - 0.061279296875 - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; @@ -186,13 +186,13 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 45; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 60; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0.1; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 2; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.05; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.5; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.35; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; @@ -325,8 +325,8 @@ public enum ArmElevatorState { PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.1, null, false, 1), PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.6, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), - PREPARE_REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), - REST(Rotation2d.fromDegrees(-90), 0.534, PREPARE_REST, true, 0.8), + PREPARE_REST(Rotation2d.fromDegrees(-91), 0.603, null, false, 0.8), + REST(Rotation2d.fromDegrees(-91), 0.534, PREPARE_REST, true, 0.8), REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 338e676..9358507 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -1,8 +1,5 @@ package frc.trigon.robot.subsystems.swerve; -import com.pathplanner.lib.util.DriveFeedforwards; - -import com.pathplanner.lib.util.swerve.SwerveSetpoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -307,8 +304,10 @@ private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaP private ChassisSpeeds calculateSelfRelativePIDToPoseSpeeds(FlippablePose2d targetPose) { final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotFuturePose(0.2); final Pose2d flippedTargetPose = targetPose.get(); - final double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); - final double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); + double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); + xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.atSetpoint() ? 0 : xSpeed; + double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); + ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.atSetpoint() ? 0 : ySpeed; final int direction = Flippable.isRedAlliance() ? -1 : 1; final ChassisSpeeds targetFieldRelativeSpeeds = new ChassisSpeeds( xSpeed * direction, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 030f862..c89000d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -33,10 +33,10 @@ public class SwerveConstants { REAR_LEFT_STEER_ENCODER_OFFSET = -0.30517578125, REAR_RIGHT_STEER_ENCODER_OFFSET = -0.121826171875; private static final double - FRONT_LEFT_WHEEL_DIAMETER = 0.038 * 2, - FRONT_RIGHT_WHEEL_DIAMETER = 0.038 * 2, - REAR_LEFT_WHEEL_DIAMETER = 0.038 * 2, - REAR_RIGHT_WHEEL_DIAMETER = 0.038 * 2; + FRONT_LEFT_WHEEL_DIAMETER = 0.1016, + FRONT_RIGHT_WHEEL_DIAMETER = 0.1016, + REAR_LEFT_WHEEL_DIAMETER = 0.1016, + REAR_RIGHT_WHEEL_DIAMETER = 0.1016; static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{ new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER), new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER), @@ -61,7 +61,7 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(4.5, 0, 0), + new PIDConstants(5.5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : new PIDConstants(13, 0, 0.25); @@ -95,6 +95,8 @@ public class SwerveConstants { configureGyro(); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.enableContinuousInput(-SwerveConstants.MAXIMUM_PID_ANGLE, SwerveConstants.MAXIMUM_PID_ANGLE); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setTolerance(1); + SwerveConstants.X_TRANSLATION_PID_CONTROLLER.setTolerance(0.02); + SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.setTolerance(0.02); } private static void configureGyro() { From de3c6829de25a3b8c7d5160510b69b20764e43b5 Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 22:05:58 -0400 Subject: [PATCH 34/53] COmmit --- .../java/frc/trigon/robot/RobotContainer.java | 8 +-- .../commandfactories/ClimbCommands.java | 4 +- .../CoralCollectionCommands.java | 2 +- .../robot/constants/OperatorConstants.java | 8 +-- .../subsystems/armelevator/ArmElevator.java | 11 ++-- .../armelevator/ArmElevatorCommands.java | 2 +- .../armelevator/ArmElevatorConstants.java | 22 ++++---- .../subsystems/climber/ClimberCommands.java | 2 +- .../subsystems/climber/ClimberConstants.java | 51 +++++-------------- .../robot/subsystems/intake/Intake.java | 3 +- .../subsystems/swerve/SwerveConstants.java | 4 +- .../swervemodule/SwerveModuleConstants.java | 4 +- 12 files changed, 50 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 8465079..3cb4428 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -121,10 +121,10 @@ private void bindControllerCommands() { OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand()); OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); -// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ArmElevatorCommands.getDebuggingCommand(false));\ - OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.toggleOnTrue(ClimberCommands.resetClimberPositionCommand()); - OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.toggleOnTrue(IntakeCommands.resetIntakePositionCommand()); - OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.toggleOnTrue(ArmElevatorCommands.resetElevatorPositionCommand()); +// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ClimberCommands.getDebuggingCommand()); + OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.toggleOnTrue(ClimberCommands.resetClimberPositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.toggleOnTrue(IntakeCommands.resetIntakePositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.toggleOnTrue(ArmElevatorCommands.resetElevatorPositionCommand().ignoringDisable(true)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index 490f3f1..a053f7d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -20,6 +20,8 @@ public class ClimbCommands { public static Command getClimbCommand() { return new SequentialCommandGroup( new InstantCommand(() -> IS_CLIMBING = true), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.BREAK_ZIP_TIE) + .until(() -> RobotContainer.CLIMBER.atState(ClimberConstants.ClimberState.BREAK_ZIP_TIE)), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB) .until(OperatorConstants.CONTINUE_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) @@ -41,7 +43,7 @@ private static Command getAdjustClimbManuallyCommand() { () -> 0, () -> 0, () -> 0 - ) + ).asProxy() ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 6ea27c9..1c4247e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -39,7 +39,7 @@ public static Command getLoadCoralCommand() { new ParallelCommandGroup( getIntakeCoralCommand(), ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST) - ).until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)), + ).until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)).unless(RobotContainer.END_EFFECTOR::hasGamePiece), ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 9d36bd7..2fd36cc 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -52,22 +52,22 @@ public class OperatorConstants { public static final Trigger FLOOR_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftBumper(),//TODO: Add operator control REEF_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.a()), - STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povLeft(), + STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povRight(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(true, true)), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(false, true)), CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(), - SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.povDown(), - SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER = DRIVER_CONTROLLER.povRight(), + SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.back(), + SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER = DRIVER_CONTROLLER.povLeft(), SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.y(), SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.t(); public static final Trigger SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), - CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); + CLIMB_TRIGGER = DRIVER_CONTROLLER.povDown().or(OPERATOR_CONTROLLER.c()); public static final Trigger RESET_CLIMBER_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.b()), RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.n()), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 8f69119..687af08 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; -import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -215,15 +214,19 @@ void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { final Rotation2d minimumSafeAngle = calculateMinimumArmSafeAngle(); final Rotation2d currentAngle = getCurrentArmAngle(); if (getElevatorPositionRotations() < metersToRotations(ArmElevatorConstants.ArmElevatorState.REST.targetPositionMeters) && currentAngle.getDegrees() < -80) { - armMasterMotor.setControl(armPositionRequest.withPosition(-0.25 - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); + sendControlToArm(-0.25); return; } if (!targetState.ignoreConstraints && minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { - armMasterMotor.setControl(armPositionRequest.withPosition(currentAngle.getRotations() - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); + sendControlToArm(currentAngle.getRotations()); return; } final double targetPosition = ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()); - armMasterMotor.setControl(armPositionRequest.withPosition(targetPosition - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); + sendControlToArm(targetPosition); + } + + void sendControlToArm(double targetRotation) { + armMasterMotor.setControl(armPositionRequest.withPosition(targetRotation - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); } void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignoreConstraints) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index f033c68..6aa4cac 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -42,7 +42,7 @@ public static Command getArmGearRatioCalulationCommand() { public static Command resetElevatorPositionCommand() { return new SequentialCommandGroup( - new InstantCommand(() -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, true)), + new InstantCommand(() -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, false)), new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), RobotContainer.ARM_ELEVATOR::resetElevatorPosition, diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 92fd94c..9526632 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -48,14 +48,14 @@ public class ArmElevatorConstants { private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = -0.061279296875; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.241455078125 - 0.25 - 0.061279296875 - ANGLE_ENCODER_GRAVITY_OFFSET; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0.184814453125 - 0.25; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.184814453125 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2.5, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 5, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 4, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; @@ -186,10 +186,10 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 60; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 50; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.05; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.04; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.5; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358; @@ -322,16 +322,16 @@ private static void configureAngleEncoder() { public enum ArmElevatorState { PREPARE_SCORE_L1(Rotation2d.fromDegrees(-8), 0.5, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.1, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.13, null, false, 1), PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.6, null, false, 1), PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), - PREPARE_REST(Rotation2d.fromDegrees(-91), 0.603, null, false, 0.8), - REST(Rotation2d.fromDegrees(-91), 0.534, PREPARE_REST, true, 0.8), + PREPARE_REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), + REST(Rotation2d.fromDegrees(-90), 0.534, PREPARE_REST, true, 0.8), REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.5, null, true, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.485, null, true, 0.7), UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1), @@ -341,8 +341,8 @@ public enum ArmElevatorState { SCORE_NET(Rotation2d.fromDegrees(70), 1.644, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 0.953, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.4, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.2, null, false, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.3, null, false, 1), COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.13, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index ffabd74..2519272 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -24,7 +24,7 @@ public static Command getDebuggingCommand() { public static Command resetClimberPositionCommand() { return new ExecuteEndCommand( - () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 3), RobotContainer.CLIMBER::resetClimberPosition, RobotContainer.CLIMBER ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); 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 70ba99d..cbaecad 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -10,44 +10,33 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.servo.Servo; -import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.hardware.simulation.SimpleMotorSimulation; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; -import java.util.function.DoubleSupplier; - public class ClimberConstants { private static final int MOTOR_ID = 18, - REVERSE_LIMIT_SENSOR_CHANNEL = 8, RIGHT_SERVO_CHANNEL = 0, LEFT_SERVO_CHANNEL = 1; private static final String MOTOR_NAME = "ClimberMotor", - REVERSE_LIMIT_SENSOR_NAME = "ClimberReverseLimitSensor", RIGHT_SERVO_NAME = "ClimberRightServo", LEFT_SERVO_NAME = "ClimberLeftServo"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); static final Servo RIGHT_SERVO = new Servo(RIGHT_SERVO_CHANNEL, RIGHT_SERVO_NAME), LEFT_SERVO = new Servo(LEFT_SERVO_CHANNEL, LEFT_SERVO_NAME); - private static final SimpleSensor - REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); static final int GROUNDED_PID_SLOT = 0, ON_CAGE_PID_SLOT = 1; private static final double GEAR_RATIO = 37.5; - private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; - private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3; // private static final int // SERVO_PULSE_WIDTH_MICROSECONDS = 20000, // SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0, @@ -84,49 +73,39 @@ public class ClimberConstants { ); static final double MAXIMUM_MANUAL_CONTROL_VOLTAGE = 4; - private static final double - HAS_CAGE_DEBOUNCE_TIME_SECONDS = 0.5, - REVERSE_LIMIT_DEBOUNCE_TIME_SECONDS = 0.1; - static final BooleanEvent - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ).debounce(REVERSE_LIMIT_DEBOUNCE_TIME_SECONDS); - private static final DoubleSupplier REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER = () -> 0; - static final double CLIMBER_TOLERANCE_ROTATIONS = 0.01; + static final double CLIMBER_TOLERANCE_ROTATIONS = 0.02; static { configureMotor(); configureServos(); - configureReverseLimitSensor(); } private static void configureMotor() { final TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 20; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 70; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0.5; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : 0.2; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 2.2; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : -0.2001953125; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot1.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; + config.Slot1.kP = RobotHardwareStats.isSimulation() ? 22.373 : 80; config.Slot1.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot1.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot1.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; - config.Slot1.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0; - config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : 0.5; + config.Slot1.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 2.2; + config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : -1; config.Slot1.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; config.Slot1.GravityType = GravityTypeValue.Elevator_Static; - config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 2 : 2; + config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 2 : 1.5; config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 10 : 3; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -139,8 +118,6 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); - MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } private static void configureServos() { @@ -160,15 +137,11 @@ private static void configureServos() { // ); } - private static void configureReverseLimitSensor() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); - } - public enum ClimberState { REST(0, 0, false), - PREPARE_FOR_CLIMB(-0.2, 1, false), - CLIMB(0, 0, true); + BREAK_ZIP_TIE(-0.32, 0, false), + PREPARE_FOR_CLIMB(0, 1, false), + CLIMB(-1.6, 0, true); public final double targetPositionRotations; public final double targetServoPower; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3d3e8e1..3c5042b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -119,15 +119,16 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); setTargetAngle(targetAngle); } + void setAngleMotorVoltage(double voltage) { angleMotor.setControl(voltageRequest.withOutput(voltage)); } + private void setTargetVoltage(double voltage) { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(voltage); intakeMotor.setControl(voltageRequest.withOutput(voltage)); } - private void setTargetAngle(Rotation2d targetAngle) { angleMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index c89000d..2ed60bb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -61,10 +61,10 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(5.5, 0, 0), + new PIDConstants(6, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : - new PIDConstants(13, 0, 0.25); + new PIDConstants(10, 0, 0.1); private static final double MAXIMUM_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : Units.radiansToDegrees(MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND), MAXIMUM_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 900; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 65420af..2981bd8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -77,10 +77,10 @@ public static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 1; + 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.016046 : 0.24025; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0.2; config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0.91963; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0.069151; From 9ee334c9b075bd564ce5e3d87fb0ac3347cf3d00 Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 22:44:52 -0400 Subject: [PATCH 35/53] commit --- .../commandfactories/AutonomousCommands.java | 56 ++++++++++--------- .../robot/constants/FieldConstants.java | 2 +- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 70f0d18..6338613 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -2,6 +2,8 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; @@ -32,13 +34,13 @@ public class AutonomousCommands { private static FlippablePose2d TARGET_SCORING_POSE = null; private static boolean IS_FIRST_CORAL = true; - public static Command getFloorAutonomousCommand(boolean isRight) { - return getCycleCoralCommand(isRight).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left")); + public static Command getFloorAutonomousCommand(boolean isRight, FieldConstants.ReefClockPosition... reefClockPositions) { + return getCycleCoralCommand(isRight, reefClockPositions).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left") + (reefClockPositions.length * 2) + "Branches"); } - public static Command getCycleCoralCommand(boolean isRight) { + public static Command getCycleCoralCommand(boolean isRight, FieldConstants.ReefClockPosition[] reefClockPositions) { return new SequentialCommandGroup( - getDriveToReefAndScoreCommand(), + getDriveToReefAndScoreCommand(reefClockPositions), getCollectCoralCommand(isRight) ); } @@ -81,13 +83,22 @@ private static Command getDriveToFirstCoralPoseCommand(boolean isRight) { ).alongWith(new InstantCommand(() -> IS_FIRST_CORAL = false)); } - public static Command getDriveToReefAndScoreCommand() { + public static Command getDriveToReefAndScoreCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { return new ParallelRaceGroup( - getDriveToReefCommand(), + getDriveToReefCommand(reefClockPositions), getCoralSequenceCommand() ); } + public static Command getDriveToReefCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { + return new SequentialCommandGroup( + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose(reefClockPositions, false)), + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> calculateClosestOpenScoringPose(reefClockPositions, true), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + ); + } + public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( CoralCollectionCommands.getIntakeCoralCommand(), @@ -98,14 +109,6 @@ public static Command getCollectCoralCommand(boolean isRight) { .unless(RobotContainer.TRANSPORTER::hasCoral); } - public static Command getDriveToReefCommand() { - return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() - ); - } - public static Command getCoralSequenceCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getLoadCoralCommand(), @@ -114,14 +117,6 @@ public static Command getCoralSequenceCommand() { ); } - public static Command getDriveToCoralCommand(boolean isRight) { - return new ConditionalCommand( - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(10), - getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null - ).repeatedly(); - } - public static Command getScoreCommand() { return new SequentialCommandGroup( getOpenElevatorWhenCloseToReefCommand().until(AutonomousCommands::canScore), @@ -157,13 +152,13 @@ private static double calculateDistanceToTargetScoringPose() { return currentTranslation.getDistance(targetTranslation); } - public static FlippablePose2d calculateClosestOpenScoringPose() { + public static FlippablePose2d calculateClosestOpenScoringPose(FieldConstants.ReefClockPosition[] reefClockPositions, boolean shouldStayBehindAlgae) { final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); double closestDistance = Double.POSITIVE_INFINITY; Pose2d closestScoringPose = null; - for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { + for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { if (scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) continue; @@ -171,7 +166,10 @@ public static FlippablePose2d calculateClosestOpenScoringPose() { final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); if (distance < closestDistance) { closestDistance = distance; - closestScoringPose = reefSideScoringPose; + if (shouldStayBehindAlgae) + closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(FieldConstants.REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS - FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, 0), new Rotation2d())); + else + closestScoringPose = reefSideScoringPose; } } } @@ -179,6 +177,14 @@ public static FlippablePose2d calculateClosestOpenScoringPose() { return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); } + public static Command getDriveToCoralCommand(boolean isRight) { + return new ConditionalCommand( + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(10), + getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null + ).repeatedly(); + } + private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index bb7d379..12308b7 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -31,7 +31,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.35, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, - REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3; + REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2; public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.25; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; From b15b4dd23f0dae9d6dc8f67b835f82a90a100755 Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 22:48:54 -0400 Subject: [PATCH 36/53] faster --- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/transporter/TransporterCommands.java | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 9526632..8f13c4d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -57,8 +57,8 @@ public class ArmElevatorConstants { ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2.5, ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 4, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, - ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, - ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; + ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 21, + ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 55; static final boolean FOC_ENABLED = true; public static final double diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3c5042b..03ecbb1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -69,7 +69,7 @@ public void updateMechanism() { @Override public void stop() { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(0); - intakeMotor.stopMotor(); +// intakeMotor.stopMotor(); angleMotor.stopMotor(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java index 5774876..f9e2b34 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java @@ -30,7 +30,7 @@ public static Command getSetTargetStateWithPulsesCommand(TransporterConstants.Tr public static Command getSetTargetStateCommand(TransporterConstants.TransporterState targetState) { return new StartEndCommand( () -> RobotContainer.TRANSPORTER.setTargetState(targetState), - RobotContainer.TRANSPORTER::stop, + () -> {}, RobotContainer.TRANSPORTER ); } @@ -38,7 +38,7 @@ public static Command getSetTargetStateCommand(TransporterConstants.TransporterS public static Command getSetTargetStateCommand(double rightMotorVoltage, double leftMotorVoltage) { return new StartEndCommand( () -> RobotContainer.TRANSPORTER.setTargetState(rightMotorVoltage, leftMotorVoltage), - RobotContainer.TRANSPORTER::stop, + () -> {}, RobotContainer.TRANSPORTER ); } From 41a47b50e6613a33e49ab6ce9b423e2ab6e6a9bd Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 22:52:40 -0400 Subject: [PATCH 37/53] fixed resetting elevator position to not hit print --- .../robot/subsystems/armelevator/ArmElevatorCommands.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 6aa4cac..8771ef5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -2,8 +2,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; @@ -14,7 +12,6 @@ import lib.commands.NetworkTablesCommand; import java.util.Set; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; public class ArmElevatorCommands { @@ -42,7 +39,9 @@ public static Command getArmGearRatioCalulationCommand() { public static Command resetElevatorPositionCommand() { return new SequentialCommandGroup( - new InstantCommand(() -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, false)), + new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, false), + RobotContainer.ARM_ELEVATOR::stop), new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), RobotContainer.ARM_ELEVATOR::resetElevatorPosition, From 73878720665b552c33add7c24e3e66fd38ac10fc Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 8 Oct 2025 06:02:08 +0300 Subject: [PATCH 38/53] big ass commit Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/java/frc/trigon/robot/RobotContainer.java | 11 ++++++----- .../commandfactories/CoralCollectionCommands.java | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 3cb4428..25bceb0 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -13,10 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.*; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.LEDConstants; -import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.*; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; @@ -167,7 +164,11 @@ private void buildAutoChooser() { addCommandsToChooser( AutonomousCommands.getFloorAutonomousCommand(true), - AutonomousCommands.getFloorAutonomousCommand(false) + AutonomousCommands.getFloorAutonomousCommand(false), + AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK) ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 1c4247e..4015aff 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -40,7 +40,7 @@ public static Command getLoadCoralCommand() { getIntakeCoralCommand(), ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST) ).until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)).unless(RobotContainer.END_EFFECTOR::hasGamePiece), - ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) From 401e263edaacfa458a43e293ff357befcdbe3f73 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 8 Oct 2025 06:07:25 +0300 Subject: [PATCH 39/53] lower predict time Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 9358507..59ec58d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -302,7 +302,7 @@ private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaP } private ChassisSpeeds calculateSelfRelativePIDToPoseSpeeds(FlippablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotFuturePose(0.2); + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotFuturePose(0.13); final Pose2d flippedTargetPose = targetPose.get(); double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.atSetpoint() ? 0 : xSpeed; From 73f86723ce67216106d4da0845913690e7134408 Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 23:15:08 -0400 Subject: [PATCH 40/53] log stuff --- .../commands/commandfactories/CoralCollectionCommands.java | 4 ++++ .../robot/commands/commandfactories/CoralPlacingCommands.java | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 1c4247e..7307f75 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -15,9 +15,13 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class CoralCollectionCommands { + @AutoLogOutput(key = "Autonomous/ShouldLoadCoral") public static boolean SHOULD_LOAD_CORAL = true; + @AutoLogOutput(key = "Autonomous/ShouldAssistIntake") public static boolean SHOULD_USE_INTAKE_ASSIST = true; public static Command getCoralCollectionCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 446f0f4..14c6fe4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -16,8 +16,10 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableTranslation2d; +import org.littletonrobotics.junction.AutoLogOutput; public class CoralPlacingCommands { + @AutoLogOutput(key = "Autonomous/ShouldScoreAutonomously") public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; From b30244aa3fd99ec349840b7af88aba80e532cc1d Mon Sep 17 00:00:00 2001 From: Shm Date: Tue, 7 Oct 2025 23:26:23 -0400 Subject: [PATCH 41/53] more logging --- .../java/frc/trigon/robot/subsystems/climber/Climber.java | 4 +++- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 3117b5c..ad11152 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -10,6 +10,8 @@ import lib.hardware.misc.servo.Servo; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Climber extends MotorSubsystem { @@ -118,7 +120,7 @@ private Pose3d calculateVisualizationPose() { return ClimberConstants.CLIMBER_VISUALIZATION_ORIGIN_POINT.transformBy(climberTransform); } - + @AutoLogOutput(key = "Climber/ClimberPosition") private double getPositionRotations() { return motor.getSignal(TalonFXSignal.POSITION); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 03ecbb1..ad9c117 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -141,6 +141,7 @@ private Pose3d calculateVisualizationPose() { return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } + @AutoLogOutput(key = "Intake/IntakeAngle") private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } From 0e65033dff01628330bfd05bc44f6b13bc412070 Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 00:08:57 -0400 Subject: [PATCH 42/53] more log changes --- .../java/frc/trigon/robot/RobotContainer.java | 2 +- .../CoralCollectionCommands.java | 2 -- .../commandfactories/CoralPlacingCommands.java | 1 - .../commandfactories/GeneralCommands.java | 18 +++++++++++------- .../subsystems/armelevator/ArmElevator.java | 7 +++++-- .../subsystems/endeffector/EndEffector.java | 5 ++--- .../trigon/robot/subsystems/intake/Intake.java | 8 +++++--- .../subsystems/transporter/Transporter.java | 2 +- 8 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 25bceb0..5cd28bc 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -114,7 +114,7 @@ private void bindControllerCommands() { OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); OperatorConstants.SHOULD_LOAD_CORAL_TOGGLE_TRIGGER.onTrue(GeneralCommands.getToggleShouldLoadCoralCommand()); - OperatorConstants.SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldManipulateCoralAtonomouslyCommand()); + OperatorConstants.SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldManipulateCoralAutonomouslyCommand()); OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand()); OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 25f10fd..70b885a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -19,9 +19,7 @@ import org.littletonrobotics.junction.AutoLogOutput; public class CoralCollectionCommands { - @AutoLogOutput(key = "Autonomous/ShouldLoadCoral") public static boolean SHOULD_LOAD_CORAL = true; - @AutoLogOutput(key = "Autonomous/ShouldAssistIntake") public static boolean SHOULD_USE_INTAKE_ASSIST = true; public static Command getCoralCollectionCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 14c6fe4..d23e307 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -19,7 +19,6 @@ import org.littletonrobotics.junction.AutoLogOutput; public class CoralPlacingCommands { - @AutoLogOutput(key = "Autonomous/ShouldScoreAutonomously") public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 5990100..3470692 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -39,7 +39,7 @@ public static Command getToggleShouldLoadCoralCommand() { ); } - public static Command getToggleShouldManipulateCoralAtonomouslyCommand() { + public static Command getToggleShouldManipulateCoralAutonomouslyCommand() { return new InstantCommand(() -> { if (CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST || CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) { CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = false; @@ -48,8 +48,7 @@ public static Command getToggleShouldManipulateCoralAtonomouslyCommand() { CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = true; CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true; } - } - ); + }); } public static Command getToggleShouldCollectCoralAtonomouslyCommand() { @@ -70,7 +69,8 @@ public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { return new WaitCommand(delaySeconds).andThen(toRun).ignoringDisable(true); } - public static Command getContinuousConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { + public static Command getContinuousConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier + condition) { return new ConditionalCommand( onTrue.onlyWhile(condition), onFalse.until(condition), @@ -110,19 +110,23 @@ public static Command getResetFlipArmOverrideCommand() { return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); } - public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, + boolean isPrepareState, BooleanSupplier shouldStartFlipped) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand + (Supplier targetState, boolean isPrepareState, BooleanSupplier + shouldStartFlipped) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : ArmElevatorCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, + boolean isPrepareState) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 687af08..a56dcda 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; @@ -98,9 +99,11 @@ public void updatePeriodically() { armMasterMotor.update(); angleEncoder.update(); elevatorMasterMotor.update(); - Logger.recordOutput("Elevator/CurrentPositionMeters", getCurrentElevatorPositionMeters()); - Logger.recordOutput("Arm/CurrentPositionDegrees", getCurrentArmAngle().getDegrees()); + Logger.recordOutput("Elevator/ElevatorPositionMeters", getCurrentElevatorPositionMeters()); + Logger.recordOutput("Arm/ArmPositionDegrees", getCurrentArmAngle().getDegrees()); Logger.recordOutput("Arm/isPrepareArmAngleAboveCurrentArmAngle", CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle()); + Logger.recordOutput("AutonomousScoring", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + Logger.recordOutput("ShouldLoadCoral", CoralCollectionCommands.SHOULD_LOAD_CORAL); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index b7bdffc..d1ff92b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Translation3d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; -import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -32,10 +31,10 @@ public void updatePeriodically() { EndEffectorConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("EndEffector/isHoldingAlgae", AlgaeManipulationCommands.isHoldingAlgae()); - Logger.recordOutput("EndEffector/distanceSensorCM", EndEffectorConstants.DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("EndEffector/EndEffectorSensorCM", EndEffectorConstants.DISTANCE_SENSOR.getScaledValue()); } - @AutoLogOutput(key = "EndEffector/HasCoral") + @AutoLogOutput(key = "EndEffector/EndEffectorHasCoral") public boolean hasGamePiece() { return EndEffectorConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index ad9c117..a0e2ff6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ 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.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; @@ -52,7 +53,9 @@ public void updatePeriodically() { intakeMotor.update(); angleMotor.update(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); - Logger.recordOutput("Intake/distanceSensorCM", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("Intake/IntakeSensorCM", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("Intake/IntakeAngle", getCurrentAngle().getDegrees()); + Logger.recordOutput("AutonomousIntake", CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST); } @Override @@ -83,7 +86,7 @@ public boolean atTargetAngle() { return angleDifferenceFromTargetAngleDegrees < IntakeConstants.ANGLE_TOLERANCE.getDegrees(); } - @AutoLogOutput(key = "Intake/HasCoral") + @AutoLogOutput(key = "Intake/IntakeHasCoral") public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } @@ -141,7 +144,6 @@ private Pose3d calculateVisualizationPose() { return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } - @AutoLogOutput(key = "Intake/IntakeAngle") private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index abbdacc..8c2f7de 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -42,7 +42,7 @@ public TransporterConstants.TransporterState getTargetState() { return targetState; } - @AutoLogOutput(key = "Transporter/hasCoral") + @AutoLogOutput(key = "Transporter/TransporterHasCoral") public boolean hasCoral() { return TransporterConstants.HAS_CORAL_BOOLEAN_EVENT.getAsBoolean(); } From ac2ca0446fa3bce7e893d6efaaba851aa5f2247b Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 00:10:23 -0400 Subject: [PATCH 43/53] fix --- .../frc/trigon/robot/subsystems/armelevator/ArmElevator.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index a56dcda..b8305d7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -102,8 +102,8 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/ElevatorPositionMeters", getCurrentElevatorPositionMeters()); Logger.recordOutput("Arm/ArmPositionDegrees", getCurrentArmAngle().getDegrees()); Logger.recordOutput("Arm/isPrepareArmAngleAboveCurrentArmAngle", CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle()); - Logger.recordOutput("AutonomousScoring", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); - Logger.recordOutput("ShouldLoadCoral", CoralCollectionCommands.SHOULD_LOAD_CORAL); + Logger.recordOutput("Arm/AutonomousScoring", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + Logger.recordOutput("Arm/ShouldLoadCoral", CoralCollectionCommands.SHOULD_LOAD_CORAL); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index a0e2ff6..9fb3af4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -55,7 +55,7 @@ public void updatePeriodically() { IntakeConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("Intake/IntakeSensorCM", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); Logger.recordOutput("Intake/IntakeAngle", getCurrentAngle().getDegrees()); - Logger.recordOutput("AutonomousIntake", CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST); + Logger.recordOutput("Intake/AutonomousIntake", CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST); } @Override From ea633be214aae4aa540e1973a2748f77c8e3c213 Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 04:12:52 -0400 Subject: [PATCH 44/53] p8 --- src/main/java/frc/trigon/robot/RobotContainer.java | 6 +++--- .../commands/commandfactories/ClimbCommands.java | 6 +++--- .../commandfactories/CoralCollectionCommands.java | 2 +- .../commands/commandfactories/GeneralCommands.java | 8 ++++---- .../armelevator/ArmElevatorCommands.java | 10 ++++++---- .../armelevator/ArmElevatorConstants.java | 14 ++++++++------ .../robot/subsystems/climber/ClimberConstants.java | 4 ++-- .../endeffector/EndEffectorConstants.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 1 - 9 files changed, 28 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 5cd28bc..6a8a1ec 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -119,9 +119,9 @@ private void bindControllerCommands() { OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); // OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ClimberCommands.getDebuggingCommand()); - OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.toggleOnTrue(ClimberCommands.resetClimberPositionCommand().ignoringDisable(true)); - OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.toggleOnTrue(IntakeCommands.resetIntakePositionCommand().ignoringDisable(true)); - OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.toggleOnTrue(ArmElevatorCommands.resetElevatorPositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.whileTrue(ClimberCommands.resetClimberPositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.whileTrue(IntakeCommands.resetIntakePositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.whileTrue(ArmElevatorCommands.resetElevatorPositionCommand().ignoringDisable(true)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index a053f7d..9c2a858 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -15,7 +15,7 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; public class ClimbCommands { - private static boolean IS_CLIMBING = false;//TODO: Make score triggers not work while climbing + private static boolean IS_CLIMBING = false; public static Command getClimbCommand() { return new SequentialCommandGroup( @@ -38,7 +38,7 @@ public static boolean isClimbing() { private static Command getAdjustClimbManuallyCommand() { return new ParallelCommandGroup( - ClimberCommands.getSetTargetSpeedCommand(OperatorConstants.DRIVER_CONTROLLER::getRightY), + ClimberCommands.getSetTargetSpeedCommand(() -> OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, @@ -50,7 +50,7 @@ private static Command getAdjustClimbManuallyCommand() { private static Command getSetSubsystemsToRestForClimbCommand() { return new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_FOR_CLIMB), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST_FOR_CLIMB) + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST) ); } } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 70b885a..edbf35a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -20,7 +20,7 @@ public class CoralCollectionCommands { public static boolean SHOULD_LOAD_CORAL = true; - public static boolean SHOULD_USE_INTAKE_ASSIST = true; + public static boolean SHOULD_USE_INTAKE_ASSIST = false; public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 3470692..d787ca3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -36,7 +36,7 @@ public static Command getToggleShouldLoadCoralCommand() { return new InstantCommand(() -> { CoralCollectionCommands.SHOULD_LOAD_CORAL = !CoralCollectionCommands.SHOULD_LOAD_CORAL; } - ); + ).ignoringDisable(true); } public static Command getToggleShouldManipulateCoralAutonomouslyCommand() { @@ -48,21 +48,21 @@ public static Command getToggleShouldManipulateCoralAutonomouslyCommand() { CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = true; CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true; } - }); + }).ignoringDisable(true); } public static Command getToggleShouldCollectCoralAtonomouslyCommand() { return new InstantCommand(() -> { CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = !CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST; } - ); + ).ignoringDisable(true); } public static Command getToggleShouldScoreCoralAtonomouslyCommand() { return new InstantCommand(() -> { CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = !CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY; } - ); + ).ignoringDisable(true); } public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 8771ef5..a25e6f9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -40,13 +40,15 @@ public static Command getArmGearRatioCalulationCommand() { public static Command resetElevatorPositionCommand() { return new SequentialCommandGroup( new ExecuteEndCommand( - () -> RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.kCCW_90deg, false), - RobotContainer.ARM_ELEVATOR::stop), + () -> RobotContainer.ARM_ELEVATOR.setTargetState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR), + RobotContainer.ARM_ELEVATOR::stop, + RobotContainer.ARM_ELEVATOR + ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), - RobotContainer.ARM_ELEVATOR::resetElevatorPosition, + () -> {}, RobotContainer.ARM_ELEVATOR - )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition); } public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 8f13c4d..fdfc790 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -54,8 +54,8 @@ public class ArmElevatorConstants { SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double - ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2.5, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 4, + ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 3.5, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 21, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 55; @@ -186,11 +186,11 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 50; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 60; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.04; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.5; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.6; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358; @@ -330,7 +330,9 @@ public enum ArmElevatorState { REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), - REST_FOR_CLIMB(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + PREPARE_FOR_REST_FOR_CLIMB(Rotation2d.fromDegrees(-29), 0.6, null, false, 0.7), + REST_FOR_CLIMB(Rotation2d.fromDegrees(-29), 0, PREPARE_FOR_REST_FOR_CLIMB, true, 0.7), + ZERO_ELEVATOR(Rotation2d.fromDegrees(90), 0.65, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.485, null, true, 0.7), UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), 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 cbaecad..7a6427b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -139,9 +139,9 @@ private static void configureServos() { public enum ClimberState { REST(0, 0, false), - BREAK_ZIP_TIE(-0.32, 0, false), + BREAK_ZIP_TIE(-0.35, 0, false), PREPARE_FOR_CLIMB(0, 1, false), - CLIMB(-1.6, 0, true); + CLIMB(-1.3, 0, true); public final double targetPositionRotations; public final double targetServoPower; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 600b68d..9d2d5b1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -98,7 +98,7 @@ public enum EndEffectorState { COLLECT_ALGAE(-11), SCORE_ALGAE(10), HOLD_CORAL(-0.5), - HOLD_ALGAE(-6); + HOLD_ALGAE(-7.5); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index c685d93..a1a263c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -197,7 +197,6 @@ private static void configureDistanceSensor() { public enum IntakeState { REST(0, MAXIMUM_ANGLE), OPEN_REST(0, MINIMUM_ANGLE), - REST_FOR_CLIMB(0, MINIMUM_ANGLE), COLLECT(-8, MINIMUM_ANGLE), EJECT(5, MAXIMUM_ANGLE); From 2c0ce29070b112cd7bf1a43c9d352411e9ad4c14 Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 04:21:14 -0400 Subject: [PATCH 45/53] fix arm falling asleep --- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 2 +- .../frc/trigon/robot/subsystems/transporter/Transporter.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 483eda7..a1fe245 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -116,7 +116,7 @@ private static Command getScoreInNetCommand() { ); } - private static Command getAtonomouslyScoreInNetCommand() { + private static Command getAutonomouslyScoreInNetCommand() { return new ParallelRaceGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index edbf35a..7fb8f1e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -41,7 +41,7 @@ public static Command getLoadCoralCommand() { new ParallelCommandGroup( getIntakeCoralCommand(), ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST) - ).until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)).unless(RobotContainer.END_EFFECTOR::hasGamePiece), + ).until(RobotContainer.TRANSPORTER::hasCoral).unless(RobotContainer.TRANSPORTER::hasCoral).unless(RobotContainer.END_EFFECTOR::hasGamePiece).finallyDo(() -> {RobotContainer.TRANSPORTER.stop(); RobotContainer.INTAKE.setTargetState(IntakeConstants.IntakeState.REST);}), ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 9fb3af4..ad5fca7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -113,7 +113,7 @@ public void resetIntakePosition() { angleMotor.setPosition(0); } - void setTargetState(IntakeConstants.IntakeState targetState) { + public void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index 8c2f7de..2d6c2b9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -47,7 +47,7 @@ public boolean hasCoral() { return TransporterConstants.HAS_CORAL_BOOLEAN_EVENT.getAsBoolean(); } - void setTargetState(TransporterConstants.TransporterState targetState) { + public void setTargetState(TransporterConstants.TransporterState targetState) { this.targetState = targetState; setTargetState(targetState.targetRightMotorVoltage, targetState.targetLeftMotorVoltage); } From ff18479106aa1b83ac9533cfa752c6de61ff3ba3 Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 04:24:36 -0400 Subject: [PATCH 46/53] fix algae position --- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index fdfc790..a756146 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -340,7 +340,7 @@ public enum ArmElevatorState { SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), - SCORE_NET(Rotation2d.fromDegrees(70), 1.644, null, false, 0.3), + SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3), SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.2, null, false, 1), From fec51635abf0bcd9375960febfba114c9ea5f6ca Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 8 Oct 2025 06:08:00 -0400 Subject: [PATCH 47/53] dang bro that's 11 files changed. not what you want to see after first qual :hurtrealbad: --- .../commandclasses/IntakeAssistCommand.java | 4 ++-- .../AlgaeManipulationCommands.java | 4 ++-- .../commandfactories/CoralPlacingCommands.java | 4 ++-- .../robot/constants/AutonomousConstants.java | 4 ++-- .../subsystems/armelevator/ArmElevator.java | 16 ++++++++++++++-- .../armelevator/ArmElevatorCommands.java | 2 +- .../subsystems/climber/ClimberCommands.java | 2 +- .../subsystems/endeffector/EndEffector.java | 11 ++++++++++- .../endeffector/EndEffectorConstants.java | 2 +- .../robot/subsystems/intake/IntakeCommands.java | 2 +- .../robot/subsystems/swerve/SwerveConstants.java | 2 +- 11 files changed, 37 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index b7c9859..20dc6ca 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -23,10 +23,10 @@ public class IntakeAssistCommand extends ParallelCommandGroup { static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.2, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), + new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index a1fe245..7550932 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), + getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.15))), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; @@ -64,7 +64,7 @@ public static Command getReefAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateReefAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), new InstantCommand(() -> IS_HOLDING_ALGAE = true), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index d23e307..dcfe1b2 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -153,11 +153,11 @@ public static boolean isPrepareArmAngleAboveCurrentArmAngle() { final Rotation2d targetAngle = targetState.prepareState == null ? targetState.targetAngle : targetState.prepareState.targetAngle; - return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle); + return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle); } private static boolean isReadyToScore(boolean shouldScoreRight) { - return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore()) + return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore(), 4) && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 118abb0..c3aae1c 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -27,8 +27,8 @@ public class AutonomousConstants { public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final double FEEDFORWARD_SCALAR = 0.6;//TODO: Calibrate + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index b8305d7..ae8a9d4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -143,13 +143,25 @@ public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolea && elevatorAtPosition(targetState.targetPositionMeters); } + public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed, double tol) { + if (targetState == null) + return false; + return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle, tol) + && elevatorAtPosition(targetState.targetPositionMeters); + } + public boolean armAtAngle(Rotation2d targetAngle) { final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees()); return currentToTargetAngleDifferenceDegrees < ArmElevatorConstants.ANGLE_TOLERANCE.getDegrees(); } + public boolean armAtAngle(Rotation2d targetAngle, double tol) { + final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees()); + return currentToTargetAngleDifferenceDegrees < tol; + } + public boolean armAboveAngle(Rotation2d targetAngle) { - return targetAngle.getDegrees() < getCurrentArmAngle().getDegrees(); + return targetAngle.getDegrees() - getCurrentArmAngle().getDegrees() < 4; } public boolean elevatorAtPosition(double positionMeters) { @@ -201,7 +213,7 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { scaleArmPositionRequestSpeed(targetState.speedScalar); - if (isStateReversed) { + if (isStateReversed && !(targetState == ArmElevatorConstants.ArmElevatorState.SCORE_L1 || targetState == ArmElevatorConstants.ArmElevatorState.PREPARE_SCORE_L1)) { setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints); return; } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index a25e6f9..3582ebd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -45,7 +45,7 @@ public static Command resetElevatorPositionCommand() { RobotContainer.ARM_ELEVATOR ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), new ExecuteEndCommand( - () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), () -> {}, RobotContainer.ARM_ELEVATOR )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition); diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 2519272..8c4e46e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -24,7 +24,7 @@ public static Command getDebuggingCommand() { public static Command resetClimberPositionCommand() { return new ExecuteEndCommand( - () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 3), + () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), RobotContainer.CLIMBER::resetClimberPosition, RobotContainer.CLIMBER ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index d1ff92b..71751dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.endeffector; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Translation3d; import frc.trigon.robot.RobotContainer; @@ -61,13 +62,21 @@ public boolean isEjecting() { } void setTargetState(EndEffectorConstants.EndEffectorState targetState) { - setEndEffectorTargetVoltage(targetState.targetVoltage); + if (targetState == EndEffectorConstants.EndEffectorState.HOLD_ALGAE) { + setCur(targetState.targetVoltage); + } else { + setEndEffectorTargetVoltage(targetState.targetVoltage); + } } void setTargetState(double targetVoltage) { setEndEffectorTargetVoltage(targetVoltage); } + private void setCur(double cur) { + endEffectorMotor.setControl(new TorqueCurrentFOC(cur).withMaxAbsDutyCycle(0.6)); + } + private void setEndEffectorTargetVoltage(double targetVoltage) { EndEffectorConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 9d2d5b1..53d5abd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -98,7 +98,7 @@ public enum EndEffectorState { COLLECT_ALGAE(-11), SCORE_ALGAE(10), HOLD_CORAL(-0.5), - HOLD_ALGAE(-7.5); + HOLD_ALGAE(-50); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index b89e08d..9171124 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -26,7 +26,7 @@ public static Command getDebuggingCommand() { public static Command resetIntakePositionCommand() { return new ExecuteEndCommand( - () -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightX() * 2), + () -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), RobotContainer.INTAKE::resetIntakePosition, RobotContainer.INTAKE ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 2ed60bb..e3833e8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -61,7 +61,7 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(6, 0, 0), + new PIDConstants(6.3, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : new PIDConstants(10, 0, 0.1); From 4be96a4dca1ab216b522c45455bec07b43eebdb2 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 9 Oct 2025 03:25:05 -0400 Subject: [PATCH 48/53] fix --- .../commandclasses/IntakeAssistCommand.java | 5 +++-- .../commandfactories/AutonomousCommands.java | 2 +- .../armelevator/ArmElevatorCommands.java | 7 +++++-- .../armelevator/ArmElevatorConstants.java | 16 ++++++++-------- .../subsystems/climber/ClimberCommands.java | 2 +- .../endeffector/EndEffectorConstants.java | 2 +- .../robot/subsystems/intake/IntakeCommands.java | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 20dc6ca..e1264b7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -23,13 +23,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup { static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.7, 0, 0, new TrapezoidProfile.Constraints(2.65, 4.5)), Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.8, 4.5)); private Translation2d distanceFromTrackedGamePiece; /** @@ -161,6 +161,7 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) { X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond); + X_PID_CONTROLLER.setGoal(-0.15); Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond); THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 6338613..96157c3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -51,7 +51,7 @@ public static Command getFindCoralCommand(boolean isRight) { SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, - () -> AutonomousConstants.AUTO_FIND_CORAL_ROTATION_POWER + () -> 0 ) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 3582ebd..a6267e5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -6,6 +6,8 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; @@ -45,10 +47,11 @@ public static Command resetElevatorPositionCommand() { RobotContainer.ARM_ELEVATOR ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), new ExecuteEndCommand( - () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), + () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), () -> {}, RobotContainer.ARM_ELEVATOR - )).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition); + ) + ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition); } public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index a756146..1da83be 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -48,8 +48,8 @@ public class ArmElevatorConstants { private static final double ARM_MOTOR_CURRENT_LIMIT = 50, ELEVATOR_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0.184814453125 - 0.25; - static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.184814453125 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0.347412109375 - 0.25; + static final double ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0.347412109375 - 0.25 - ANGLE_ENCODER_GRAVITY_OFFSET; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; @@ -186,11 +186,11 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 60; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 55; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 5.6; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 4.3; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358; @@ -252,7 +252,7 @@ private static void configureElevatorMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 12; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 17; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875; @@ -341,11 +341,11 @@ public enum ArmElevatorState { SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.6), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.2, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1), PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.3, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.13, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.16, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 8c4e46e..bc68eab 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -24,7 +24,7 @@ public static Command getDebuggingCommand() { public static Command resetClimberPositionCommand() { return new ExecuteEndCommand( - () -> RobotContainer.CLIMBER.setTargetVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), + () -> RobotContainer.CLIMBER.setTargetVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), RobotContainer.CLIMBER::resetClimberPosition, RobotContainer.CLIMBER ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 53d5abd..df134ce 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -50,7 +50,7 @@ public class EndEffectorConstants { private static final double DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; - private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 5; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 4; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 9171124..fb84bca 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -26,7 +26,7 @@ public static Command getDebuggingCommand() { public static Command resetIntakePositionCommand() { return new ExecuteEndCommand( - () -> RobotContainer.INTAKE.setAngleMotorVoltage(-OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), + () -> RobotContainer.INTAKE.setAngleMotorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), RobotContainer.INTAKE::resetIntakePosition, RobotContainer.INTAKE ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); From ab141c8c9eb851b273220c95c5d892c78aca3705 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 9 Oct 2025 05:20:14 -0400 Subject: [PATCH 49/53] COMMIT STUFF --- .../AlgaeManipulationCommands.java | 2 +- .../armelevator/ArmElevatorCommands.java | 14 ++++++++------ .../armelevator/ArmElevatorConstants.java | 10 +++++----- .../robot/subsystems/endeffector/EndEffector.java | 2 +- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 7550932..80ab0ad 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.15))), + getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index a6267e5..96f42cd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; @@ -40,18 +41,19 @@ public static Command getArmGearRatioCalulationCommand() { } public static Command resetElevatorPositionCommand() { - return new SequentialCommandGroup( + return new ParallelCommandGroup( new ExecuteEndCommand( - () -> RobotContainer.ARM_ELEVATOR.setTargetState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR), - RobotContainer.ARM_ELEVATOR::stop, - RobotContainer.ARM_ELEVATOR + () -> RobotContainer.ARM_ELEVATOR.setTargetArmState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR, false), + () -> {} ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), () -> {}, RobotContainer.ARM_ELEVATOR - ) - ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)).finallyDo(RobotContainer.ARM_ELEVATOR::resetElevatorPosition); + ), + SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT) + ).finallyDo(() -> {RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST);}); } public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 1da83be..6f548d4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -252,7 +252,7 @@ private static void configureElevatorMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 17; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 22; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875; @@ -339,13 +339,13 @@ public enum ArmElevatorState { SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), - SCORE_L4(Rotation2d.fromDegrees(-10), 1.5, PREPARE_SCORE_L4, false, 0.8), + SCORE_L4(Rotation2d.fromDegrees(-5), 1.5, PREPARE_SCORE_L4, false, 0.8), SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.6), + SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-30), 0.3, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.16, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.3, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.18, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index 71751dc..ee741d8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -61,7 +61,7 @@ public boolean isEjecting() { return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 1; } - void setTargetState(EndEffectorConstants.EndEffectorState targetState) { + public void setTargetState(EndEffectorConstants.EndEffectorState targetState) { if (targetState == EndEffectorConstants.EndEffectorState.HOLD_ALGAE) { setCur(targetState.targetVoltage); } else { From a876e7940d03fc3137babb3c670f4a7f398c2f9a Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Thu, 9 Oct 2025 12:41:04 +0300 Subject: [PATCH 50/53] fix --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 2 +- src/main/java/frc/trigon/robot/constants/FieldConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index dcfe1b2..970f144 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -43,7 +43,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( CoralCollectionCommands.getLoadCoralCommand(), - getPrepareArmElevatorIfWontHitReef(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 12308b7..55b7029 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -32,7 +32,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2; - public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.25; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.17; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); From fa8494fca42df92b475befa18d0f7918c2685893 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Thu, 9 Oct 2025 13:53:02 +0300 Subject: [PATCH 51/53] stupid zBook --- .../CoralPlacingCommands.java | 5 +++-- .../robot/constants/FieldConstants.java | 2 +- .../armelevator/ArmElevatorCommands.java | 22 ++++++++++++++++--- 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 970f144..6938960 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -10,13 +10,13 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableTranslation2d; -import org.littletonrobotics.junction.AutoLogOutput; public class CoralPlacingCommands { public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; @@ -79,8 +79,9 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh } private static Command getPrepareArmElevatorIfWontHitReef(boolean shouldScoreRight) { - return GeneralCommands.runWhen( + return GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore), + ArmElevatorCommands.getStayInPlaceCommand(), () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 55b7029..aa6b4af 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -32,7 +32,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2; - public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.17; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.15; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 96f42cd..e047d5a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -44,16 +44,21 @@ public static Command resetElevatorPositionCommand() { return new ParallelCommandGroup( new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setTargetArmState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR, false), - () -> {} + () -> { + } ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), new ExecuteEndCommand( () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), - () -> {}, + () -> { + }, RobotContainer.ARM_ELEVATOR ), SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT) - ).finallyDo(() -> {RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST);}); + ).finallyDo(() -> { + RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); + RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST); + }); } public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { @@ -77,6 +82,17 @@ public static Command getSetTargetStateCommand(Supplier { + RobotContainer.ARM_ELEVATOR.setTargetArmAngle(RobotContainer.ARM_ELEVATOR.getCurrentArmAngle(), true); + RobotContainer.ARM_ELEVATOR.setTargetElevatorPositionMeters(RobotContainer.ARM_ELEVATOR.getCurrentElevatorPositionMeters(), true); + }, + RobotContainer.ARM_ELEVATOR::stop, + RobotContainer.ARM_ELEVATOR + ); + } + public static Command getPrepareForStateCommand(Supplier targetState) { return getPrepareForStateCommand(targetState, () -> false); } From 7ebdf2a528b6f5ddf78b059a142965bef2d2e615 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 9 Oct 2025 08:21:45 -0400 Subject: [PATCH 52/53] dfretf --- .../commands/commandfactories/CoralPlacingCommands.java | 4 ++-- .../robot/subsystems/armelevator/ArmElevatorConstants.java | 6 +++--- .../robot/subsystems/transporter/TransporterConstants.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 970f144..74943f9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -25,7 +25,7 @@ public class CoralPlacingCommands { public static Command getScoreInReefCommand(boolean shouldScoreRight) { return new ConditionalCommand( getAutonomouslyScoreCommand(shouldScoreRight), - getScoreCommand(shouldScoreRight), + getScoreCommand(), () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ).onlyIf(CoralCollectionCommands::hasCoral); } @@ -40,7 +40,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { ); } - private static Command getScoreCommand(boolean shouldScoreRight) { + private static Command getScoreCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getLoadCoralCommand(), GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 6f548d4..187e2db 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -55,7 +55,7 @@ public class ArmElevatorConstants { SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; static final double ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 2, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 3.5, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 3.2, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 21, ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 55; @@ -344,8 +344,8 @@ public enum ArmElevatorState { SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.3, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.18, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.35, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.22, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 3a58174..261fbc3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -103,7 +103,7 @@ private static void configureBeamBreak() { public enum TransporterState { REST(0, 0), - COLLECT(-4, 5), + COLLECT(-3.5, 5), ALIGN_CORAL(-5, 6), HOLD_CORAL(-1, 1), EJECT(5, -5); From 1a5275c5ce9be6e26cf18d1173883905f107b630 Mon Sep 17 00:00:00 2001 From: Yishai Levy Date: Thu, 9 Oct 2025 15:23:18 +0300 Subject: [PATCH 53/53] Update FieldConstants.java --- src/main/java/frc/trigon/robot/constants/FieldConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index aa6b4af..55b7029 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -32,7 +32,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2; - public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.15; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.17; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout();