Merge pull request #83 from grt192/relative-encoder-mode

Utah regional

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

@@ -10,7 +10,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 
-import frc.robot.motorcontrol.HallEffectMagnet;
+import frc.robot.sensors.HallEffectSensor;
 
 import org.photonvision.PhotonCamera;
 
@@ -38,16 +38,16 @@ public static final class TankConstants{
     public static final class SwerveConstants {
         public static final double TL_OFFSET_RADS = IS_R1 
             ? -5.704799652099609
-            : -0.2999996840953827; // 2.8415929694944104 flipped
+            : -0.3458388725942889; // 2.795753780995504 flipped
         public static final double TR_OFFSET_RADS = IS_R1 
             ? -0.842838776116
-            : -0.4716239094773709; // 2.6699687441124222 flipped
+            : -0.44043676455947356; // 2.7011558890303196 flipped
         public static final double BL_OFFSET_RADS = IS_R1
             ? -0.793255341057
-            : -2.4979224959996085; // 0.6436701575901846 flipped
+            : -0.4294843792954861; // 2.712108274294307 flipped
         public static final double BR_OFFSET_RADS = IS_R1
             ? 0.561284053322
-            : 1.8687766234027308; // -1.2728160301870624 flipped
+            : 1.5007363875680646; // -1.6408562660217285 flipped
 
         public static final int TL_DRIVE = 2;
         public static final int TL_STEER = 3;
@@ -129,13 +129,13 @@ public static final class TiltedElevatorConstants {
         public static final int EXTENSION_FOLLOW_ID = 8;
         public static final int EXTENSION_FOLLOW_B_ID = 9;
 
-        public static final float EXTENSION_LIMIT = (float) Units.inchesToMeters(63);
-        public static final double EXTENSION_TOLERANCE = Units.inchesToMeters(1);
+        public static final float EXTENSION_LIMIT_METERS = (float) Units.inchesToMeters(62.5);
+        public static final double EXTENSION_TOLERANCE_METERS = Units.inchesToMeters(1);
 
         public static final int ZERO_LIMIT_ID = 1;
         public static final int LEFT_HALL_ID = 4;
-        public static final HallEffectMagnet[] LEFT_MAGNETS = {
-            new HallEffectMagnet(EXTENSION_LIMIT)
+        public static final HallEffectSensor.Magnet[] LEFT_MAGNETS = {
+            new HallEffectSensor.Magnet(EXTENSION_LIMIT_METERS)
         };
     }
 
@@ -157,7 +157,7 @@ public static final class VisionConstants {
 
     public static final class LEDConstants {
         public static final int LED_PWM_PORT = 1;
-        public static final int LED_LENGTH = 91;
+        public static final int LED_LENGTH = 144;
     }
 
     public static final class GripperConstants {
@@ -173,7 +173,7 @@ public static final class RollerConstants {
         public static final int RIGHT_ID = 14;
         public static final int LIMIT_SWITCH_ID = 0;
 
-        public static final double ALLOW_OPEN_HEIGHT = Units.inchesToMeters(17);
+        public static final double ALLOW_OPEN_EXTENSION_METERS = Units.inchesToMeters(17);
     }
 
     public static final class MoverConstants {

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
 import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
 
 /**
@@ -62,7 +63,9 @@ public void autonomousPeriodic() {}
     @Override
     public void teleopInit() {
         if (robotContainer.driveSubsystem instanceof BaseSwerveSubsystem) {
-            ((BaseSwerveSubsystem) robotContainer.driveSubsystem).setVisionEnabled(false);
+            BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) robotContainer.driveSubsystem;
+            swerveSubsystem.setVisionEnabled(false);
+            swerveSubsystem.setChargingStationLocked(false);
         }
 
         // This makes sure that the autonomous stops running when

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

@@ -17,9 +17,12 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
+import frc.robot.commands.auton.BalanceAndTaxiAutonSequence;
 import frc.robot.commands.auton.BalanceAutonSequence;
+import frc.robot.commands.auton.BottomBalanceAutonSequence;
 import frc.robot.commands.auton.BottomOnePieceAutonSequence;
 import frc.robot.commands.auton.BottomTwoPieceAutonSequence;
+import frc.robot.commands.auton.PreloadedOnlyAutonSequence;
 import frc.robot.commands.auton.TopOnePieceAutonSequence;
 import frc.robot.commands.auton.TopTwoPieceAutonSequence;
 import frc.robot.commands.auton.test.BoxAutonSequence;
@@ -40,7 +43,6 @@
 import frc.robot.vision.SwitchableCamera;
 import frc.robot.vision.PhotonWrapper;
 import frc.robot.subsystems.drivetrain.TankSubsystem;
-import frc.robot.subsystems.leds.LEDStrip;
 import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.tiltedelevator.ElevatorState;
 import frc.robot.subsystems.tiltedelevator.TiltedElevatorSubsystem;
@@ -100,12 +102,13 @@ public class RobotContainer {
     private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Driver");
     private final SendableChooser<Command> autonChooser;
     private final BaseBalancerCommand balancerCommand;
+    private final MotorTestCommand testCommand;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
-        driveController = new XboxDriveController();
+        driveController = new DualJoystickDriveController();
 
         photonWrapper = new PhotonWrapper();
         switchableCamera = new SwitchableCamera(shuffleboardTab);
@@ -117,37 +120,49 @@ public RobotContainer() {
         signalLEDSubsystem = new LEDSubsystem(); 
 
         superstructure = new Superstructure(rollerSubsystem, tiltedElevatorSubsystem, signalLEDSubsystem, switchableCamera);
+
         balancerCommand = new DefaultBalancerCommand(driveSubsystem);
 
-        // Configure the button bindings
-        configureButtonBindings();
+        // Configure button bindings
+        configureDriveBindings();
+        configureMechBindings();
 
-        // Add auton sequences to the chooser and add the chooser to shuffleboard
+        // Initialize auton and test commands
         autonChooser = new SendableChooser<>();
         autonChooser.setDefaultOption("Skip auton", new InstantCommand());
 
         if (driveSubsystem instanceof BaseSwerveSubsystem) {
             final BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) driveSubsystem;
 
+            testCommand = new MotorTestCommand(swerveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
+
+            autonChooser.addOption("Red preloaded only", new PreloadedOnlyAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
             autonChooser.addOption("Red top auton (1-piece)", new TopOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
-            autonChooser.addOption("Red top auton (2-piece)", new TopTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
+            // autonChooser.addOption("Red top auton (2-piece)", new TopTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
             autonChooser.addOption("Red balance auton", new BalanceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
+            // autonChooser.addOption("Red balance and taxi auton", new BalanceAndTaxiAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
             autonChooser.addOption("Red bottom auton (1-piece)", new BottomOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
-            autonChooser.addOption("Red bottom auton (2-piece)", new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
+            // autonChooser.addOption("Red bottom auton (2-piece)", new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
+            // autonChooser.addOption("Red bottom balance auton", new BottomBalanceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
 
+            autonChooser.addOption("Blue preloaded only", new PreloadedOnlyAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
             autonChooser.addOption("Blue top auton (1-piece)", new TopOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
-            autonChooser.addOption("Blue top auton (2-piece)", new TopTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
+            // autonChooser.addOption("Blue top auton (2-piece)", new TopTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
             autonChooser.addOption("Blue balance auton", new BalanceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
+            // autonChooser.addOption("Blue balance and taxi auton", new BalanceAndTaxiAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
             autonChooser.addOption("Blue bottom auton (1-piece)", new BottomOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
-            autonChooser.addOption("Blue bottom auton (2-piece)", new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
+            // autonChooser.addOption("Blue bottom auton (2-piece)", new BottomTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
+            // autonChooser.addOption("Blue bottom balance auton", new BottomBalanceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, false));
 
-            // autonChooser.addOption("10\" straight-line path", new TenFeetStraightLinePath(swerveSubsystem));
-            // autonChooser.addOption("20\" straight-line path", new TwentyFeetStraightLinePath(swerveSubsystem));
+            // autonChooser.addOption("10' straight-line path", new TenFeetStraightLinePath(swerveSubsystem));
+            // autonChooser.addOption("20' straight-line path", new TwentyFeetStraightLinePath(swerveSubsystem));
             // autonChooser.addOption("High rotation straight-line path", new HighRotationLinePath(swerveSubsystem));
             // autonChooser.addOption("Rotating S-curve", new RotatingSCurveAutonSequence(swerveSubsystem));
             // autonChooser.addOption("Box auton", new BoxAutonSequence(swerveSubsystem));
             // autonChooser.addOption("No-stopping box auton", new ContinuousBoxAutonSequence(swerveSubsystem));
             // autonChooser.addOption("GRT path", new GRTAutonSequence(swerveSubsystem));
+        } else {
+            testCommand = null;
         }
 
         shuffleboardTab.add("Auton", autonChooser)
@@ -156,17 +171,9 @@ public RobotContainer() {
     }
 
     /**
-     * Use this method to define your button->command mappings. Buttons can be
-     * created by instantiating a {@link GenericHID} or one of its subclasses
-     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
-     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     * Configures button bindings for the drive subsystem and controller.
      */
-    private void configureButtonBindings() {
-        driveBindings();
-        mechBindings();
-    }
-
-    private void driveBindings() {
+    private void configureDriveBindings() {
         driveController.getBalancerButton().whileTrue(balancerCommand);
         driveController.getCameraSwitchButton().onTrue(new InstantCommand(switchableCamera::switchCamera));
 
@@ -180,19 +187,25 @@ private void driveBindings() {
                 boolean relative = driveController.getSwerveRelative();
 
                 if (driveController.getSwerveHeadingLock()) {
-                    double currentHeading = swerveSubsystem.getRobotPosition().getRotation().getRadians();
-                    double lockHeading = (Math.abs(MathUtil.angleModulus(currentHeading)) > (Math.PI / 2)) ? Math.PI : 0;
+                    double currentHeadingRads = swerveSubsystem.getFieldHeading().getRadians();
+                    double lockHeadingRads = (Math.abs(currentHeadingRads) > Math.PI / 2.0) ? Math.PI : 0;
 
-                    swerveSubsystem.setDrivePowersWithHeadingLock(xPower, yPower, new Rotation2d(lockHeading), relative);
+                    swerveSubsystem.setDrivePowersWithHeadingLock(xPower, yPower, new Rotation2d(lockHeadingRads), relative);
                 } else {
                     swerveSubsystem.setDrivePowers(xPower, yPower, angularPower, relative);
                 }
             }, swerveSubsystem));
 
-            mechAButton.onTrue(new DropperChooserCommand(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem));
-
             driveController.getFieldResetButton().onTrue(new InstantCommand(swerveSubsystem::resetFieldAngle, swerveSubsystem));
             driveController.getChargingStationLockButton().onTrue(new InstantCommand(swerveSubsystem::toggleChargingStationLocked, swerveSubsystem));
+
+            /*
+            brSwitch.onTrue(new InstantCommand(() -> {
+                swerveSubsystem.setSteerRelativeEncoderFeedback(true);
+            }, swerveSubsystem)).onFalse(new InstantCommand(() -> {
+                swerveSubsystem.setSteerRelativeEncoderFeedback(false);
+            }, swerveSubsystem));
+            */
         } else if (driveSubsystem instanceof TankSubsystem) {
             final TankSubsystem tankSubsystem = (TankSubsystem) driveSubsystem;
 
@@ -212,36 +225,41 @@ private void driveBindings() {
         }
     }
 
-    private void mechBindings() {
+    /**
+     * Configures button bindings for the roller / elevator mechs and controller.
+     */
+    private void configureMechBindings() {
+        mechAButton.onTrue(new DropperChooserCommand(driveSubsystem, rollerSubsystem, tiltedElevatorSubsystem));
+
         rollerSubsystem.setDefaultCommand(new RunCommand(() -> {
-            double rollPower = (.95 * mechController.getRightTriggerAxis()) - (.65 * mechController.getLeftTriggerAxis());
-            rollerSubsystem.setRollPower(rollPower);
+            double forwardPower = 0.95 * mechController.getRightTriggerAxis();
+            double reversePower = 0.65 * mechController.getLeftTriggerAxis();
+            rollerSubsystem.setRollPower(forwardPower - reversePower);
         }, rollerSubsystem));
 
-        mechYButton.onTrue(new InstantCommand(() -> {
-            rollerSubsystem.openMotor();;
-        }, rollerSubsystem));
+        mechYButton.onTrue(new InstantCommand(rollerSubsystem::openMotor, rollerSubsystem));
 
         tiltedElevatorSubsystem.setDefaultCommand(new RunCommand(() -> {
             double yPower = -mechController.getLeftY();
-            double pov = mechController.getPOV();
-            if(pov == 0){
-                tiltedElevatorSubsystem.setState(ElevatorState.SUBSTATION);
-            } else if (pov == 270){
-                tiltedElevatorSubsystem.setState(ElevatorState.HYBRID);
-            } else if (pov == 180){
-                tiltedElevatorSubsystem.setState(ElevatorState.GROUND);
-                tiltedElevatorSubsystem.offsetState = OffsetState.OVERRIDE_HAS_PIECE;
-            } else if (pov == 90){
-                tiltedElevatorSubsystem.resetOffset();
+
+            int pov = mechController.getPOV();
+            switch (pov) {
+                case 0 -> tiltedElevatorSubsystem.setState(ElevatorState.SUBSTATION);
+                case 270 -> tiltedElevatorSubsystem.setState(ElevatorState.HYBRID);
+                case 180 -> {
+                    tiltedElevatorSubsystem.setState(ElevatorState.GROUND);
+                    tiltedElevatorSubsystem.offsetState = OffsetState.OVERRIDE_HAS_PIECE;
+                }
+                case 90 -> tiltedElevatorSubsystem.resetOffset();
             }
-            tiltedElevatorSubsystem.setManualPower(yPower);
 
+            tiltedElevatorSubsystem.setManualPower(yPower);
             tiltedElevatorSubsystem.changeOffsetDistMeters(yPower);
         }, tiltedElevatorSubsystem));
 
         mechBButton.onTrue(new InstantCommand(() -> {
-            tiltedElevatorSubsystem.toggleState(ElevatorState.GROUND, ElevatorState.CHUTE);
+            // tiltedElevatorSubsystem.toggleState(ElevatorState.GROUND, ElevatorState.CHUTE);
+            tiltedElevatorSubsystem.setState(ElevatorState.GROUND);
         }, tiltedElevatorSubsystem));
 
         mechXButton.onTrue(new InstantCommand(() -> {
@@ -257,9 +275,11 @@ private void mechBindings() {
         }, tiltedElevatorSubsystem));
 
         signalLEDSubsystem.setDefaultCommand(new RunCommand(() -> {
-            signalLEDSubsystem.setColor(blSwitch.getAsBoolean()
-                ? new Color(255, 100, 0)
-                : new Color(192, 8, 254));
+            if (blSwitch.getAsBoolean()) {
+                signalLEDSubsystem.setColor(255, 100, 0);
+            } else {
+                signalLEDSubsystem.setColor(192, 8, 254);
+            }
         }, signalLEDSubsystem));
     }
 
@@ -270,17 +290,12 @@ private void mechBindings() {
     public Command getAutonomousCommand() {
         return autonChooser.getSelected();
     }
-    
+
     /**
      * Use this to pass the test command to the main {@link Robot} class.
      * @return the command to run in test
      */
     public Command getTestCommand() {
-        if (!(driveSubsystem instanceof BaseSwerveSubsystem)) return null;
-        return new MotorTestCommand(
-            (BaseSwerveSubsystem) driveSubsystem,
-            tiltedElevatorSubsystem,
-            rollerSubsystem
-        );
+        return testCommand;
     }
 }

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

@@ -0,0 +1,49 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+import frc.robot.commands.balancing.DefaultBalancerCommand;
+import frc.robot.commands.balancing.GoOverCommand;
+import frc.robot.positions.FieldPosition;
+import frc.robot.positions.PlacePosition;
+import frc.robot.positions.PlacePosition.PlaceState;
+import frc.robot.subsystems.RollerSubsystem;
+import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
+import frc.robot.subsystems.tiltedelevator.TiltedElevatorSubsystem;
+
+public class BalanceAndTaxiAutonSequence extends BaseAutonSequence {
+    private static final FieldPosition INITIAL_POSE = FieldPosition.B2_INIT;
+    private static final PlacePosition PLACE_POSE = PlacePosition.B2HIGH;
+
+    /**
+     * Balancing auton sequence.
+     * @param swerveSubsystem The swerve subsystem.
+     * @param rollerSubsystem The roller subsystem.
+     * @param tiltedElevatorSubsystem The tilted elevator subsystem.
+     * @param isRed Whether this is a red auton path.
+     */
+    public BalanceAndTaxiAutonSequence(
+        BaseSwerveSubsystem swerveSubsystem, RollerSubsystem rollerSubsystem, TiltedElevatorSubsystem tiltedElevatorSubsystem,
+        boolean isRed // TODO: better way of passing this
+    ) {
+        super(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, INITIAL_POSE.getPose(isRed)); // TODO: better
+
+        Pose2d initialPose = INITIAL_POSE.getPose(isRed);
+        PlaceState placeState = PLACE_POSE.getPlaceState(isRed);
+
+        addCommands(
+            // Interrupt balancer after 14.5 seconds have elapsed in the sequence.
+            // TODO: very ugly way of implementing this at the moment.
+            new WaitCommand(14.5).deadlineWith(
+                // Place preloaded gamepiece
+                goAndPlace(initialPose, placeState).andThen(
+                    // Go over the charging station to taxi
+                    new GoOverCommand(swerveSubsystem),
+                    // Go and balance on charging station from the other side
+                    new DefaultBalancerCommand(swerveSubsystem)
+                )
+            )
+        );
+    }
+}