update driving code

Team2470 · Feb 6, 2024 · 8633032 · 8633032
1 parent fc4e4c6
commit 8633032
Show file tree

Hide file tree

Showing 9 changed files with 298 additions and 63 deletions.
diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
@@ -2,5 +2,5 @@
     "enableCppIntellisense": false,
     "currentLanguage": "java",
     "projectYear": "2024",
-    "teamNumber": 3081
+    "teamNumber": 2470
 }
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2.0,
-      "y": 7.0
+      "x": 0.7,
+      "y": 6.7
     },
-    "rotation": -1.0
+    "rotation": 60.0
   },
   "command": {
     "type": "sequential",
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example Path"
+            "pathName": "Path1"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
diff --git a/src/main/deploy/pathplanner/paths/Path1.path b/src/main/deploy/pathplanner/paths/Path1.path
@@ -0,0 +1,161 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7,
+        "y": 6.7
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7164034718694439,
+        "y": 7.019690992506985
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9518202708280417,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 1.6549716576762825,
+        "y": 7.0
+      },
+      "nextControl": {
+        "x": 2.1717038325554348,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.766614212915926,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 2.6061789698808355,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0,
+      "rotationDegrees": 60.0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 0.0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "first shot",
+      "waypointRelativePos": 1.3,
+      "command": {
+        "type": "sequential",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "stop"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "shoot"
+              }
+            }
+          ]
+        }
+      }
+    },
+    {
+      "name": "pickup",
+      "waypointRelativePos": 1.55,
+      "command": {
+        "type": "sequential",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "stop"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "pickup"
+              }
+            }
+          ]
+        }
+      }
+    },
+    {
+      "name": "second shot",
+      "waypointRelativePos": 2.0,
+      "command": {
+        "type": "sequential",
+        "data": {
+          "commands": [
+            {
+              "type": "sequential",
+              "data": {
+                "commands": [
+                  {
+                    "type": "named",
+                    "data": {
+                      "name": "stop"
+                    }
+                  },
+                  {
+                    "type": "named",
+                    "data": {
+                      "name": "shoot"
+                    }
+                  }
+                ]
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "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": 60.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java b/src/main/java/com/swervedrivespecialties/swervelib/Mk4iSwerveModuleHelper.java
@@ -4,10 +4,7 @@
 import com.swervedrivespecialties.swervelib.rev.*;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 
-/**
- * @deprecated use {@link MkSwerveModuleBuilder} instead, which provides support for CANivores.
- */
-@Deprecated(since = "2023.1.2.0", forRemoval = true)
+
 public final class Mk4iSwerveModuleHelper {
     private Mk4iSwerveModuleHelper() {
     }
@@ -18,6 +15,12 @@ private static DriveControllerFactory<?, Integer> getFalcon500DriveFactory(Mk4Mo
                 .withCurrentLimit(configuration.getDriveCurrentLimit())
                 .build();
     }
+    private static DriveControllerFactory<?, Integer> getKrakenDriveFactory(Mk4ModuleConfiguration configuration) {
+        return new KrakenDriveControllerFactoryBuilder()
+                .withVoltageCompensation(configuration.getNominalVoltage())
+                .withCurrentLimit(configuration.getDriveCurrentLimit())
+                .build();
+    }
 
     private static SteerControllerFactory<?, SteerConfiguration<CanCoderAbsoluteConfiguration>> getFalcon500SteerFactory(Mk4ModuleConfiguration configuration) {
         return new Falcon500SteerControllerFactoryBuilder()
@@ -193,6 +196,28 @@ public static SwerveModule createNeo(
         );
     }
 
+        public static SwerveModule createKrakenNeo(
+                ShuffleboardLayout container,
+                Mk4ModuleConfiguration configuration,
+                GearRatio gearRatio,
+                int driveMotorPort,
+                int steerMotorPort,
+                int steerEncoderPort,
+                double steerOffset
+        ) {
+                return new SwerveModuleFactory<>(
+                        gearRatio.getConfiguration(),
+                        getKrakenDriveFactory(configuration),
+                        getNeoSteerFactory(configuration)
+                ).create(
+                        container,
+                        driveMotorPort,
+                        new SteerConfiguration<>(
+                                steerMotorPort,
+                                new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset)
+                        )
+                );
+        }
     /**
      * Creates a Mk4i swerve module that uses NEOs for driving and steering.
      * Module information is displayed in the specified ShuffleBoard container.

diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/CtreUtils.java
@@ -1,12 +1,20 @@
 package com.swervedrivespecialties.swervelib.ctre;
 
 import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix6.StatusCode;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 
 public final class CtreUtils {
     private CtreUtils() {}
 
+    public static void checkCtreError(StatusCode statusCode, String message) {
+        if (RobotBase.isReal() && statusCode != StatusCode.OK) {
+            DriverStation.reportError(String.format("%s: %s", message, statusCode.toString()), false);
+        }
+    }
+
     public static void checkCtreError(ErrorCode errorCode, String message) {
         if (RobotBase.isReal() && errorCode != ErrorCode.OK) {
             DriverStation.reportError(String.format("%s: %s", message, errorCode.toString()), false);