Merge pull request #116 from grt192/mbr

Monterey Bay Regional

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

@@ -41,13 +41,13 @@ public static final class SwerveConstants {
             : -2.4960072914785663;
         public static final double TR_OFFSET_RADS = IS_R1 
             ? -1.1199431538621365 // 2.0216494997276566
-            : -0.41136027574934353;
+            : -0.3756485104600369; //2.7659441431297562;
         public static final double BL_OFFSET_RADS = IS_R1
             ? -2.374440566697393
-            : 0.6375202695515494;
+            : -0.0563106973979135 ; //3.0852819561918796
         public static final double BR_OFFSET_RADS = IS_R1
             ? -2.8609512726492206 // 0.28064138094057256
-            : -2.667189121246338;
+            :  0.6332055648141584; //-2.5083870887756348
 
         public static final int TL_DRIVE = 2;
         public static final int TL_STEER = 3;
@@ -147,12 +147,19 @@ public static final class VisionConstants {
             new Rotation3d(Math.PI, 0, 0)
         );
 
-        public static final PhotonCamera BACK_CAMERA = new PhotonCamera("HD_USB_Camera");
-        public static final Transform3d BACK_CAMERA_POSE = new Transform3d(
+        public static final PhotonCamera RIGHT_CAMERA = new PhotonCamera("HD_USB_Camera");
+        public static final Transform3d RIGHT_CAMERA_POSE = new Transform3d(
             // new Translation3d(Units.inchesToMeters(7.375), Units.inchesToMeters(10.597), Units.inchesToMeters(22.638875)),
-            new Translation3d(Units.inchesToMeters(7.625), Units.inchesToMeters(7.767605), Units.inchesToMeters(22.25)),
-            new Rotation3d(Math.PI, 0, Math.PI)
+            new Translation3d(Units.inchesToMeters(10.125), Units.inchesToMeters(-7.767605), Units.inchesToMeters(22.25)),
+            new Rotation3d(Math.PI, 0, 0)
         );
+
+        // public static final PhotonCamera BACK_CAMERA = new PhotonCamera("HD_USB_Camera");
+        // public static final Transform3d BACK_CAMERA_POSE = new Transform3d(
+        //     // new Translation3d(Units.inchesToMeters(7.375), Units.inchesToMeters(10.597), Units.inchesToMeters(22.638875)),
+        //     new Translation3d(Units.inchesToMeters(7.625), Units.inchesToMeters(7.767605), Units.inchesToMeters(22.25)),
+        //     new Rotation3d(Math.PI, 0, Math.PI)
+        // );
     }
 
     public static final class LEDConstants {
@@ -190,6 +197,25 @@ public static final class MoverConstants {
 
     public static final class BalancerConstants {
         public static final double GRT_CHARGING_STATION_KP = 0.3 / 35;
-        public static final double COMP_CHARGING_STATION_KP = 0.0072;
+        public static final double COMP_CHARGING_STATION_KP = 0.007;
+    }
+
+    public static final class FieldConstants {
+        public static final double ROBOT_LENGTH_INCHES = 38.5;
+
+        public static final double GRID_X_INCHES = 53.938 + (ROBOT_LENGTH_INCHES / 2.0);
+        public static final double ALIGNMENT_OFFSET_INCHES = 12.885;
+
+        public static final double C3_Y_INCHES = 196.19;
+        public static final double C2_Y_INCHES = 174.19;
+        public static final double C1_Y_INCHES = 152.19;
+
+        public static final double B3_Y_INCHES = 130.19;
+        public static final double B2_Y_INCHES = 108.19;
+        public static final double B1_Y_INCHES = 86.19;
+
+        public static final double A3_Y_INCHES = 64.19;
+        public static final double A2_Y_INCHES = 42.19;
+        public static final double A1_Y_INCHES = 20.19;
     }
 }

src/main/java/frc/robot/Robot.java

@@ -47,6 +47,11 @@ public void disabledPeriodic() {}
 
     @Override
     public void autonomousInit() {
+        if (robotContainer.driveSubsystem instanceof BaseSwerveSubsystem) {
+            BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) robotContainer.driveSubsystem;
+            swerveSubsystem.setVisionEnabled(false);
+        }
+
         // Schedule the autonomous command and cancel testing
         autonomousCommand = robotContainer.getAutonomousCommand();
         if (testCommand != null) testCommand.cancel();
@@ -60,6 +65,7 @@ public void autonomousPeriodic() {}
     public void teleopInit() {
         if (robotContainer.driveSubsystem instanceof BaseSwerveSubsystem) {
             BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) robotContainer.driveSubsystem;
+            swerveSubsystem.setVisionEnabled(true);
             swerveSubsystem.setChargingStationLocked(false);
         }
 

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

@@ -65,6 +65,7 @@
 import frc.robot.subsystems.drivetrain.BaseDrivetrain;
 import frc.robot.subsystems.RollerSubsystem;
 import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.RollerSubsystem.HeldPiece;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -157,6 +158,10 @@ public RobotContainer() {
             autonInitialPoseChooser.addOption(position.name(), position);
         }
 
+        // shuffleboardTab.add(driveSubsystem)
+        //    .withPosition(9, 0)
+        //    .withSize(3, 3);
+
         if (driveSubsystem instanceof BaseSwerveSubsystem) {
             final BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) driveSubsystem;
             testCommand = new MotorTestCommand(swerveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
@@ -212,7 +217,13 @@ private void configureDriveBindings() {
                 double angularPower = driveController.getRotatePower();
                 boolean relative = driveController.getSwerveRelative();
 
-                if (driveController.getSwerveHeadingLock()) {
+                if (driveController.approachShelf()) { //changed to 0.45
+                    if (rollerSubsystem.getPiece() == HeldPiece.EMPTY){
+                        swerveSubsystem.setDrivePowersWithHeadingLock(0.375, yPower, new Rotation2d(0), false);
+                    } else {
+                        swerveSubsystem.setDrivePowers(0, 0, 0, relative);
+                    }
+                } else if (driveController.getSwerveHeadingLock()) {
                     double currentHeadingRads = swerveSubsystem.getDriverHeading().getRadians();
                     double lockHeadingRads = (Math.abs(currentHeadingRads) > Math.PI / 2.0) ? Math.PI : 0;
 
@@ -322,9 +333,13 @@ private void configureMechBindings() {
     public Command getAutonomousCommand() {
         // TODO: find some way to set up listeners such that we can preconstruct this before auton starts
         if (!(driveSubsystem instanceof BaseSwerveSubsystem)) return null;
+
+        PlacePosition initialPose = autonInitialPoseChooser.getSelected();
+        if (initialPose == null) return null;
+
         return autonPathChooser.getSelected().create(
             (BaseSwerveSubsystem) driveSubsystem, rollerSubsystem, tiltedElevatorSubsystem,
-            autonInitialPoseChooser.getSelected(), isRedEntry.getBoolean(false)
+            initialPose, isRedEntry.getBoolean(false)
         );
     }
 

src/main/java/frc/robot/commands/auton/BalanceAutonSequence.java

@@ -25,6 +25,7 @@ public BalanceAutonSequence(
         super(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, initialPosition, isRed);
 
         addCommands(
+            new WaitCommand(1.4), //to let elevator lower all the way untested
             // Go and balance on charging station
             new DefaultBalancerCommand(swerveSubsystem, true)
         );

src/main/java/frc/robot/commands/auton/BaseAutonSequence.java

@@ -108,8 +108,13 @@ protected SequentialCommandGroup goAndPlace(Pose2d initialPose, Pose2d finalPose
      * @return The `SequentialCommandGroup` representing running the commands in order.
      */
     protected SequentialCommandGroup goAndPlace(Pose2d initialPose, Pose2d finalPose, ElevatorState elevatorState, boolean startsMoving) {
-        return new TiltedElevatorCommand(tiltedElevatorSubsystem, elevatorState)
-            .alongWith(FollowPathCommand.from(swerveSubsystem, initialPose, List.of(), finalPose, startsMoving, false))
+        TiltedElevatorCommand elevatorCommand = new TiltedElevatorCommand(tiltedElevatorSubsystem, elevatorState);
+
+        Command driveForwardCommand = elevatorState == ElevatorState.CONE_MID || elevatorState == ElevatorState.CONE_HIGH
+            ? elevatorCommand.andThen(FollowPathCommand.from(swerveSubsystem, initialPose, List.of(), finalPose, startsMoving, false))
+            : elevatorCommand.alongWith(FollowPathCommand.from(swerveSubsystem, initialPose, List.of(), finalPose, startsMoving, false));
+
+        return driveForwardCommand
             .andThen(new SwerveIdleCommand(swerveSubsystem))
             .andThen(new WaitCommand(0.1))
             .andThen(DropperChooserCommand.getSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, elevatorState));

src/main/java/frc/robot/commands/balancing/DefaultBalancerCommand.java

@@ -80,7 +80,7 @@ public void execute() {
         double currentPitchDegs = ahrs.getPitch();
 
         if (!reachedStation) {
-            returnDrivePower = -0.80 * direction;
+            returnDrivePower = -0.75 * direction;
             if ((reverse && currentPitchDegs <= -10.0) || (!reverse && currentPitchDegs >= 10.0)) {
                 reachedStation = true;
                 balanceLog.append("Reached Charging Station");
@@ -101,8 +101,8 @@ public void execute() {
                 }
             } else {
                 returnDrivePower = -1 * drivePID.calculate(currentPitchDegs, 0);
-
-                if ((reverse && Math.abs(currentPitchDegs) <= 1.0 && deltaPitchDegs <= 0.1) || (!reverse && Math.abs(currentPitchDegs) >= -1.0 && deltaPitchDegs >= -0.1)){
+                // if ((reverse && Math.abs(currentPitchDegs) <= 1.0 && deltaPitchDegs <= 0.1) || (!reverse && Math.abs(currentPitchDegs) >= -1.0 && deltaPitchDegs >= -0.1)){
+                if (Math.abs(currentPitchDegs) <= 1.0 && ((reverse && deltaPitchDegs <= 0.1) || (!reverse && deltaPitchDegs >= -0.1))){
                     balanced = true;
                     balanceLog.append("Robot balanced");
                 }

src/main/java/frc/robot/commands/dropping/AutoAlignCommand.java

@@ -285,6 +285,7 @@ private void scheduleAlignCommandWith(PlacePosition newPosition, boolean driveFo
             wrappedAlignCommand = new AlignToNodeCommand(swerveSubsystem, tiltedElevatorSubsystem, targetPlacePosition, isRed);
         }
 
+        // System.out.println("Aligning with " + targetPlacePosition.name());
         wrappedAlignCommand.schedule();
 
         // Lock parallel to the grid for better strafing

src/main/java/frc/robot/commands/swerve/GoToPointCommand.java

@@ -92,4 +92,9 @@ public boolean isFinished() {
         double yErrorMeters = Math.abs(targetPose.getY() - currentPose.getY());
         return xErrorMeters < X_TOLERANCE_METERS && yErrorMeters < Y_TOLERANCE_METERS && isHeadingAligned(THETA_TOLERANCE_RADS);
     }
+
+    @Override
+    public void end(boolean interrupted) {
+        currentPose = null;
+    }
 }

src/main/java/frc/robot/controllers/BaseDriveController.java

@@ -33,6 +33,14 @@ public abstract class BaseDriveController {
      */
     public abstract boolean getSwerveHeadingLock();
 
+    /**
+     * Gets whether the swerve is approaching the shelf to intake a piece.
+     * @return Whether the swerve should automatically approach the shelf.
+     */
+    public boolean approachShelf() {
+        return false;
+    }
+
     public abstract JoystickButton getBalancerButton();
     public abstract JoystickButton getFieldResetButton();
     public abstract JoystickButton getCameraSwitchButton();

src/main/java/frc/robot/controllers/DualJoystickDriveController.java

@@ -99,7 +99,12 @@ public JoystickButton getDriveForwardButton() {
 
     @Override
     public JoystickButton getCancelAutoAlignButton() {
-        return rightBackButton;
+        return leftBackButton;
+    }
+
+    @Override
+    public boolean approachShelf() {
+        return rightBackButton.getAsBoolean();
     }
 
     /**
@@ -108,7 +113,7 @@ public JoystickButton getCancelAutoAlignButton() {
      */
     private double getDriveScaling() {
         boolean isSlowMode = leftJoystick.getTrigger();
-        return isSlowMode ? 0.3 : 1.0;
+        return isSlowMode ? 0.24 : 1.0;
     }
 
     /**
@@ -117,6 +122,6 @@ private double getDriveScaling() {
      */
     private double getTurnScaling() {
         boolean isSlowMode = leftJoystick.getTrigger();
-        return isSlowMode ? 0.25 : 0.5;
+        return isSlowMode ? 0.125 : 0.5;
     }
 }

src/main/java/frc/robot/positions/FieldPosition.java

@@ -5,101 +5,102 @@
 import edu.wpi.first.math.util.Units;
 
 import frc.robot.util.FieldUtil;
+import static frc.robot.Constants.FieldConstants.*;
 
 public enum FieldPosition {
     C3(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(195.55),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(C3_Y_INCHES), // 195.55
         Rotation2d.fromDegrees(180)
     )),
     C2(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(174.123),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(C2_Y_INCHES), // 174.123
         Rotation2d.fromDegrees(180)
     )),
     C1(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(152.123),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(C1_Y_INCHES), // 152.123
         Rotation2d.fromDegrees(180)
     )),
 
     B3(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(129.75),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(B3_Y_INCHES), // 129.75
         Rotation2d.fromDegrees(180)
     )),
     B2(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(107.801),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(B2_Y_INCHES), // 107.801
         Rotation2d.fromDegrees(180)
     )),
     B1(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(86.149),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(B1_Y_INCHES), // 86.149
         Rotation2d.fromDegrees(180)
     )),
 
     A3(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(64.818),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(A3_Y_INCHES), // 64.818
         Rotation2d.fromDegrees(180)
     )),
     A2(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(41.761),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(A2_Y_INCHES), // 41.761
         Rotation2d.fromDegrees(180)
     )),
     A1(new Pose2d(
-        Units.inchesToMeters(74),
-        Units.inchesToMeters(20.016),
+        Units.inchesToMeters(GRID_X_INCHES),
+        Units.inchesToMeters(A1_Y_INCHES), // 20.016
         Rotation2d.fromDegrees(180)
     )),
 
     C3_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(195.55),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(C3_Y_INCHES), // 195.55
         Rotation2d.fromDegrees(180)
     )),
     C2_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(174.123),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(C2_Y_INCHES), // 174.123
         Rotation2d.fromDegrees(180)
     )),
     C1_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(152.123),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(C1_Y_INCHES), // 152.123
         Rotation2d.fromDegrees(180)
     )),
 
     B3_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(129.75),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(B3_Y_INCHES), // 129.75
         Rotation2d.fromDegrees(180)
     )),
     B2_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(107.801),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(B2_Y_INCHES), // 107.801
         Rotation2d.fromDegrees(180)
     )),
     B1_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(86.149),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(B1_Y_INCHES), // 86.149
         Rotation2d.fromDegrees(180)
     )),
 
     A3_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(64.818),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(A3_Y_INCHES), // 64.818
         Rotation2d.fromDegrees(180)
     )),
     A2_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(41.761),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(A2_Y_INCHES), // 41.761
         Rotation2d.fromDegrees(180)
     )),
     A1_INIT(new Pose2d(
-        Units.inchesToMeters(86.885),
-        Units.inchesToMeters(20.016),
+        Units.inchesToMeters(GRID_X_INCHES + ALIGNMENT_OFFSET_INCHES),
+        Units.inchesToMeters(A1_Y_INCHES), // 20.016
         Rotation2d.fromDegrees(180)
     )),
 

src/main/java/frc/robot/subsystems/RollerSubsystem.java

@@ -32,6 +32,7 @@ public enum HeldPiece {
     }
 
     private HeldPiece heldPiece = HeldPiece.EMPTY;
+    private HeldPiece lastHeldPiece = HeldPiece.EMPTY;
 
     public boolean allowOpen = true;
     private boolean prevAllowedOpen = true;
@@ -196,6 +197,7 @@ private void openingLogic() {
         if (openTimer.hasStarted()) openMotor.set(0.5);
         else if (closeTimer.hasStarted()) openMotor.set(-0.2);
         else if (heldPiece == HeldPiece.CONE) openMotor.setVoltage(-4.0);
+        else if (rollPower > 0) openMotor.setVoltage(-1.0);
         else openMotor.set(0);
     }