diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39ce2e87..0cf9fc47 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,6 +27,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.units.Unit; +import frc.robot.subsystems.arm.ArmPose; +import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.module.ModuleConstants; /** @@ -221,11 +223,16 @@ private ArmConstants() {} } public static class ArmSetpoints { - public record ArmSetpoint(double armPoseDegs, double wristPoseDegs) {} + private ArmSetpoints() { + throw new IllegalStateException("Static classes should not be constructed"); + } - public static final ArmSetpoint STOW_SETPOINT = new ArmSetpoint(0.0, 45.0); - public static final ArmSetpoint INTAKE_SETPOINT = new ArmSetpoint(0.0, 35.0); - public static final ArmSetpoint AMP_SETPOINT = new ArmSetpoint(90.0, 135.0); + public static final ArmPose STOW_SETPOINT = new + ArmPose("ArmPoses/Stow", false, 0.0, 45.0); + public static final ArmPose INTAKE_SETPOINT = + new ArmPose("ArmPoses/Intake", false, 0.0, 35.0); + public static final ArmPose AMP_SETPOINT = + new ArmPose("ArmPoses/Amp", false, 90.0, 135.0); } public static class ShooterConstants { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPose.java b/src/main/java/frc/robot/subsystems/arm/ArmPose.java new file mode 100644 index 00000000..eb515ea3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmPose.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.arm; + +import com.gos.lib.properties.GosDoubleProperty; + +public class ArmPose { + private final GosDoubleProperty m_wristAngleProperty; + private final GosDoubleProperty m_armAngleProperty; + + private final double m_armAngle; + private final double m_wristAngle; + + public ArmPose(String name, boolean isConstant, double armAngle, double wristAngle) { + m_armAngleProperty = new GosDoubleProperty(isConstant, name + "Arm Angle", armAngle); + m_wristAngleProperty = new GosDoubleProperty(isConstant, name + "Wrist Angle", wristAngle); + + m_armAngle = 0.0; + m_wristAngle = 0.0; + } + + public ArmPose(double armAngle, double wristAngle) { + m_armAngleProperty = null; + m_wristAngleProperty = null; + + m_armAngle = armAngle; + m_wristAngle = wristAngle; + } + + public double armAngle() { + if (m_armAngleProperty != null) { + return m_armAngleProperty.getValue(); + } + return m_armAngle; + } + + public double wristAngle() { + if (m_wristAngleProperty != null) { + return m_wristAngleProperty.getValue(); + } + + return m_wristAngle; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 2cf36c26..679c6367 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -77,16 +78,19 @@ public void periodic() { ArmConstants.ARM_UPPER_LIMIT.getValue()) : m_desiredArmPoseDegs; + // if we're disabled go back to hold pose + if (DriverStation.isDisabled()) { + m_desiredState = ArmState.DISABLED; + } + // check to make sure we're not in manual control - m_io.enableBrakeMode(m_desiredArmPoseDegs <= Double.NEGATIVE_INFINITY - || m_desiredWristPoseDegs <= Double.NEGATIVE_INFINITY); + m_io.enableBrakeMode(m_desiredState != ArmState.DISABLED); - if (m_desiredArmPoseDegs > Double.NEGATIVE_INFINITY) { + if (m_desiredState != ArmState.DISABLED) { + // set the arms angle m_io.setArmAngle(m_desiredArmPoseDegs); Logger.recordOutput("Arm/Arm Setpoint Degs", m_desiredArmPoseDegs); - } - if (m_desiredWristPoseDegs > Double.NEGATIVE_INFINITY) { // check to see if the wrist is currently too close to the rest of the arm double wristGap = m_inputs.wristPositionDegs + m_inputs.armPositionDegs; if (wristGap < ArmConstants.WRIST_ARM_GAP.getValue()) { @@ -109,23 +113,23 @@ public void periodic() { public void handleState() { switch(m_desiredState) { case STOW -> { - m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armPoseDegs(); - m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristPoseDegs(); + m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armAngle(); + m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristAngle(); } case AUTO_AIM -> { - ArmSetpoints.ArmSetpoint aimSetpoint = AimbotUtils.aimbotCalculate( + ArmPose aimSetpoint = AimbotUtils.aimbotCalculate( new Pose3d(m_poseSupplier.get()), m_inputs.armPositionDegs); - m_desiredArmPoseDegs = aimSetpoint.armPoseDegs(); - m_desiredWristPoseDegs = aimSetpoint.wristPoseDegs(); + m_desiredArmPoseDegs = aimSetpoint.armAngle(); + m_desiredWristPoseDegs = aimSetpoint.wristAngle(); } case INTAKE -> { - m_desiredArmPoseDegs = ArmSetpoints.INTAKE_SETPOINT.armPoseDegs(); - m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristPoseDegs(); + m_desiredArmPoseDegs = ArmSetpoints.INTAKE_SETPOINT.armAngle(); + m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristAngle(); } case AMP -> { - m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armPoseDegs(); - m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristPoseDegs(); + m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle(); + m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } default -> { m_desiredArmPoseDegs = m_inputs.armPositionDegs; @@ -134,34 +138,11 @@ public void handleState() { } } - - /* Command Factories */ public Command setDesiredState(ArmState state) { - return runOnce(() -> m_desiredState = state); - } - - public Command setArmDesiredPose(double armPose, double wristPose) { - return run(() -> { - m_desiredArmPoseDegs = armPose; - - double estWristGap = armPose + wristPose; - if (estWristGap < ArmConstants.WRIST_ARM_GAP.getValue()) { - double underGap = ArmConstants.WRIST_ARM_GAP.getValue() - estWristGap; - m_desiredWristPoseDegs = wristPose + underGap; - } else { - m_desiredWristPoseDegs = wristPose; - } - }); - } - - public Command setArmPositionFactory(double degrees) { - return run(() -> m_desiredArmPoseDegs = degrees); - } - - public Command setWristPositionFactory(double degrees) { - return run(() -> m_desiredWristPoseDegs = degrees); + return runEnd(() -> m_desiredState = state, + () -> m_desiredState = ArmState.STOW); } public Command enableBrakeMode(boolean enabled) { diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 2190a2fa..894965e3 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.ArmSetpoints; +import frc.robot.subsystems.arm.ArmPose; import org.littletonrobotics.junction.Logger; public class AimbotUtils { @@ -30,7 +31,7 @@ public static Transform3d getShooterTransformation(double armAngle) { ); } - public static ArmSetpoints.ArmSetpoint aimbotCalculate(Pose3d robotPose, double armAngle) { + public static ArmPose aimbotCalculate(Pose3d robotPose, double armAngle) { // the position to target Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); @@ -50,6 +51,6 @@ public static ArmSetpoints.ArmSetpoint aimbotCalculate(Pose3d robotPose, double double desiredWristAngle = Units.radiansToDegrees(Math.atan(robotToSpeaker.getZ()/groundDistance)); Logger.recordOutput("Arm/ Wrist Aimbot Raw", desiredWristAngle); double safeArmAngle = ArmConstants.WRIST_ARM_GAP.getValue() - desiredWristAngle; - return new ArmSetpoints.ArmSetpoint(safeArmAngle, desiredWristAngle); + return new ArmPose(safeArmAngle, desiredWristAngle); } }