diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 66e9947..63b4a8b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -435,11 +435,23 @@ private void configureBindings() { // rightDriveController.getRightThumb().whileTrue(intakeSubsystem.openAndEject()); CORAL.and(rightDriveController.getBottomThumb()) - .whileTrue(gripperSubsystem.intakeSpinCoral().until(gripperSubsystem.HAS_PIECE)); + .whileTrue( + gripperSubsystem + .intakeSpinCoral() + .withDeadline( + Commands.waitSeconds(0.2) + .andThen( + Commands.waitUntil( + gripperSubsystem.HAS_PIECE)))); CORAL.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinCoral()); ALGAE.and(rightDriveController.getBottomThumb()).onTrue(gripperSubsystem.intakeSpinAlgae()); - ALGAE.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinAlgae()); + ALGAE.and(rightDriveController.getTrigger()) + .and(stateManager.PROCESSOR) + .whileTrue(gripperSubsystem.slowEjectSpinAlgae()); + ALGAE.and(rightDriveController.getTrigger()) + .and(stateManager.PROCESSOR.negate()) + .whileTrue(gripperSubsystem.ejectSpinAlgae()); leftDriveController .getTrigger() diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 591827c..f4ee38d 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -19,9 +19,9 @@ public class ElevatorConstants { ; public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() - .withMotionMagicAcceleration(4) // these are guesses, come back here - .withMotionMagicCruiseVelocity(4) // also guess - .withMotionMagicJerk(4); + .withMotionMagicAcceleration(190 / .3) // these are guesses, come back here + .withMotionMagicCruiseVelocity(190) // also guess + .withMotionMagicJerk(0); public static final int elevatorLeaderId = 9; diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java index 760d665..ac79114 100644 --- a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java +++ b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java @@ -36,7 +36,7 @@ public class SuperstructureStateManager extends SubsystemBase { public static class SuperstructureState { private static LoggedNetworkNumber elevatorHeightLogged = - new LoggedNetworkNumber("Elevator Height", 165); + new LoggedNetworkNumber("Elevator Height", 170); private static LoggedNetworkNumber armHeightLogged = new LoggedNetworkNumber("Arm Height", 0); private static LoggedNetworkNumber wristRotationLogged = @@ -111,42 +111,47 @@ default boolean checksOut(Position position, SuperstructureStateManager stateMan public static enum Position { Sussy(1, 1, 1, null), CenterZoneNull(1, 1, 1, null, TRUE, FALSE), - Home(165, 0, 1.58, CenterZoneNull), + Home(170, 0, 1.58, CenterZoneNull), ChuteDownPre( - 165, + 170, 0, 1.58, CenterZoneNull, (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean() || DEFAULT.checksOut(a, s, e), (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), ChuteDown( - 165, + 170, 0, 1.58, ChuteDownPre, (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean(), (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), ChuteDownNull(0, 0, 0, ChuteDown, TRUE, FALSE), + HandoffPrep(170, -0.5, 1.58, ChuteDownNull), + Handoff(132, -0.5, 1.58, HandoffPrep), + PointUp(170, 2.15, 1.58, CenterZoneNull), + // (a, s, e) -> { + // boolean armChecksOut = s.armSubsystem.getPosition() > 1.0; + // boolean wristAtPosition = Math.abs(s.wristSubsystem.getFlippedPosition() - + // a.wristRotation()) < 0.1; + // return armChecksOut && wristAtPosition; + // }, TRUE), + UpZoneNull(0, 0, 0, PointUp, TRUE, FALSE), ChuteUpPre( - 165, - 0, - 1.58, - CenterZoneNull, + 170, + 2.15, + -1.58, + UpZoneNull, (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean() || DEFAULT.checksOut(a, s, e), (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), ChuteUp( - 165, - 0, - 1.58, + 170, + 2.15, + -1.58, ChuteUpPre, (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean(), (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), ChuteUpNull(0, 0, 0, ChuteUp, TRUE, FALSE), - HandoffPrep(165, -0.5, 1.58, ChuteDownNull), - Handoff(132, -0.5, 1.58, HandoffPrep), - PointUp(165, 2.15, 1.58, CenterZoneNull), - AlgaePointUp(165, 2.15, -1.58, ChuteUpNull), - UpZoneNull(0, 0, 0, PointUp, TRUE, FALSE), // L1Prep(297, 2.15, -1.58, ChuteUpNull), L1(130, 0.9, 0, UpZoneNull), L2Prep(120, 2.15, -1.58, UpZoneNull), @@ -164,18 +169,20 @@ public static enum Position { // NetAlgaePrep(297, 2.15, -1.58, UpZoneNull), // NetAlgae(297, 2.15, -1.58, NetAlgaePrep), Source(130, 2.15, 0, PointUp), - AlgaeHome(100, 2.15, -1.58, AlgaePointUp), + AlgaeHome(100, 2.15, -1.58, ChuteUpNull), AlgaeHomeNull(0.0, 0.0, 0.0, AlgaeHome, TRUE, FALSE), - Climb(165, 0, 1.58, ChuteUpNull), + // Climb(170, 0, 1.58, ChuteUpNull), Processor(65, 1.58, -1.58, AlgaeHomeNull), - // IcecreamCoral(165, 0, 1.58, AlgaeHome), - // IcecreamAlgae(165, 0, 1.58, AlgaeHome), + // IcecreamCoral(170, 0, 1.58, AlgaeHome), + // IcecreamAlgae(170, 0, 1.58, AlgaeHome), GroundAlgaePrep(65, 1.58, 1.58, AlgaeHomeNull), GroundAlgae(20, 1.58, 1.58, GroundAlgaePrep), - StartPrep(140, 0, -1.58, ChuteUpNull), + StartPrepPrepPrep(170, 2.15, 1.58, ChuteUpNull), + StartPrepPrep(140, 0, 1.58, StartPrepPrepPrep), + StartPrep(140, 0, -1.58, StartPrepPrep), Start(0, 0, -1.58, StartPrep), - Tunable(165, 0, 1.58, CenterZoneNull, FALSE); + Tunable(170, 0, 1.58, CenterZoneNull, FALSE); private double elevatorHeight; private double armHeight; @@ -270,6 +277,8 @@ public boolean isAtTargetAllegedly() { private SuperstructureState.Position targetPostition = SuperstructureState.Position.Start; private SuperstructureState.Position lastPosition = SuperstructureState.Position.Start; + private SuperstructureState.Position trueFinalTarget = SuperstructureState.Position.Start; + private List outList = new ArrayList<>(); private enum CoralAlgaeMode { @@ -301,6 +310,8 @@ public Pose2d getLastScoringPose() { public final Trigger ALGAE = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.Algae); public final Trigger ARMWRIST = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.ArmWrist); + public final Trigger PROCESSOR = new Trigger(() -> trueFinalTarget == Position.Processor); + public Command setLeftCoralMode() { return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.LeftCoral); } @@ -344,6 +355,8 @@ public void periodic() { } private void setFinalTarget(SuperstructureState.Position myPosition) { + trueFinalTarget = myPosition; + outList.clear(); // Set the [ (A') => (C') ] target of the system (initialize pathing command) boolean found = false; @@ -405,7 +418,9 @@ private Command internalGoToPosition(SuperstructureState.Position myPosition) { if (myPosition == Position.ChuteUpNull) { chuteCanMove = false; } - if (lastPosition.parent() == Position.CenterZoneNull && lastPosition.isRealPosition(this)) { + if ((lastPosition.parent() == Position.CenterZoneNull && lastPosition.isRealPosition(this)) + || (lastPosition.parent() == Position.UpZoneNull + && lastPosition.isRealPosition(this))) { chuteCanMove = true; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java index dc835be..61387f9 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java @@ -5,7 +5,7 @@ public class ArmPivotIOSim implements ArmPivotIO { private double voltage = 0; public void updateInputs(ArmPivotIOInputs inputs) { - position += 0.02 * voltage; + position += 0.02 * voltage * 0.5; inputs.position = position; inputs.throughboreEncoderPosition = position; diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java b/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java index 9ce9cb6..3096a95 100644 --- a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java +++ b/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java @@ -7,7 +7,7 @@ public class ChuteIOSim implements ChuteIO { private double voltage = 0; public void updateInputs(ChuteIOInputs inputs) { - position += 0.02 * voltage * 20; + position += 0.02 * (voltage + 0.5) * 20; inputs.voltage = voltage; diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java b/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java index 4fe35df..5e5145e 100644 --- a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java +++ b/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java @@ -92,7 +92,7 @@ public Command moveChuteDown() { setVoltage(1.5) .until(STALLING) .andThen(setDown()) - .andThen(setVoltage(0.5)))) + .andThen(setVoltage(0.0)))) .beforeStarting( () -> { isUp = false; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index 0299077..4b87790 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -3,8 +3,8 @@ import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; // import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.ElevatorConstants; @@ -17,7 +17,7 @@ public class ElevatorIOTalonFX implements ElevatorIO { // new TalonFX( // ElevatorConstants.elevatorFollowerId, // ElevatorConstants.elevatorFollowerCanbus); - private final PositionVoltage motionMagicVoltage = new PositionVoltage(0); + private final MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0); public ElevatorIOTalonFX() { elevatorLeader.setPosition(0); diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java index 3f52984..b3c2208 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java @@ -54,6 +54,10 @@ public Command ejectSpinAlgae() { return setVoltage(-12); } + public Command slowEjectSpinAlgae() { + return setVoltage(-3); + } + public Command setVoltage(double voltage) { return run( () -> { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java index 6ade56d..0e0507c 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.java @@ -17,4 +17,6 @@ public class WristIOInputs { } public void setVoltage(double voltage); + + public void setPositionControl(double reference); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java b/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java index 150a9f6..7a6b740 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.wrist; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel; @@ -18,27 +19,44 @@ public class WristIONeo550 implements WristIO { new DutyCycleEncoder(WristConstants.WRIST_THROUGHBORE_ENCODER_ID, 2 * Math.PI, 0); public WristIONeo550() { - wristMotor.getEncoder().setPosition(0); - SparkBaseConfig config = new SparkMaxConfig() .smartCurrentLimit((int) WristConstants.WristCurrent) .secondaryCurrentLimit(WristConstants.WristCurrent) .idleMode(IdleMode.kBrake); + + config.encoder.positionConversionFactor(Math.PI / 76.1); + + config.closedLoop + .p(WristConstants.WRIST_KP / 12) + .i(WristConstants.WRIST_KI / 12) + .d(WristConstants.WRIST_KD / 12); + + config.softLimit + .forwardSoftLimit(WristConstants.upperLimit) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(WristConstants.lowerLimit) + .reverseSoftLimitEnabled(true); + wristMotor.configure( config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + + wristMotor.getEncoder().setPosition(-Math.PI / 2); } private boolean shutdown = false; private double lastVoltage = 0; + private boolean usingVoltage = true; + public void updateInputs(WristIOInputs inputs) { - inputs.position = wristMotor.getEncoder().getPosition(); inputs.voltage = wristMotor.getBusVoltage() * wristMotor.getAppliedOutput(); inputs.current = wristMotor.getOutputCurrent(); inputs.temperature = wristMotor.getMotorTemperature(); inputs.throughboreEncoderPosition = throughboreEncoder.get() - 4.22; + wristMotor.getEncoder().setPosition(inputs.throughboreEncoderPosition); + inputs.position = wristMotor.getEncoder().getPosition(); inputs.throughboreConnected = throughboreEncoder.isConnected(); if (inputs.temperature > 60) { @@ -49,23 +67,37 @@ public void updateInputs(WristIOInputs inputs) { inputs.shutdown = shutdown; - if (inputs.throughboreEncoderPosition >= WristConstants.upperLimit && lastVoltage > 0) { + if (inputs.throughboreEncoderPosition >= WristConstants.upperLimit + && (inputs.voltage > 0 || lastVoltage > 0)) { lastVoltage = 0; + usingVoltage = true; } - if (inputs.throughboreEncoderPosition <= WristConstants.lowerLimit && lastVoltage < 0) { + if (inputs.throughboreEncoderPosition <= WristConstants.lowerLimit + && (inputs.voltage < 0 || lastVoltage < 0)) { lastVoltage = 0; + usingVoltage = true; } if (!inputs.throughboreConnected) { lastVoltage = 0; + usingVoltage = true; } if (shutdown) { lastVoltage = 0; + usingVoltage = true; } - wristMotor.setVoltage(lastVoltage); + if (usingVoltage) { + wristMotor.setVoltage(lastVoltage); + } } public void setVoltage(double voltage) { lastVoltage = voltage; + usingVoltage = true; + } + + public void setPositionControl(double reference) { + wristMotor.getClosedLoopController().setReference(reference, ControlType.kPosition); + usingVoltage = false; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java index d9df398..ced810d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java @@ -1,10 +1,24 @@ package frc.robot.subsystems.wrist; +import edu.wpi.first.math.controller.PIDController; +import frc.robot.constants.WristConstants; + public class WristIOSim implements WristIO { private double position = 0; private double voltage = 0; + private double reference = 0; + private boolean usingPosition = false; + + private PIDController controller = + new PIDController( + WristConstants.WRIST_KP, WristConstants.WRIST_KI, WristConstants.WRIST_KD); + public void updateInputs(WristIOInputs inputs) { + if (usingPosition) { + voltage = controller.calculate(position, reference); + } + position += 0.02 * voltage; inputs.position = position; @@ -14,5 +28,11 @@ public void updateInputs(WristIOInputs inputs) { public void setVoltage(double voltage) { this.voltage = voltage; + usingPosition = false; + } + + public void setPositionControl(double reference) { + this.reference = reference; + usingPosition = true; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a4674e0..b1856a6 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -88,10 +88,11 @@ private Command followReferenceThrubore() { isWristFlipped ? -reference : reference); if (controller.atSetpoint()) { voltage = 0; + setVoltageSave(voltage); } else { - voltage = Math.min(12.0, Math.max(-12.0, voltage)); // Clamp voltage + voltage = Math.min(12.0, Math.max(-12.0, voltage)); + setPositionSave(isWristFlipped ? -reference : reference); // Clamp voltage } - setVoltageSave(voltage); }); } @@ -124,6 +125,19 @@ private boolean isSafeToMove() { private void setVoltageSave(double voltage) { if (isSafeToMove()) { wristIO.setVoltage(voltage); + } else { + wristIO.setVoltage(0); + } + // if ( + // wristIO.setVoltage(voltage); + // ) + } + + private void setPositionSave(double referenceInternal) { + if (isSafeToMove()) { + wristIO.setPositionControl(referenceInternal); + } else { + wristIO.setVoltage(0); } // if ( // wristIO.setVoltage(voltage);