Zach/manualscoring

.gitignore

@@ -172,4 +172,5 @@ gradle-app.setting
 # Simulator
 simgui*.json
 networktables.json
-*.wpilog
+*.wpilog
+/.run/

src/main/java/org/team1540/robot2023/Constants.java

@@ -152,7 +152,7 @@ public static final class Swerve {
         /* Module Specific Constants */
         /* Front Left Module - Module 0 */
         public static final class Mod0 {
-            private static final int moduleID = 1; //4
+            private static final int moduleID = 2; //4
             public static final SwerveModuleConstants constants = new SwerveModuleConstants(moduleID, ModuleCorner.FRONT_LEFT);
         }
 

src/main/java/org/team1540/robot2023/RobotContainer.java

@@ -6,10 +6,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.team1540.lib.RevBlinkin;
@@ -101,23 +98,23 @@ private void configureButtonBindings() {
         driver.b().whileTrue(new TurnToGamePiece(drivetrain,driver, TurnToGamePiece.GamePiece.CONE));
         //Coop: button(X, Cube vision [HOLD], pilot)
         driver.x().whileTrue(new TurnToGamePiece(drivetrain,driver, TurnToGamePiece.GamePiece.CUBE));
-
+        driver.a().onTrue(drivetrain.updateOdometryAnd(new PrintCommand("Updating odometry")));
 
         // Copilot
 //        driver.start().onTrue(new InstantCommand(drivetrain::updateWithApriltags).andThen(new PrintCommand("Rezeroing")).ignoringDisable(true));
         controlPanel.onButton(ButtonPanel.PanelButton.STYLE_PURPLE).onTrue(blinkins.commandSetGamepiece(false));
         controlPanel.onButton(ButtonPanel.PanelButton.STYLE_YELLOW).onTrue(blinkins.commandSetGamepiece(true));
 
        //coop:button(LTrigger, Confirm alignment [PRESS], pilot)
-        controlPanel.onButton(ButtonPanel.PanelButton.TOP_LEFT     ).whileTrue(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.LEFT),    intake, driver, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.TOP_CENTER   ).whileTrue(new AutoCube(drivetrain, arm,Constants.Auto.highCube.withPolePosition(PolePosition.CENTER),    intake, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.TOP_RIGHT    ).whileTrue(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.RIGHT),    intake, driver, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_LEFT  ).whileTrue(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.LEFT),     intake, driver, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_CENTER).whileTrue(new AutoCube(drivetrain, arm,Constants.Auto.midCube.withPolePosition(PolePosition.CENTER),     intake, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_RIGHT ).whileTrue(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.RIGHT),     intake, driver, true));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_LEFT  ).whileTrue(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.LEFT),  intake, driver));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_CENTER).whileTrue(new AutoHybrid(drivetrain, arm,Constants.Auto.middleHybridNode.withPolePosition(PolePosition.CENTER),  intake, driver));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_RIGHT ).whileTrue(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.RIGHT),  intake, driver));
+        controlPanel.onButton(ButtonPanel.PanelButton.TOP_LEFT     ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.LEFT),    intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.TOP_CENTER   ).whileTrue(drivetrain.updateOdometryAnd(new AutoCube(drivetrain, arm,Constants.Auto.highCube.withPolePosition(PolePosition.CENTER),    intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.TOP_RIGHT    ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.RIGHT),    intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_LEFT  ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.LEFT),     intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_CENTER).whileTrue(drivetrain.updateOdometryAnd(new AutoCube(drivetrain, arm,Constants.Auto.midCube.withPolePosition(PolePosition.CENTER),     intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_RIGHT ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.RIGHT),     intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_LEFT  ).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.LEFT),  intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_CENTER).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.middleHybridNode.withPolePosition(PolePosition.CENTER),  intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_RIGHT ).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.RIGHT),  intake, driver, false)));
 
         // coop:button(A, Run Intake [PRESS],copilot)
         copilot.a().toggleOnTrue(intakeCommand);
@@ -226,7 +223,7 @@ private void initAutos() {
 //        manager.addAuto(new Auto2PieceTaxi(drivetrain, arm, intake, ScoringGridLocation.BOTTOM_GRID));
 //        manager.addAuto("MiddleGrid1PieceSideBalance", new Auto1PieceSideBalance(drivetrain, arm, intake));
 //        manager.addAuto("MiddleGridSideBalance", new AutoSideBalance(drivetrain, arm, intake));
-        manager.addAuto("ScoreHighCube", new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, true));
+        manager.addAuto("ScoreHighCube", new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false));
         manager.addAuto("ScoreMidCube", new AutoHybrid(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake));
         manager.addDefaultAuto("DoNothing", new InstantCommand(), null);
     }

src/main/java/org/team1540/robot2023/commands/auto/AutoCube.java

@@ -1,6 +1,7 @@
 package org.team1540.robot2023.commands.auto;
 
 import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.team1540.robot2023.commands.arm.Arm;
 import org.team1540.robot2023.commands.arm.ResetArmPositionCommand;
 import org.team1540.robot2023.commands.arm.SetArmPosition;
@@ -12,7 +13,7 @@
 import java.util.function.BooleanSupplier;
 
 public class AutoCube extends SequentialCommandGroup {
-    public AutoCube(Drivetrain drivetrain, Arm arm, GridScoreData positions, WheeledGrabber intake, boolean shouldAlign){
+    public AutoCube(Drivetrain drivetrain, Arm arm, GridScoreData positions, WheeledGrabber intake, CommandXboxController controller, boolean shouldAlign){
         BooleanSupplier shouldRun;
         if (shouldAlign) {
             shouldRun = drivetrain::updateWithScoringApriltags;
@@ -28,11 +29,11 @@ public AutoCube(Drivetrain drivetrain, Arm arm, GridScoreData positions, Wheeled
                                         new SetArmPosition(arm, positions.approach),
                                         Commands.sequence(
                                                 new ProxyCommand(() -> new WaitCommand((arm.timeToState(positions.approach)-150)/1000)),
+
                                                 new GrabberOuttakeCommand(intake)
 
                                         )
                                 ),
-                                //                                    new WaitUntilCommand(() -> controller.getLeftTriggerAxis() > 0.95).unless(() -> controller == null || positions.polePosition == PolePosition.CENTER),
                                 Commands.deadline(
                                         Commands.sequence(
                                                 new SetArmPosition(arm, AutoHybrid.catchNull(positions.retreat)).unless(() -> positions.retreat == null),

src/main/java/org/team1540/robot2023/commands/auto/sequence/Auto1PieceSideBalance.java

@@ -12,7 +12,7 @@
 public class Auto1PieceSideBalance extends AutoCommand {
     public Auto1PieceSideBalance(Drivetrain drivetrain, Arm arm, WheeledGrabber intake) {
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, true),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, true),
                 getPathPlannerDriveCommand(drivetrain, "MiddleGrid1PieceSideBalance"),
                 new AutoSideBalanceCommand(drivetrain)
         );

src/main/java/org/team1540/robot2023/commands/auto/sequence/Auto1PieceTaxi.java

@@ -12,7 +12,7 @@
 public class Auto1PieceTaxi extends AutoCommand {
     public Auto1PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber intake, ScoringGridLocation.OuterGrid grid) {
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, true),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, true),
                 getPathPlannerDriveCommand(drivetrain, grid.getPathName("1PieceTaxi"))
         );
     }

src/main/java/org/team1540/robot2023/commands/auto/sequence/Auto2PieceTaxiCone.java

@@ -41,7 +41,7 @@ public Auto2PieceTaxiCone(Drivetrain drivetrain, Arm arm, WheeledGrabber intake,
                                 )
                         )
                 ),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, true)
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, true)
 
         );
     }

src/main/java/org/team1540/robot2023/commands/auto/sequence/Auto2PieceTaxiConeVision.java

@@ -49,7 +49,7 @@ public Auto2PieceTaxiConeVision(Drivetrain drivetrain, Arm arm, WheeledGrabber i
                                 )
                         )
                 ),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, true)
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, true)
 
         );
     }

src/main/java/org/team1540/robot2023/commands/auto/sequence/AutoMiddleGrid1PieceBalance.java

@@ -16,7 +16,7 @@ public class AutoMiddleGrid1PieceBalance extends AutoCommand {
     public AutoMiddleGrid1PieceBalance(Drivetrain drivetrain, Arm arm, WheeledGrabber intake) {
         addCommands(
 //                new AutoGridScore(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 new RetractAndPivotCommand(arm, Rotation2d.fromDegrees(45)),
                 getPathPlannerDriveCommand(drivetrain, "MiddleGrid1PieceBalance", new PathConstraints(1, 1), false),
                 new AutoBalanceCommand(drivetrain)

src/main/java/org/team1540/robot2023/commands/auto/sequence/AutoMiddleGrid1PieceTaxiBalance.java

@@ -21,7 +21,7 @@ public AutoMiddleGrid1PieceTaxiBalance(Drivetrain drivetrain, Arm arm, WheeledGr
         List<Command> commands = getPathPlannerDriveCommandGroup(drivetrain, "MiddleGrid1PieceTaxiBalance", new PathConstraints(1, 1), false);
         addCommands(
 //                new AutoGridScore(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 new RetractAndPivotCommand(arm, Rotation2d.fromDegrees(45)),
                 commands.get(0),
                 Commands.parallel(

src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid1PieceBalance.java

@@ -17,7 +17,7 @@ public class AutoBottomGrid1PieceBalance extends AutoCommand {
     public AutoBottomGrid1PieceBalance(Drivetrain drivetrain, Arm arm, WheeledGrabber intake) {
         addCommands(
 //                new AutoGridScore(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new RetractAndPivotCommand(arm, Rotation2d.fromDegrees(-45)),
                         getPathPlannerDriveCommand(drivetrain, "BottomGrid1PieceBalance", new PathConstraints(4, 2), false)

src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2PieceTaxi.java

@@ -25,7 +25,7 @@ public AutoBottomGrid2PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber i
                 new PathConstraints(4,2)
         }, false);
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2PieceTaxiVision.java

@@ -31,7 +31,7 @@ public AutoBottomGrid2PieceTaxiVision(Drivetrain drivetrain, Arm arm, WheeledGra
 
         addCommands(
                 new InstantCommand(() -> limelight.setPipeline(Limelight.Pipeline.GAME_PIECE)),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxi.java

@@ -26,7 +26,7 @@ public AutoBottomGrid2_5PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber
         }, false);
         setName("BottomGrid2.5PieceTaxi");
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake,null,false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/top/AutoTopGrid1PieceBalance.java

@@ -17,7 +17,7 @@ public class AutoTopGrid1PieceBalance extends AutoCommand {
     public AutoTopGrid1PieceBalance(Drivetrain drivetrain, Arm arm, WheeledGrabber intake) {
         addCommands(
 //                new AutoGridScore(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake),
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new RetractAndPivotCommand(arm, Rotation2d.fromDegrees(-45)),
                         getPathPlannerDriveCommand(drivetrain, "TopGrid1PieceBalance", new PathConstraints(4, 2), false)

src/main/java/org/team1540/robot2023/commands/auto/sequence/top/AutoTopGrid2PieceTaxi.java

@@ -22,7 +22,7 @@ public AutoTopGrid2PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber inta
         List<Command> pathCommands = getPathPlannerDriveCommandGroup(drivetrain, "TopGrid2PieceTaxi");
         addCommands(
 
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/top/AutoTopGrid2PieceVision.java

@@ -27,7 +27,7 @@ public AutoTopGrid2PieceVision(Drivetrain drivetrain, Arm arm, WheeledGrabber in
         limelight.setPipeline(Limelight.Pipeline.GAME_PIECE);
         addCommands(
 
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/top/AutoTopGrid2_5PieceTaxi.java

@@ -22,7 +22,7 @@ public AutoTopGrid2_5PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber in
         List<Command> pathCommands = getPathPlannerDriveCommandGroup(drivetrain, "TopGrid3PieceTaxi");
         setName("TopGrid2.5PieceTaxi");
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/auto/sequence/top/AutoTopGrid3PieceTaxi.java

@@ -26,7 +26,7 @@ public class AutoTopGrid3PieceTaxi extends AutoCommand {
     public AutoTopGrid3PieceTaxi(Drivetrain drivetrain, Arm arm, WheeledGrabber intake) {
         List<Command> pathCommands = getPathPlannerDriveCommandGroup(drivetrain, "TopGrid3PieceTaxi", new PathConstraints(5, 3), false);
         addCommands(
-                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, false),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(

src/main/java/org/team1540/robot2023/commands/drivetrain/Drivetrain.java

@@ -120,6 +120,18 @@ public boolean updateWithScoringApriltags() {
         return LimelightManager.getInstance().applyFrontEstimates(poseEstimator, getYaw(), getModulePositions());
     }
 
+    public Command updateOdometryAnd(Command cmd) {
+        return new ParallelCommandGroup(
+                new InstantCommand(() -> {
+                    boolean isGood = this.updateWithScoringApriltags();
+                    if (isGood) {
+                        this.zeroFieldOrientation();
+                    }
+                }),
+                cmd
+        );
+    }
+
 
     public void resetAllToAbsolute() {
         DataLogManager.log("Zeroing swerve module relative encoders");

src/main/java/org/team1540/robot2023/utils/swerve/ModuleMagnetOffset.java

@@ -7,13 +7,13 @@
 public class ModuleMagnetOffset {
     private static final double[] offsets = new double[]{
             75.469, // Module 1
-            91.318,  // Module 2
-            256.729, // Module 3
+            91.318 - 90,  // Module 2
+            256.729 + 3, // Module 3
             9.0,     // Module 4
             239.766, // Module 5
             32.08,  // Module 6
             27.861, // Module 7
-            105.011  // Module 8
+            105.011 + 3  // Module 8
     };
 
     /**