Merge branch 'main' into feature-kraken-drive

Mechanical-Advantage · Feb 17, 2024 · ee51b0b · ee51b0b
2 parents d1d8a31 + 19f6619
commit ee51b0b
Show file tree

Hide file tree

Showing 35 changed files with 908 additions and 391 deletions.
diff --git a/ascope_assets/Joystick_6328Console/config.json b/ascope_assets/Joystick_6328Console/config.json
@@ -0,0 +1,145 @@
+{
+  "name": "6328 Console",
+  "components": [
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        95,
+        215
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 1
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        265,
+        215
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 2
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        435,
+        215
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 3
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        560,
+        230
+      ],
+      "sizePx": [
+        80,
+        100
+      ],
+      "sourceIndex": 4
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        640,
+        230
+      ],
+      "sizePx": [
+        80,
+        100
+      ],
+      "sourceIndex": 5
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        1995,
+        210
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 8
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        2095,
+        210
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 9
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        2195,
+        210
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 10
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        2295,
+        210
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 11
+    },
+    {
+      "type": "button",
+      "isYellow": true,
+      "isEllipse": false,
+      "centerPx": [
+        2395,
+        210
+      ],
+      "sizePx": [
+        100,
+        160
+      ],
+      "sourceIndex": 12
+    }
+  ]
+}
diff --git a/ascope_assets/Joystick_6328Console/image.png b/ascope_assets/Joystick_6328Console/image.png
diff --git a/ascope_assets/Robot_MA24B/config.json b/ascope_assets/Robot_MA24B/config.json
@@ -0,0 +1,85 @@
+{
+  "name": "MA24B",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": 90
+    }
+  ],
+  "position": [
+    0,
+    0,
+    0
+  ],
+  "cameras": [
+    {
+      "name": "Northstar 0",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": 30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    },
+    {
+      "name": "Northstar 1",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": -30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        -0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    }
+  ],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": 90
+        }
+      ],
+      "zeroedPosition": [
+        0.238,
+        0.0,
+        -0.298
+      ]
+    }
+  ]
+}
diff --git a/ascope_assets/Robot_MA24B/model.glb b/ascope_assets/Robot_MA24B/model.glb
diff --git a/ascope_assets/Robot_MA24B/model_0.glb b/ascope_assets/Robot_MA24B/model_0.glb
diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java
@@ -20,7 +20,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final int loopPeriodMs = 20;
+  public static final double loopPeriodSecs = 0.02;
   private static RobotType robotType = RobotType.DEVBOT;
   public static final boolean tuningMode = true;
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -35,12 +35,15 @@
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC;
+import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO;
+import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC;
+import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOSim;
 import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
@@ -121,11 +124,18 @@ public RobotContainer() {
                   new ModuleIOSim(DriveConstants.moduleConfigs[2]),
                   new ModuleIOSim(DriveConstants.moduleConfigs[3]));
           flywheels = new Flywheels(new FlywheelsIOSim());
+
+          feeder = new Feeder(new FeederIOSim());
+          indexer = new Indexer(new IndexerIOSim());
+          intake = new Intake(new IntakeIOSim());
+          rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});
+
           arm = new Arm(new ArmIOSim());
         }
       }
     }
 
+    // No-op implementation for replay
     if (drive == null) {
       drive =
           new Drive(
@@ -200,20 +210,20 @@ private void configureButtonBindings() {
                 () ->
                     drive.setTeleopDriveGoal(
                         -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
-            .withName("DriveTeleop"));
+            .withName("Drive Teleop Input"));
+
+    // Aim and Rev Flywheels
     controller
         .a()
         .whileTrue(
             Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
-                .alongWith(superstructure.aimCommand(), flywheels.shootCommand()));
-
-    Trigger readyToShootTrigger =
+                .alongWith(superstructure.aim(), flywheels.shootCommand())
+                .withName("Aim"));
+    // Shoot
+    Trigger readyToShoot =
         new Trigger(
-            () ->
-                drive.isAutoAimGoalCompleted()
-                    && flywheels.atSetpoint()
-                    && superstructure.atArmSetpoint());
-    readyToShootTrigger
+            () -> drive.isAutoAimGoalCompleted() && flywheels.atGoal() && superstructure.atGoal());
+    readyToShoot
         .whileTrue(
             Commands.run(
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
@@ -222,26 +232,30 @@ private void configureButtonBindings() {
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
     controller
         .rightTrigger()
-        .and(readyToShootTrigger)
+        .and(readyToShoot)
         .onTrue(
-            rollers
-                .feedShooterCommand()
-                .withTimeout(1.0)
-                // Take over superstructure and flywheels, cancelling the main aiming command
-                .deadlineWith(superstructure.aimCommand(), flywheels.shootCommand()));
-
+            Commands.parallel(
+                    Commands.waitSeconds(0.5),
+                    Commands.waitUntil(controller.rightTrigger().negate()))
+                .deadlineWith(
+                    rollers.feedShooter(), superstructure.aim(), flywheels.shootCommand()));
+    // Intake Floor
     controller
         .leftTrigger()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand())));
+                .intake()
+                .alongWith(
+                    Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))
+                .withName("Floor Intake"));
+    // Eject Floor
     controller
         .leftBumper()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand())));
+                .intake()
+                .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
+                .withName("Eject To Floor"));
 
     controller
         .y()