Merge pull request #33 from OakvilleDynamics/event/heartland-regional

Heartland Regional Code
OakvilleDynamics · Mar 18, 2024 · 302bacd · 302bacd
2 parents 7bffbb3 + 29c1811
commit 302bacd
Show file tree

Hide file tree

Showing 12 changed files with 150 additions and 96 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
     id "com.diffplug.spotless" version "6.25.0"
     id "com.peterabeles.gversion" version "1.10.2"
 }

diff --git a/src/main/deploy/pathplanner/autos/BL Path.auto b/src/main/deploy/pathplanner/autos/BL Path.auto
@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BL Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/BR Path.auto b/src/main/deploy/pathplanner/autos/BR Path.auto
@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BR Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/RL Path.auto b/src/main/deploy/pathplanner/autos/RL Path.auto
@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RL Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/RR Path.auto b/src/main/deploy/pathplanner/autos/RR Path.auto
@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RR Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/BL Path.path b/src/main/deploy/pathplanner/paths/BL Path.path
@@ -3,115 +3,66 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.9768671894476314,
-        "y": 6.963795382379539
+        "x": 1.1057381983470564,
+        "y": 6.994425860528696
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.9957017800320328,
-        "y": 6.94664535958123
+        "x": 2.1057381983470567,
+        "y": 6.994425860528696
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.682149452840573,
-        "y": 7.12696370376103
+        "x": 2.86891037330523,
+        "y": 6.318561490543663
       },
       "prevControl": {
-        "x": 2.689912568446376,
-        "y": 7.126964631641936
+        "x": 1.86891037330523,
+        "y": 6.318561490543663
       },
       "nextControl": {
-        "x": 2.67438633723477,
-        "y": 7.1269627758801235
+        "x": 3.868910373305229,
+        "y": 6.318561490543663
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.289357896858155,
-        "y": 7.4351908948681915
+        "x": 5.175837629100388,
+        "y": 6.518552051996682
       },
       "prevControl": {
-        "x": 7.412776437653603,
-        "y": 7.323945067406474
+        "x": 5.711521937502425,
+        "y": 6.5084845171724846
       },
       "nextControl": {
-        "x": 8.322657686837363,
-        "y": 7.4394169284577
+        "x": 4.640153320698351,
+        "y": 6.52861958682088
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.682149452840573,
-        "y": 7.12696370376103
+        "x": 4.5393891577171175,
+        "y": 7.256243723540236
       },
       "prevControl": {
-        "x": 2.687481984411307,
-        "y": 7.110049826653473
+        "x": 5.3367389307207,
+        "y": 7.748303518762803
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.85,
-      "rotationDegrees": -89.4767832055039,
-      "rotateFast": false
-    },
-    {
-      "waypointRelativePos": 1.9,
-      "rotationDegrees": 0,
-      "rotateFast": false
-    }
-  ],
+  "rotationTargets": [],
   "constraintZones": [],
-  "eventMarkers": [
-    {
-      "name": "DUMP",
-      "waypointRelativePos": 0.95,
-      "command": {
-        "type": "parallel",
-        "data": {
-          "commands": [
-            {
-              "type": "wait",
-              "data": {
-                "waitTime": 10.0
-              }
-            }
-          ]
-        }
-      }
-    },
-    {
-      "name": "SUCK",
-      "waypointRelativePos": 2.0,
-      "command": {
-        "type": "parallel",
-        "data": {
-          "commands": []
-        }
-      }
-    },
-    {
-      "name": "DUMP",
-      "waypointRelativePos": 3.0,
-      "command": {
-        "type": "parallel",
-        "data": {
-          "commands": []
-        }
-      }
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
@@ -120,11 +71,14 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -89.6793498452957,
+    "rotation": 0,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
-  "previewStartingState": null,
-  "useDefaultConstraints": false
+  "previewStartingState": {
+    "rotation": -91.11576892507215,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -77,8 +77,8 @@ public static class MechanismConstants {
     public static final int INTAKE_MOTOR_FRONT = 52;
     public static final boolean INTAKE_MOTOR_SUSHI_INVERTED = true;
     public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false;
-    public static final double INTAKE_MOTOR_SPEED_FRONT = 0.2;
-    public static final double INTAKE_MOTOR_SPEED_SUSHI = 0.8;
+    public static final double INTAKE_MOTOR_SPEED_FRONT = 0.1;
+    public static final double INTAKE_MOTOR_SPEED_SUSHI = 1;
 
     // Conveyor Motors
     public static final int CONVEYOR_MOTOR_1 = 1;
@@ -92,8 +92,8 @@ public static class MechanismConstants {
     public static final int FLYWHEEL_MOTOR_2 = 4;
     public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true;
     public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false;
-    public static final double FLYWHEEL_MOTOR_FULL_SPEED = 1.0;
-    public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.40;
+    public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.51;
+    public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20;
     public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15;
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.Timer;
@@ -89,6 +90,8 @@ public void robotInit() {
           e.printStackTrace();
         }
       }
+      PortForwarder.add(5800, "photonvision-limelight", 5800);
+      PortForwarder.add(5800, "photonvision-rpi", 5800);
     }
 
     // Start logging

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -32,6 +34,7 @@
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -44,6 +47,7 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final SwerveSubsystem drivebase =
       new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
+
   // CommandJoystick rotationController = new CommandJoystick(1);
   // Replace with CommandPS4Controller or CommandJoystick if needed
   CommandJoystick driverController = new CommandJoystick(1);
@@ -58,6 +62,8 @@ public class RobotContainer {
   private final FlyWheel FlyWheel = new FlyWheel();
   private final Elevator elevator = new Elevator();
 
+  private final LoggedDashboardChooser<Command> autoChooser;
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
 
@@ -70,6 +76,15 @@ public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
 
+    autoChooser =
+        new LoggedDashboardChooser<>("Autonomous Chooser", AutoBuilder.buildAutoChooser());
+
+    autoChooser.addDefaultOption("Do nothing", null);
+    autoChooser.addOption("BL", new PathPlannerAuto("BL Path"));
+    autoChooser.addOption("BR", new PathPlannerAuto("BR Path"));
+    autoChooser.addOption("RL", new PathPlannerAuto("RR Path"));
+    autoChooser.addOption("RR", new PathPlannerAuto("RR Path"));
+
     final AbsoluteDriveAdv closedAbsoluteDriveAdv =
         new AbsoluteDriveAdv(
             drivebase,
@@ -187,8 +202,14 @@ private void configureBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
+    if (autoChooser.get() == null) {
+      doNothing();
+    }
     // An example command will be run in autonomous
-    return drivebase.getAutonomousCommand("New Path", true);
+    if (autoChooser.get() == null) {
+      return doNothing();
+    }
+    return drivebase.getAutonomousCommand(autoChooser.get().getName());
   }
 
   public void setDriveMode() {
@@ -198,4 +219,10 @@ public void setDriveMode() {
   public void setMotorBrake(boolean brake) {
     drivebase.setMotorBrake(brake);
   }
+
+  public Command doNothing() {
+    Command nothing = new InstantCommand();
+    nothing.setName("NOTHING");
+    return nothing;
+  }
 }
diff --git a/src/main/java/frc/robot/commands/FlyWCommand.java b/src/main/java/frc/robot/commands/FlyWCommand.java
@@ -30,9 +30,9 @@ public void execute() {
     } else if (FlyWJoystick.getPOV() == 225
         || FlyWJoystick.getPOV() == 180
         || FlyWJoystick.getPOV() == 135) {
-      m_FlyWSubsystem.enableflywheellow();
-    } else if (FlyWJoystick.getRawButton(12)) {
       m_FlyWSubsystem.enableflywheelreduced();
+    } else if (FlyWJoystick.getRawButton(12)) {
+      m_FlyWSubsystem.enableflywheellow();
     } else if (FlyWJoystick.getRawButton(11) || FlyWJoystick.getRawButton(7)) {
       m_FlyWSubsystem.reverseflywheel();
       System.out.println("Flywheels Moving in Reverse");