Merge pull request #14 from 5152Alotobots/AutonDevBranch

Auton dev branch merge - WORKING
5152Alotobots · Feb 22, 2023 · 79e008b · 79e008b
2 parents 1e3f6a2 + 100c71c
commit 79e008b
Show file tree

Hide file tree

Showing 22 changed files with 742 additions and 255 deletions.
diff --git a/src/main/deploy/pathplanner/Hurry.path b/src/main/deploy/pathplanner/Hurry.path
diff --git a/src/main/deploy/pathplanner/New New New Path.path b/src/main/deploy/pathplanner/New New New Path.path
diff --git a/src/main/deploy/pathplanner/New New Path.path b/src/main/deploy/pathplanner/New New Path.path
diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path
diff --git a/src/main/deploy/pathplanner/chargeblue.path b/src/main/deploy/pathplanner/chargeblue.path
@@ -0,0 +1,99 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 2.0,
+        "y": 0.7729961174208069
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.014314742915336,
+        "y": 0.8016256032442725
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.7,
+        "y": 0.8016256032512075
+      },
+      "prevControl": {
+        "x": 6.7,
+        "y": 0.8052042889800074
+      },
+      "nextControl": {
+        "x": 6.7,
+        "y": 0.7980469175224075
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": 4.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.695634819841068,
+        "y": 2.147211437280018
+      },
+      "prevControl": {
+        "x": 6.695634819841068,
+        "y": 2.1409487372546177
+      },
+      "nextControl": {
+        "x": 6.695634819841068,
+        "y": 2.1534741373054183
+      },
+      "holonomicAngle": -180.0,
+      "isReversal": false,
+      "velOverride": 10.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.9042599513770457,
+        "y": 2.15
+      },
+      "prevControl": {
+        "x": 3.9042599513770457,
+        "y": 2.1468686499872995
+      },
+      "nextControl": null,
+      "holonomicAngle": -180.0,
+      "isReversal": false,
+      "velOverride": 10.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}
diff --git a/src/main/java/frc/robot/ChargedUp/Arm/SubSys_Arm.java b/src/main/java/frc/robot/ChargedUp/Arm/SubSys_Arm.java
diff --git a/...ommands/Auto_YOUBETTERWORKORELSE_Cmd.java → ...argedUp/AutoCommands/Auto_Charge_Cmd.java b/...ommands/Auto_YOUBETTERWORKORELSE_Cmd.java → ...argedUp/AutoCommands/Auto_Charge_Cmd.java
@@ -4,21 +4,34 @@
 
 package frc.robot.ChargedUp.AutoCommands;
 
+import com.pathplanner.lib.PathPlannerTrajectory;
+
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.ChargedUp.ChargeStation.Cmd_AutoBalance;
 import frc.robot.Library.DriveTrains.SubSys_DriveTrain;
+import frc.robot.Library.DriveTrains.Cmds_SubSys_DriveTrain.Cmds_PathPlanner.Cmd_SubSys_DriveTrain_FollowPathPlanner_Traj;
 import frc.robot.Library.DriveTrains.SwerveDrive.*;
+import frc.robot.Library.Gyroscopes.Pigeon2.SubSys_PigeonGyro;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.htm
 
-public class Auto_YOUBETTERWORKORELSE_Cmd extends SequentialCommandGroup {
+public class Auto_Charge_Cmd extends SequentialCommandGroup {
   private final SubSys_DriveTrain m_DriveTrain;
+  private final SubSys_PigeonGyro m_pigeonGyro;
+
   /** Creates a new Auto_Challenge1_Cmd. */
-  public Auto_YOUBETTERWORKORELSE_Cmd(SubSys_DriveTrain driveSubSys) {
+  public Auto_Charge_Cmd(SubSys_DriveTrain driveSubSys, SubSys_PigeonGyro pigeonGyro) {
     m_DriveTrain = driveSubSys;
+    m_pigeonGyro = pigeonGyro;
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands();
+    addCommands(
+      new Cmd_SubSys_DriveTrain_FollowPathPlanner_Traj(driveSubSys, "chargeblue", true, true),
+      new Cmd_AutoBalance(m_pigeonGyro, m_DriveTrain)
+
+      );
+
   }
 }