diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 1b14eb1a..2e768d9f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -2,10 +2,12 @@ import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; 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.RobotContainer; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -81,31 +83,29 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean isArmAboveSafeAngle() { + return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); + } + public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) - return this.targetState == targetState && atTargetAngle(isStateReversed); + return this.targetState == targetState && atAngle(subtractFrom360Degrees(targetState.targetAngle)); return atState(targetState); } public boolean atState(ArmConstants.ArmState targetState) { - return this.targetState == targetState && atTargetAngle(); + return this.targetState == targetState && atAngle(targetState.targetAngle); } - public boolean atTargetAngle(boolean isStateReversed) { + public boolean atTargetAngle() { if (isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees()); - return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return atAngle(subtractFrom360Degrees(targetState.targetAngle)); } - return atTargetAngle(); - } - - public boolean atTargetAngle() { - final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.targetAngle.minus(getAngle()).getDegrees()); - return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return atAngle(targetState.targetAngle); } - public boolean hasGamePiece() { - return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); + public Rotation2d getAngle() { + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } void setTargetState(ArmConstants.ArmState targetState) { @@ -120,7 +120,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) if (isStateReversed) { setTargetState( - ArmConstants.FULL_ROTATION.minus(targetState.targetAngle) + subtractFrom360Degrees(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; @@ -132,40 +132,58 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetAngle(targetAngle); - setTargetVoltage(targetVoltage); + setEndEffectorTargetVoltage(targetVoltage); } - void prepareForState(ArmConstants.ArmState targetState) { - prepareForState(targetState, false); + void setArmState(ArmConstants.ArmState targetState) { + setArmState(targetState, false); } - void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { + void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) { this.isStateReversed = isStateReversed; this.targetState = targetState; - + if (isStateReversed) { - setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); + setTargetAngle(subtractFrom360Degrees(targetState.targetAngle)); return; } setTargetAngle(targetState.targetAngle); } - private Rotation2d getAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + void setEndEffectorState(ArmConstants.ArmState targetState) { + setEndEffectorTargetVoltage(targetState.targetEndEffectorVoltage); } private void setTargetAngle(Rotation2d targetAngle) { - armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); + armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); + } + + private Rotation2d calculateMinimumArmSafeAngle() { + final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); + final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); + final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); + return isElevatorAboveSafeZone + ? Rotation2d.fromRadians(0) + : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); } - private void setTargetVoltage(double targetVoltage) { + private void setEndEffectorTargetVoltage(double targetVoltage) { ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + private boolean atAngle(Rotation2d targetAngle) { + final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees()); + return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + } + + private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { + return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); + } + private Pose3d calculateVisualizationPose() { final Transform3d armTransform = new Transform3d( - new Translation3d(), + new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()), new Rotation3d(0, getAngle().getRadians(), 0) ); return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index c7bcd58c..7b96ab63 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -2,8 +2,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import frc.trigon.robot.RobotContainer; +import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; import lib.commands.NetworkTablesCommand; @@ -32,33 +33,27 @@ public static Command getGearRatioCalulationCommand() { ); } - public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { - return new StartEndCommand( - () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed), - RobotContainer.ARM::stop, - RobotContainer.ARM - ); - } - public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { - return new StartEndCommand( - () -> RobotContainer.ARM.setTargetState(targetState), - RobotContainer.ARM::stop, - RobotContainer.ARM - ); + return getSetTargetStateCommand(targetState, false); } - public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { - return new StartEndCommand( - () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed), - RobotContainer.ARM::stop, + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState), + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), + interrupted -> RobotContainer.ARM.stop(), + () -> false, RobotContainer.ARM ); } public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { - return new StartEndCommand( - () -> RobotContainer.ARM.prepareForState(targetState), + return getPrepareForStateCommand(targetState, false); + } + + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + return new ExecuteEndCommand( + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), RobotContainer.ARM::stop, RobotContainer.ARM ); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 0bdb08ef..ab2c8de8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -50,18 +50,19 @@ public class ArmConstants { static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); private static final double - ARM_GEAR_RATIO = 50, + ARM_GEAR_RATIO = 40, END_EFFECTOR_GEAR_RATIO = 17; private static final double ARM_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; private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false; static final double - ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0.6 : 0, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 1.5 : 0, + ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10; static final boolean FOC_ENABLED = true; + public static final double ARM_LENGTH_METERS = 0.52; private static final int ARM_MOTOR_AMOUNT = 2, END_EFFECTOR_MOTOR_AMOUNT = 1; @@ -69,7 +70,6 @@ public class ArmConstants { ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT), END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT); private static final double - ARM_LENGTH_METERS = 0.67, ARM_MASS_KILOGRAMS = 3.5, END_EFFECTOR_MOMENT_OF_INERTIA = 0.003, END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12; @@ -95,7 +95,7 @@ public class ArmConstants { static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), - Units.Volts.of(1.5), + Units.Volts.of(3), Units.Second.of(1000) ); @@ -110,17 +110,19 @@ public class ArmConstants { ); static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0.0954, 0.9517), + new Translation3d(0, -0.0954, 0.3573), new Rotation3d(0, 0, 0) ); - + /** + * 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 ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); private static final double 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); - static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360); static { configureArmMasterMotor(); @@ -143,13 +145,13 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 38 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0067258 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.063357 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.15048 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; + 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.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; @@ -158,7 +160,8 @@ private static void configureArmMasterMotor() { config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; ARM_MASTER_MOTOR.applyConfiguration(config); ARM_MASTER_MOTOR.setPhysicsSimulation(ARM_SIMULATION); @@ -179,7 +182,8 @@ private static void configureArmFollowerMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; ARM_FOLLOWER_MOTOR.applyConfiguration(config); @@ -196,7 +200,8 @@ private static void configureEndEffectorMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; - config.CurrentLimits.SupplyCurrentLimit = 80; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = 80; END_EFFECTOR_MOTOR.applyConfiguration(config); END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION); @@ -234,7 +239,7 @@ public enum ArmState { PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), - SCORE_L2(Rotation2d.fromDegrees(90), 4), + SCORE_L2(Rotation2d.fromDegrees(180), 4), PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0), SCORE_L3(Rotation2d.fromDegrees(90), 4), PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 7063a1d5..bdbed2b9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -9,11 +9,13 @@ 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.RobotContainer; import frc.trigon.robot.subsystems.MotorSubsystem; -import org.littletonrobotics.junction.Logger; +import frc.trigon.robot.subsystems.arm.ArmConstants; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.utilities.Conversions; +import org.littletonrobotics.junction.Logger; public class Elevator extends MotorSubsystem { private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR; @@ -23,7 +25,7 @@ public class Elevator extends MotorSubsystem { ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10 - ).withEnableFOC(ElevatorConstants.FOC_ENABLED); + ).withEnableFOC(ElevatorConstants.FOC_ENABLED); private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST; public Elevator() { @@ -75,14 +77,40 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } + public double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + + public boolean isElevatorAboveSafeZone() { + return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; + } + + public double getElevatorHeightFromMinimumSafeZone() { + return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + } + void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); + if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { + masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); + return; + } setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); } void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); + } + + private double calculateMinimumSafeElevatorHeightRotations() { + final double armCos = RobotContainer.ARM.getAngle().getRadians(); + final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; + final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() + ? 0 : elevatorHeightFromArm) + + minimumElevatorSafeZone; + return metersToRotations(minimumSafeHeightMeters); } private Pose3d getFirstStageComponentPose() { @@ -117,10 +145,6 @@ private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Jerk = positionRequest.Acceleration * 10; } - private double getPositionMeters() { - return rotationsToMeters(getPositionRotations()); - } - private double rotationsToMeters(double positionsRotations) { return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index aba71757..0957aa70 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.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.subsystems.arm.ArmConstants; +import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; import java.util.Set; @@ -18,15 +20,15 @@ public static Command getDebbugingCommand() { } public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { - return new StartEndCommand( + return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetState(targetState), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ); } - + public static Command getSetTargetStateCommand(double targetPositionRotations) { - return new StartEndCommand( + return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b56f6d03..05758a80 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -2,7 +2,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; @@ -12,6 +15,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.subsystems.arm.ArmConstants; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -71,7 +75,7 @@ public class ElevatorConstants { new Rotation3d(0, 0, 0) ); static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17 ,0.0814), + new Translation3d(0, -0.17, 0.0814), new Rotation3d(0, 0, 0) ); static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( @@ -88,6 +92,15 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + /** + * The lowest point in the Elevators zone where the safety logic applies. + */ + public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; + + /** + * The highest point in the Elevators zone where the safety logic applies. + */ + public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static { @@ -106,13 +119,13 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5:0; - config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; @@ -159,8 +172,9 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.7), - SCORE_L1(0.603, 1), - SCORE_L2(0.603, 1), + LOAD_CORAL(0.5519, 0.7), + SCORE_L1(0.203, 1), + SCORE_L2(0.203, 1), SCORE_L3(1.003, 1), SCORE_L4(1.382, 1), PREPARE_L1(0.603, 1), 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 560d9c9f..eb47449c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -93,7 +93,7 @@ public class IntakeConstants { ); static final Pose3d INTAKE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0.29449, 0.32349), + new Translation3d(0.3234, 0, 0.2944), new Rotation3d(0, MINIMUM_ANGLE.getRadians(), 0) ); private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12;