merged develop into branch switching

build.gradle

@@ -150,3 +150,10 @@ spotless {
 }
 
 tasks.compileJava.dependsOn(tasks.spotlessApply)
+sourceSets {
+    main {
+        java {
+            srcDir "${buildDir.absolutePath}/generated/sources/annotationProcessor/java/main"
+        }
+    }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -43,7 +43,7 @@
 public class RobotContainer {
 
     public final CommandXboxController primaryXboxController;
-    public final CommandXboxController seconaryXboxController;
+    public final CommandXboxController secondaryXboxController;
 
     @Logged(name = "Vision/Limelight")
     public final LimelightAprilTagSystem limelight;
@@ -91,8 +91,8 @@ public RobotContainer() {
         //        aprilTagCamSim.addCamera(simCam);
         //        reefCam.setCamera(aprilTagCamSim.getAprilTagCamSims().get(0).getCam());
 
-        primaryXboxController = new CommandXboxController(Constants.PRIMARY_XBOX_CONTROLLER);
-        seconaryXboxController = new CommandXboxController(Constants.SECONDARY_CONTROLLER_PORT);
+        primaryXboxController = new CommandXboxController(Constants.PRIMARY_XBOX_CONTROLLER_PORT);
+        secondaryXboxController = new CommandXboxController(Constants.SECONDARY_XBOX_CONTROLLER_PORT);
         limelight = new LimelightAprilTagSystem("limelight", drivetrain);
         coralManipulator = new CoralManipulatorSystem();
         configureBindings();
@@ -108,23 +108,6 @@ public RobotContainer() {
      * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
      * joysticks}.
      */
-    public void updateVision() {
-        Optional<AprilTagPose> aprilTagPoseOpt = limelight.getEstimatedPose();
-
-        if (aprilTagPoseOpt.isPresent() && !drivetrain.isMotionBlur()) {
-            AprilTagPose pose = aprilTagPoseOpt.get();
-
-            if (pose.getNumTags() > 0) {
-                // drivetrain.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999));
-                drivetrain.addVisionMeasurement(pose.getEstimatedRobotPose(), pose.getTimestamp());
-            }
-        }
-    }
-
-    //    public void updateVisionSim() {
-    //        aprilTagCamSim.update(drivetrain.getState().Pose);
-    //    }
-
     private void configureBindings() {
         drivetrain.setDefaultCommand(
                 drivetrain.applyRequest(
@@ -154,23 +137,15 @@ private void configureBindings() {
                         () -> isTargetBranchLeft));
         primaryXboxController
                 .povRight()
-                .onTrue(coralManipulator.transitionTo(CoralManipulatorState.SCORE_L2));
-        primaryXboxController
+                .onTrue(coralManipulator.setQueueState(CoralManipulatorState.L2));
+        secondaryXboxController
                 .povDown()
-                .onTrue(coralManipulator.transitionTo(CoralManipulatorState.SCORE_L3));
-        primaryXboxController
+                .onTrue(coralManipulator.setQueueState(CoralManipulatorState.L3));
+        secondaryXboxController
                 .povLeft()
-                .onTrue(coralManipulator.transitionTo(CoralManipulatorState.SCORE_L4));
-        primaryXboxController
-                .leftBumper()
-                .onTrue(coralManipulator.transitionTo(CoralManipulatorState.STOWED));
-        primaryXboxController
-                .rightBumper()
-                .onTrue(coralManipulator.transitionTo(CoralManipulatorState.INTAKE_CORAL));
-
-        seconaryXboxController.leftBumper().onTrue(setTargetBranch(ReefAlignCommand.Branches.LEFT));
-        seconaryXboxController.rightTrigger().onTrue(setTargetBranch(ReefAlignCommand.Branches.RIGHT));
-
+                .onTrue(coralManipulator.setQueueState(CoralManipulatorState.L4));
+        primaryXboxController.leftTrigger().onTrue(coralManipulator.selectQueuedStateCommand());
+        primaryXboxController.rightTrigger().onTrue((coralManipulator.scoreState()));
     }
 
     private void assembleMechanisms() {
@@ -219,7 +194,4 @@ public Command setTargetBranch(ReefAlignCommand.Branches branch){
     public Command getAutonomousCommand() {
         return null;
     }
-
-
-
 }

src/main/java/frc/robot/constants/Constants.java

@@ -14,8 +14,8 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final int PRIMARY_XBOX_CONTROLLER = 0;
-    public static final int SECONDARY_CONTROLLER_PORT = 1;
+    public static final int PRIMARY_XBOX_CONTROLLER_PORT = 0;
     public static final String CANIVORE_BUS = "canivoreBus";
     public static final String RIO_BUS = "rio";
+    public static final int SECONDARY_XBOX_CONTROLLER_PORT = 1;
 }

src/main/java/frc/robot/subsystems/coral/CoralManipulatorState.java

@@ -6,7 +6,8 @@
 
 public enum CoralManipulatorState {
     INTAKE_CORAL(ArmPosition.DOWN, ElevatorPosition.INTAKE, GrabberState.ROLL_IN),
-    L1(ArmPosition.POS_L1, ElevatorPosition.POS_L1, GrabberState.ROLL_OUT),
+    L1(ArmPosition.POS_L1, ElevatorPosition.POS_L1, GrabberState.OFF),
+    SCORE_L1(ArmPosition.SCORE_L1, ElevatorPosition.POS_L1, GrabberState.ROLL_OUT),
     L2(ArmPosition.POS_L2, ElevatorPosition.POS_L2, GrabberState.OFF),
     SCORE_L2(ArmPosition.SCORE_L2, ElevatorPosition.POS_L2, GrabberState.ROLL_OUT),
     L3(ArmPosition.POS_L3, ElevatorPosition.SAFE_POSITION, GrabberState.OFF),

src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java

@@ -3,12 +3,14 @@
 import com.ctre.phoenix6.StatusCode;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
 import frc.robot.subsystems.coral.arm.ArmPosition;
 import frc.robot.subsystems.coral.arm.ArmSubsystem;
 import frc.robot.subsystems.coral.elevator.ElevatorPosition;
 import frc.robot.subsystems.coral.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.coral.grabber.GrabberSubsystem;
 import frc.robot.util.state.StatefulSubsystem;
+import java.util.Map;
 
 @Logged
 public class CoralManipulatorSystem extends StatefulSubsystem<CoralManipulatorState> {
@@ -25,6 +27,17 @@ public CoralManipulatorSystem() {
         super(CoralManipulatorState.IDLE);
     }
 
+    private CoralManipulatorState queuedState = CoralManipulatorState.IDLE;
+
+    @Logged(name = "States/Queued State")
+    public String getCQueuedState() {
+        return queuedState.toString();
+    }
+
+    private CoralManipulatorState getQueuedState() {
+        return queuedState;
+    }
+
     @Logged(name = "States/Coral Manipulator State")
     public String getCoralManipulatorState() {
         return getCurrentState().toString();
@@ -70,6 +83,34 @@ public String elevatorTransitionState() {
         return elevator.transitioningTo().orElse(ElevatorPosition.HOLD).toString();
     }
 
+    public void queueState(CoralManipulatorState state) {
+        queuedState = state;
+    }
+
+    public Command selectQueuedStateCommand() {
+        return new SelectCommand(
+                Map.of(
+                        CoralManipulatorState.L1, transitionTo(CoralManipulatorState.L1),
+                        CoralManipulatorState.L2, transitionTo(CoralManipulatorState.L2),
+                        CoralManipulatorState.L3, transitionTo(CoralManipulatorState.L3),
+                        CoralManipulatorState.L4, transitionTo(CoralManipulatorState.L4)),
+                this::getQueuedState);
+    }
+
+    public Command scoreState() {
+        return new SelectCommand(
+                Map.of(
+                        CoralManipulatorState.L1, transitionTo(CoralManipulatorState.SCORE_L1),
+                        CoralManipulatorState.L2, transitionTo(CoralManipulatorState.SCORE_L2),
+                        CoralManipulatorState.L3, transitionTo(CoralManipulatorState.SCORE_L3),
+                        CoralManipulatorState.L4, transitionTo(CoralManipulatorState.SCORE_L4)),
+                this::getQueuedState);
+    }
+
+    public Command setQueueState(CoralManipulatorState queuedState) {
+        return runOnce(() -> queueState(queuedState));
+    }
+
     @Override
     protected StatusCode initializeTransition(CoralManipulatorState targetState) {
         Command coralManipulatorCommand;

src/main/java/frc/robot/subsystems/coral/arm/ArmConfig.java

@@ -2,23 +2,24 @@
 
 import com.ctre.phoenix6.configs.*;
 import com.ctre.phoenix6.signals.*;
+import frc.robot.Robot;
 
 class ArmConfig {
 
     static final int ARM_KRAKEN_ID = 10;
     static final int ARM_CANCODER_ID = 0;
 
-    static final double MAGNET_OFFSET = 0.501221;
+    static final double MAGNET_OFFSET = -0.490479;
     static final double GEAR_RATIO = 75.6055;
 
     static final double ARM_DEPLOY_LOWER_BOUND = 0;
 
     private static final Slot0Configs SLOT_0_REAL_CONFIGS =
             new Slot0Configs()
-                    .withKP(1440)
+                    .withKP(1100)
                     .withKI(0)
-                    .withKD(300)
-                    .withKG(7.5)
+                    .withKD(350)
+                    .withKG(10)
                     .withKV(1)
                     .withKA(2)
                     .withGravityType(GravityTypeValue.Arm_Cosine);
@@ -44,7 +45,7 @@ class ArmConfig {
 
     static final MotionMagicConfigs motionMagicConfigs =
             new MotionMagicConfigs()
-                    .withMotionMagicCruiseVelocity(0.5)
+                    .withMotionMagicCruiseVelocity(2)
                     .withMotionMagicAcceleration(1)
                     .withMotionMagicJerk(0);
 
@@ -55,11 +56,14 @@ class ArmConfig {
                                     .withFeedbackRemoteSensorID(ARM_CANCODER_ID)
                                     .withFeedbackSensorSource(
                                             FeedbackSensorSourceValue.FusedCANcoder)
-                                    .withRotorToSensorRatio(GEAR_RATIO)
-                                    .withSensorToMechanismRatio(1))
+                                    .withRotorToSensorRatio(Robot.isReal() ? GEAR_RATIO : 1)
+                                    .withSensorToMechanismRatio(Robot.isReal() ? 1 : GEAR_RATIO))
                     .withMotorOutput(
                             new MotorOutputConfigs()
-                                    .withInverted(InvertedValue.Clockwise_Positive)
+                                    .withInverted(
+                                            Robot.isReal()
+                                                    ? InvertedValue.Clockwise_Positive
+                                                    : InvertedValue.CounterClockwise_Positive)
                                     .withNeutralMode(NeutralModeValue.Brake))
                     .withSlot0(SLOT_0_REAL_CONFIGS)
                     .withSlot1(SLOT_1_WOOD_CONFIGS)

src/main/java/frc/robot/subsystems/coral/arm/ArmPosition.java

@@ -8,6 +8,7 @@ public enum ArmPosition implements SetpointEnum {
     DOWN(-0.254),
     UP(0.254),
     POS_L1(-0.1),
+    SCORE_L1(-0.1),
     POS_L2(.13),
     SCORE_L2(.07),
     POS_L3(.13),

src/main/java/frc/robot/subsystems/coral/elevator/ElevatorSubsystem.java

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.units.AngleUnit;
@@ -96,6 +97,15 @@ public StatusCode moveTo(Angle setpoint) {
 
     @Override
     public double updateMechPos() {
-        return inchesToMeters(elevatorPosition.getValueAsDouble());
+        return inchesToMeters(elevatorPosition.getValueAsDouble() * 6) + inchesToMeters(1);
+    }
+
+    @Override
+    protected boolean isTransitionFinished() {
+        if (super.isTransitionFinished()
+                && elevatorPosition == ElevatorPosition.BOTTOM.getHeight()) {
+            primaryElevatorMotor.setControl(new NeutralOut());
+        }
+        return super.isTransitionFinished();
     }
 }

src/main/java/frc/robot/subsystems/drivetrain/TunerConstants.java

@@ -181,7 +181,7 @@ public class TunerConstants {
 
     // Back Left
     private static final int kBackLeftDriveMotorId = 8;
-    private static final int kBackLeftSteerMotorId = 21;
+    private static final int kBackLeftSteerMotorId = 7;
     private static final int kBackLeftEncoderId = 24;
     private static final Angle kBackLeftEncoderOffset = Rotations.of(0.212890625);
     private static final boolean kBackLeftSteerMotorInverted = false;