Added Slink AScope Files

.vscode/settings.json

@@ -36,6 +36,6 @@
     "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
   },
   "[java]": {
-    "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
+    "editor.defaultFormatter": "mwpb.java-prettier-formatter"
   }
 }

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

@@ -22,7 +22,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */

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

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
@@ -34,6 +35,7 @@
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import frc.robot.subsystems.sim.Sim;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -47,6 +49,7 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
   private final Flywheel flywheel;
+  private final Sim sim;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -69,6 +72,7 @@ public RobotContainer() {
                 new ModuleIOSparkMax(2),
                 new ModuleIOSparkMax(3));
         flywheel = new Flywheel(new FlywheelIOSparkMax());
+        sim = new Sim();
         // drive = new Drive(
         // new GyroIOPigeon2(true),
         // new ModuleIOTalonFX(0),
@@ -88,6 +92,7 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         flywheel = new Flywheel(new FlywheelIOSim());
+        sim = new Sim();
         break;
 
       default:
@@ -100,6 +105,7 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
         flywheel = new Flywheel(new FlywheelIO() {});
+        sim = new Sim();
         break;
     }
 
@@ -170,8 +176,37 @@ private void configureButtonBindings() {
     controller
         .a()
         .whileTrue(
-            Commands.startEnd(
-                () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+            new FunctionalCommand(
+                () -> {},
+                () -> {
+                  flywheel.runVelocity(controller.getRightTriggerAxis() * flywheelSpeedInput.get());
+                },
+                (v) -> {
+                  flywheel.stop();
+                },
+                () -> {
+                  return false;
+                },
+                flywheel));
+    controller
+        .y()
+        .whileTrue(
+            new FunctionalCommand(
+                () -> {},
+                () -> {
+                  sim.setIntakeAngle(controller.getRightTriggerAxis() * 90);
+                },
+                (v) -> {
+                  sim.setIntakeAngle(0);
+                },
+                () -> {
+                  return false;
+                },
+                sim));
+    // Commands.startEnd(
+    //     () -> flywheel.runVelocity(controller.getRightTriggerAxis() * flywheelSpeedInput.get()),
+    //     flywheel::stop,
+    //     flywheel));
   }
 
   /**

src/main/java/frc/robot/subsystems/sim/Sim.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.sim;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Sim extends SubsystemBase {
+  /** Creates a new Sim. */
+  public Sim() {}
+
+  double intakeAngle = 0;
+
+  @Override
+  public void periodic() {
+    Pose3d[] poses = {
+      new Pose3d(new Translation3d(), new Rotation3d(0, Units.degreesToRadians(intakeAngle), 0))
+    };
+    Logger.recordOutput("Poses", poses);
+    // This method will be called once per scheduler run
+  }
+
+  public void setIntakeAngle(double param) {
+    intakeAngle = param;
+  }
+}

src/main/java/frc/robot/util/userAssets/README.txt

@@ -0,0 +1,3 @@
+This folder contains extra assets for the odometry, 3D field, and joystick views. For more details, see the "Custom Fields/Robots/Joysticks" page in the AdvantageScope documentation (available through the documentation tab in the app or the URL below).
+
+https://github.com/Mechanical-Advantage/AdvantageScope/blob/main/docs/CUSTOM-ASSETS.md

src/main/java/frc/robot/util/userAssets/Robot_Slink/config.json

@@ -0,0 +1,25 @@
+{
+    "name": "Slink",
+    "rotations": [{"axis" : "x", "degrees" : 90}, {"axis" : "z", "degrees" : 180}], 
+    "position": [-0.3429, -0.3429, 0.1],
+    "cameras": [
+      {
+        "name": "Right Eye",
+        "rotations": [{ "axis": "y", "degrees": -25 }, { "axis": "z", "degrees": -25 }],
+        "position": [0.233, -0.288, 0.259],
+        "resolution": [1280, 720], 
+        "fov": 70 
+      },
+      {
+        "name": "Left Eye",
+        "rotations": [{ "axis": "y", "degrees": -25 }, { "axis": "z", "degrees": 25 }],
+        "position": [0.233, 0.288, 0.259],
+        "resolution": [1280, 720], 
+        "fov": 70 
+      }
+    ],
+    "components": [{
+      "zeroedRotations":[{"axis": "x", "degrees" : 90}],
+      "zeroedPositions" : [0,0,0]
+    }]
+  }