Initial Robot Implementation

.Glass/glass.json

@@ -0,0 +1,5 @@
+{
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,36 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Alerts": "Alerts",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "open": true,
+        "testing": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/deploy/pathplanner/navgrid.json

@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.0,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,6 +5,7 @@
 
 package com.stuypulse.robot;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -14,6 +15,11 @@ public class Robot extends TimedRobot {
     private RobotContainer robot;
     private Command auto;
 
+    public static boolean isBlue() {
+        return DriverStation.getAlliance().isPresent()
+                && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue;
+    }
+
     /*************************/
     /*** ROBOT SCHEDULEING ***/
     /*************************/

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -1,18 +1,29 @@
-/************************ PROJECT PHIL ************************/
-/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/
-/* This work is licensed under the terms of the MIT license.  */
-/**************************************************************/
-
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.commands.Test;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.dropper.DropperDrop;
+import com.stuypulse.robot.commands.dropper.DropperStop;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
+import com.stuypulse.robot.commands.swerve.SwervePIDToPose;
+import com.stuypulse.robot.commands.swerve.SwervePathFind;
+import com.stuypulse.robot.constants.Field;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Field.CoralBranch;
+import com.stuypulse.robot.subsystems.dropper.Dropper;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.AprilTagVision;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RepeatCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 public class RobotContainer {
 
@@ -21,13 +32,18 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final SwerveDrive swerve = SwerveDrive.getInstance();
+    public final Dropper dropper = Dropper.getInstance();
+    public final AprilTagVision vision = AprilTagVision.getInstance();
+    public final Odometry odometry = Odometry.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
     // Robot container
 
     public RobotContainer() {
+        swerve.configureAutoBuilder();
         configureDefaultCommands();
         configureButtonBindings();
         configureAutons();
@@ -37,13 +53,23 @@ public RobotContainer() {
     /*** DEFAULTS ***/
     /****************/
 
-    private void configureDefaultCommands() {}
+    private void configureDefaultCommands() {
+        // swerve.setDefaultCommand(new Test());
+        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
+    }
 
     /***************/
     /*** BUTTONS ***/
     /***************/
 
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+        driver.getRightTriggerButton()
+            .onTrue(new DropperDrop())
+            .onFalse(new DropperStop());
+
+        driver.getRightButton()
+            .whileTrue(new SwervePIDToPose(() -> Field.getTargetPoseForCoralBranch(Field.getClosestBranch())));
+    }
 
     /**************/
     /*** AUTONS ***/

src/main/java/com/stuypulse/robot/commands/Test.java

@@ -0,0 +1,39 @@
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.commands.swerve.SwervePathFind;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.constants.Field.CoralBranch;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class Test extends SequentialCommandGroup{
+
+    public Test() {
+        addCommands(
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.A)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.B)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.C)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.D)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.E)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.F)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.G)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.H)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.I)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.J)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.K)),
+            SwervePathFind.toNearestCoralStation(),
+            SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(CoralBranch.L)),
+            SwervePathFind.toNearestCoralStation()
+        );
+    }
+}

src/main/java/com/stuypulse/robot/commands/dropper/DropperDrop.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.dropper;
+
+import com.stuypulse.robot.subsystems.dropper.Dropper;
+import com.stuypulse.robot.subsystems.dropper.Dropper.State;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class DropperDrop extends InstantCommand{
+
+    private Dropper dropper;
+
+    public DropperDrop() {
+        this.dropper = Dropper.getInstance();
+    }
+
+    @Override
+    public void initialize() {
+        dropper.setState(State.DROPPING);
+    }
+}

src/main/java/com/stuypulse/robot/commands/dropper/DropperShootSequence.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.dropper;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.dropper.Dropper;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+public class DropperShootSequence extends SequentialCommandGroup{
+    private final Dropper dropper;
+
+    public DropperShootSequence() {
+        dropper = Dropper.getInstance();
+        addCommands(
+            new DropperDrop(),
+            new WaitCommand(Settings.Dropper.DROP_TIME),
+            new DropperStop()
+        );
+    }
+}

src/main/java/com/stuypulse/robot/commands/dropper/DropperStop.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.dropper;
+
+import com.stuypulse.robot.subsystems.dropper.Dropper;
+import com.stuypulse.robot.subsystems.dropper.Dropper.State;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class DropperStop extends InstantCommand{
+
+    private Dropper dropper;
+
+    public DropperStop() {
+        this.dropper = Dropper.getInstance();
+    }
+
+    @Override
+    public void initialize() {
+        dropper.setState(State.STOP);
+    }
+}