Merge pull request #3 from StuyPulse/ian

Fix CANCoder IDS and reorganize swerve module initialization

Author: IanShiii

src/main/deploy/pathplanner/paths/Example Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -1,6 +1,6 @@
 {
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
+  "robotWidth": 0.7366,
+  "robotLength": 0.7366,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
@@ -12,20 +12,20 @@
   "robotMass": 74.088,
   "robotMOI": 6.0,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 6.857,
   "maxDriveSpeed": 5.45,
-  "driveMotorType": "krakenX60",
+  "driveMotorType": "vortex",
   "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,
+  "flModuleX": 0.2804033,
+  "flModuleY": 0.28,
+  "frModuleX": 0.28,
+  "frModuleY": -0.28,
+  "blModuleX": -0.28,
+  "blModuleY": 0.28,
+  "brModuleX": -0.28,
+  "brModuleY": -0.28,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []

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

@@ -8,18 +8,24 @@
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Field.CoralBranch;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
-public class AutoPilot extends SequentialCommandGroup{
+public class AutoPilot extends SequentialCommandGroup {
 
     public AutoPilot() {
         ArrayList<Command> commands = new ArrayList<>();
         for (CoralBranch branch : CoralBranch.values()) {
             Command command = new SequentialCommandGroup(
+                SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch).transformBy(new Transform2d(-1.0, 0, new Rotation2d())))
+                    .onlyIf(() -> !branch.onDriverStationSide()),
                 SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch)),
                 new DropperShootSequence(),
+                SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch).transformBy(new Transform2d(-1.0, 0, new Rotation2d())))
+                    .onlyIf(() -> !branch.onDriverStationSide()),
                 SwervePathFind.toNearestCoralStation(),
                 new WaitCommand(Settings.Dropper.WAIT_TIME_AT_CORAL_STATION)
             );

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -2,6 +2,7 @@
 
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.streams.numbers.IStream;
 import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
 import com.stuypulse.stuylib.streams.vectors.VStream;
@@ -27,7 +28,7 @@ public class SwerveDriveDrive extends Command {
     public SwerveDriveDrive(Gamepad driver) {
         swerve = SwerveDrive.getInstance();
 
-        speed = VStream.create(driver::getLeftStick)
+        speed = VStream.create(this::getDriverInputAsVelocity)
             .filtered(
                 new VDeadZone(Drive.DEADBAND),
                 x -> x.clamp(1),
@@ -48,6 +49,10 @@ public SwerveDriveDrive(Gamepad driver) {
         addRequirements(swerve);
     }
 
+    private Vector2D getDriverInputAsVelocity() {
+        return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x);
+    }
+
     @Override
     public void execute() {
         swerve.drive(speed.get(), turn.get());

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAwayFromReef.java

@@ -0,0 +1,37 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.math.Vector2D;
+import com.stuypulse.stuylib.streams.numbers.IStream;
+import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
+import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.constants.Settings.Driver.Turn;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveDriveAwayFromReef extends Command {
+
+    private final SwerveDrive swerve;
+
+    public SwerveDriveDriveAwayFromReef() {
+        swerve = SwerveDrive.getInstance();
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void execute() {
+        Translation2d robot = Odometry.getInstance().getPose().getTranslation();
+        Translation2d reefToRobot = robot.minus(Field.REEF_CENTER);
+        Vector2D velocity = new Vector2D(reefToRobot).normalize().mul(1.5);
+        swerve.drive(velocity, 0);
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwervePIDToPose.java

@@ -7,8 +7,8 @@
 import com.stuypulse.robot.constants.Field;
 import com.stuypulse.robot.constants.Settings.Swerve;
 import com.stuypulse.robot.constants.Settings.Swerve.Alignment;
-import com.stuypulse.robot.constants.Settings.Swerve.Controllers.Chassis.Theta;
-import com.stuypulse.robot.constants.Settings.Swerve.Controllers.Chassis.XY;
+import com.stuypulse.robot.constants.Settings.Swerve.Alignment.Theta;
+import com.stuypulse.robot.constants.Settings.Swerve.Alignment.XY;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.util.HolonomicController;

src/main/java/com/stuypulse/robot/commands/swerve/SwervePathFind.java

@@ -3,6 +3,7 @@
 import java.util.function.Supplier;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
 import com.stuypulse.robot.constants.Field;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.odometry.Odometry;

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -130,6 +130,7 @@ public static boolean isValidAprilTagId(int id) {
 
     /*** REEF POSITIONS ***/
 
+    Translation2d REEF_CENTER = new Translation2d(Units.inchesToMeters(144.0 + 93.5 / 2), WIDTH / 2);
     double CENTER_OF_TROUGH_TO_BRANCH = Units.inchesToMeters(13.0/2.0);
 
     public enum CoralBranch {
@@ -159,6 +160,24 @@ public boolean isLeftPeg() {
                     return false;
             }
         }
+
+        /**  
+         * Represents the half of the reef facing the driver station.
+         * The other half is facing the barge / the opposite alliance
+        */
+        public boolean onDriverStationSide() {
+            switch (this) {
+                case A:
+                case B:
+                case C:
+                case D:
+                case L:
+                case K:
+                    return true;
+                default:
+                    return false;
+            }
+        }
     }
 
     public static Pose2d getTargetPoseForCoralBranch(CoralBranch position) { 

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -28,9 +28,9 @@ public interface Turn {
         }
         public interface CANCoderIds {
             int FRONT_LEFT = 1;
-            int FRONT_RIGHT = 2;
-            int BACK_LEFT = 3;
-            int BACK_RIGHT = 4;
+            int BACK_LEFT = 2;
+            int BACK_RIGHT = 3;
+            int FRONT_RIGHT = 4;
         }
     }