Arm speedup setpoints (#211)

* Made some changes that should help some stuff

* YAY

* Made some finalized changes

---------

Co-authored-by: mmilunicmobile <apullinune@gmail.com>

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()

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;
 

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<SuperstructureState.Position> 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;
         }
 

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;

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;
 

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;

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);

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(
                 () -> {

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);
 }

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;
     }
 }

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;
     }
 }