autonomous and gyro

TeamParadise · May 21, 2024 · 504a14a · 504a14a
1 parent 441f7a0
commit 504a14a
Show file tree

Hide file tree

Showing 15 changed files with 222 additions and 94 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": false,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
diff --git a/networktables.json b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/...eploy/pathplanner/autos/Example Auto.auto → ...ploy/pathplanner/autos/Defense Right.auto b/...eploy/pathplanner/autos/Example Auto.auto → ...ploy/pathplanner/autos/Defense Right.auto
@@ -2,25 +2,19 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2.0,
-      "y": 7.0
+      "x": 0.52,
+      "y": 2.1
     },
-    "rotation": -25.0
+    "rotation": 0
   },
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "named",
-          "data": {
-            "name": "Run Flywheel"
-          }
-        },
         {
           "type": "path",
           "data": {
-            "pathName": "Example Path"
+            "pathName": "Defense Right"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/autos/Leave Right.auto b/src/main/deploy/pathplanner/autos/Leave Right.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.52,
+      "y": 2.1
+    },
+    "rotation": 0.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Leave Right"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/None.auto b/src/main/deploy/pathplanner/autos/None.auto
@@ -0,0 +1,18 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.53,
+      "y": 2.1
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": []
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/...eploy/pathplanner/paths/Example Path.path → ...ploy/pathplanner/paths/Defense Right.path b/...eploy/pathplanner/paths/Example Path.path → ...ploy/pathplanner/paths/Defense Right.path
@@ -3,41 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 0.52,
+        "y": 2.1
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.013282910175676,
-        "y": 6.5274984191074985
+        "x": 1.519999999999999,
+        "y": 2.1
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.166769560390973,
-        "y": 5.0964860911203305
+        "x": 8.284942222836145,
+        "y": 1.5713680085678599
       },
       "prevControl": {
-        "x": 4.166769560390973,
-        "y": 6.0964860911203305
-      },
-      "nextControl": {
-        "x": 6.166769560390973,
-        "y": 4.0964860911203305
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.0,
-        "y": 1.0
-      },
-      "prevControl": {
-        "x": 6.75,
-        "y": 2.5
+        "x": 7.284942222836145,
+        "y": 1.5713680085678599
       },
       "nextControl": null,
       "isLocked": false,
@@ -60,6 +44,9 @@
   },
   "reversed": false,
   "folder": null,
-  "previewStartingState": null,
-  "useDefaultConstraints": false
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/Leave Right.path b/src/main/deploy/pathplanner/paths/Leave Right.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.52,
+        "y": 2.100052572198168
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.5200000000000005,
+        "y": 2.100052572198168
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.747066385009336,
+        "y": 0.4993131989841808
+      },
+      "prevControl": {
+        "x": 0.7071301441350659,
+        "y": 2.1441096191673616
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,8 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+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.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -40,6 +42,7 @@ public class RobotContainer {
 
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser;
+  private final SendableChooser<Command> realAutoChooser;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -61,7 +64,7 @@ public RobotContainer() {
     // }
 
     // Set up auto routines
-    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+    autoChooser = new LoggedDashboardChooser<>("SysID", AutoBuilder.buildAutoChooser());
 
     // Set up SysId routines
     autoChooser.addOption(
@@ -75,6 +78,9 @@ public RobotContainer() {
     autoChooser.addOption(
         "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
+    realAutoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto chooser", realAutoChooser);
+
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -98,6 +104,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return autoChooser.get();
+    return realAutoChooser.getSelected();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -39,7 +39,7 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Drive extends SubsystemBase {
-  public static final double WHEEL_RADIUS = Units.inchesToMeters(3.0);
+  public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
   public static final double TRACK_WIDTH = Units.inchesToMeters(26.0);
 
   // TODO: NON-SIM FEEDFORWARD GAINS MUST BE TUNED

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java
@@ -13,16 +13,14 @@
 
 package frc.robot.subsystems.drive;
 
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.Pigeon2Configuration;
-import com.ctre.phoenix6.hardware.Pigeon2;
 import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.SPI;
 
 /**
  * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
@@ -41,9 +39,7 @@ public class DriveIOSparkMax implements DriveIO {
   private final RelativeEncoder rightEncoder = rightLeader.getEncoder();
   private final SparkPIDController leftPID = leftLeader.getPIDController();
   private final SparkPIDController rightPID = rightLeader.getPIDController();
-
-  private final Pigeon2 pigeon = new Pigeon2(20);
-  private final StatusSignal<Double> yaw = pigeon.getYaw();
+  private final AHRS gyro = new AHRS(SPI.Port.kMXP);
 
   public DriveIOSparkMax() {
     leftLeader.restoreFactoryDefaults();
@@ -76,10 +72,7 @@ public DriveIOSparkMax() {
     leftFollower.burnFlash();
     rightFollower.burnFlash();
 
-    pigeon.getConfigurator().apply(new Pigeon2Configuration());
-    pigeon.getConfigurator().setYaw(0.0);
-    yaw.setUpdateFrequency(100.0);
-    pigeon.optimizeBusUtilization();
+    gyro.reset();
   }
 
   @Override
@@ -98,7 +91,7 @@ public void updateInputs(DriveIOInputs inputs) {
     inputs.rightCurrentAmps =
         new double[] {rightLeader.getOutputCurrent(), rightFollower.getOutputCurrent()};
 
-    inputs.gyroYaw = Rotation2d.fromDegrees(yaw.refresh().getValueAsDouble());
+    inputs.gyroYaw = gyro.getRotation2d();
   }
 
   @Override

diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+    "fileName": "NavX.json",
+    "name": "NavX",
+    "version": "2024.1.0",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2024/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-java",
+            "version": "2024.1.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-cpp",
+            "version": "2024.1.0",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "PathplannerLib.json",
   "name": "PathplannerLib",
-  "version": "2024.2.3",
+  "version": "2024.2.8",
   "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
   "frcYear": "2024",
   "mavenUrls": [
@@ -12,15 +12,15 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-java",
-      "version": "2024.2.3"
+      "version": "2024.2.8"
     }
   ],
   "jniDependencies": [],
   "cppDependencies": [
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-cpp",
-      "version": "2024.2.3",
+      "version": "2024.2.8",
       "libName": "PathplannerLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,