Merge branch 'main' of https://github.com/frcteam3044/2025swervebase

FRCTeam3044 · Feb 15, 2025 · 8a3d3a6 · 8a3d3a6
2 parents 65a9d7a + 32d0f09
commit 8a3d3a6
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 11 deletions.
diff --git a/simgui.json b/simgui.json
@@ -73,6 +73,9 @@
           }
         }
       },
+      "LiveWindow": {
+        "open": true
+      },
       "SmartDashboard": {
         "Field": {
           "double[]##v_/SmartDashboard/Field/notes": {
@@ -97,8 +100,5 @@
       "open": false
     },
     "visible": true
-  },
-  "NetworkTables View": {
-    "visible": false
   }
 }
diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java
@@ -49,7 +49,8 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                 this.registerToRootState(test, auto, teleop, disabled);
 
                 // Teleop
-                ManualState manual = new ManualState(this, driverController, operatorController, drive, elevator, LEDs);
+                ManualState manual = new ManualState(this, driverController, operatorController, drive, elevator,
+                                shoulder, endEffector, LEDs);
                 ScoreGamePiece scoreGamePiece = new ScoreGamePiece(this);
                 ScoreCoral scoreCoral = new ScoreCoral(this, buttonBoard, drive, endEffector, elevator, shoulder, LEDs);
                 ScoreAlgae scoreAlgae = new ScoreAlgae(this);
@@ -84,8 +85,8 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                 goToIntake.withChild(goToReefIntake, buttonBoard::getAlgaeMode, 0, "Reef selected")
                                 .withChild(goToStationIntake, () -> !buttonBoard.getAlgaeMode(), 1, "Station selected");
 
-                intakeGamePiece.withChild(intakeCoral, buttonBoard::getAlgaeMode, 0, "Reef selected")
-                                .withChild(intakeAlgae, () -> !buttonBoard.getAlgaeMode(), 1, "Station selected");
+                intakeGamePiece.withChild(intakeCoral, () -> !buttonBoard.getAlgaeMode(), 0, "Reef selected")
+                                .withChild(intakeAlgae, buttonBoard::getAlgaeMode, 1, "Station selected");
 
                 // Specific Algae intake and score
                 goToScoreAlgae.withChild(goToScoreNet, () -> !buttonBoard

diff --git a/src/main/java/frc/robot/statemachine/states/tele/IntakeCoral.java b/src/main/java/frc/robot/statemachine/states/tele/IntakeCoral.java
@@ -14,9 +14,9 @@ public IntakeCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, D
             EndEffector endEffector, LEDs LEDs) {
         super(stateMachine);
 
-        // TODO: Get others to make heights and (Marcus) rotation
         startWhenActive(DriveCommands.pointControl(drive, buttonBoard::getIntakeStationTarget));
         startWhenActive(endEffector.runIntake());
+        // TODO: Elevator/Shoulder
         startWhenActive(LEDs.intakingAndScoringCoral());
     }
 }
diff --git a/src/main/java/frc/robot/statemachine/states/tele/ManualState.java b/src/main/java/frc/robot/statemachine/states/tele/ManualState.java
@@ -2,6 +2,7 @@
 
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -11,16 +12,18 @@
 import frc.robot.statemachine.reusable.SmartXboxController;
 import frc.robot.statemachine.reusable.State;
 import frc.robot.statemachine.reusable.StateMachineBase;
+import frc.robot.subsystems.EndEffector.EndEffector;
 import frc.robot.subsystems.LEDs.LEDs;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveCommands;
 import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.shoulder.Shoulder;
 import frc.robot.util.AllianceUtil;
 
 public class ManualState extends State {
         @SuppressWarnings("unused")
         public ManualState(StateMachineBase stateMachine, CommandXboxController driver, CommandXboxController operator,
-                        Drive drive, Elevator elevator, LEDs LEDs) {
+                        Drive drive, Elevator elevator, Shoulder shoulder, EndEffector endEffector, LEDs LEDs) {
                 super(stateMachine);
 
                 SmartXboxController driverController = new SmartXboxController(driver, loop);
@@ -50,5 +53,15 @@ public ManualState(StateMachineBase stateMachine, CommandXboxController driver,
 
                 driverController.a().onTrue(
                                 DriveCommands.goToPointJoystickRot(drive, new Pose2d(3, 3, new Rotation2d()), rotVel));
+
+                operatorController.leftTrigger().onTrue(endEffector.runIntakeReverse());
+                operatorController.rightTrigger().onTrue(endEffector.runIntake());
+
+                DoubleSupplier rightY = () -> -MathUtil.applyDeadband(operatorController.getHID().getRightY(), 0.01);
+                DoubleSupplier leftY = () -> -MathUtil.applyDeadband(operatorController.getHID().getLeftY(), 0.05);
+
+                startWhenActive(LEDs.simMorseCode());
+                startWhenActive(elevator.elevatorMove(rightY));
+                startWhenActive(shoulder.manualPivot(leftY));
         }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java
@@ -214,8 +214,12 @@ public static Command goToPoint(Drive drive, Supplier<Pose2d> pose) {
 
     public static Command pointControl(Drive drive, Supplier<Pose2d> pose) {
         return Commands
-                .run(() -> drive.runVelocity(DriveConstants.pointController.calculate(drive.getPose(), pose.get(), 0,
-                        pose.get().getRotation())));
+                .run(() -> {
+                    drive.runVelocity(DriveConstants.pointController.calculate(drive.getPose(), pose.get(), 0,
+                            pose.get().getRotation()));
+
+                    System.out.println(pose.get().getY());
+                }).withName("Point Control");
     }
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -131,7 +131,7 @@ public class DriveConstants {
                         .withSwerveModule(COTS.ofMAXSwerve(driveGearbox, turnGearbox, wheelCOF, 2));
 
         public static final Pathfinder pathfinder = (new PathfinderBuilder(Field.REEFSCAPE_2025))
-                        .setRobotLength(mapleBumperSize.in(Meters) + 0.05)
+                        .setNormalizeCorners(false).setRobotLength(mapleBumperSize.in(Meters) + 0.05)
                         .setRobotWidth(mapleBumperSize.in(Meters) + 0.05).build();
 
         public static final PIDController xController = new ConfigurablePIDController(1, 0, 0,