Worlds

src/main/deploy/pathplanner/autos/5mForwardRotate180.auto

@@ -2,7 +2,7 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 0.4,
+      "x": 2.3,
       "y": 7.0
     },
     "rotation": 0
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "5mForwardRotate"
+            "pathName": "bonkRotateForward"
           }
         }
       ]

src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto

@@ -11,12 +11,6 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "named",
-          "data": {
-            "name": "SubwooferLaunch"
-          }
-        },
         {
           "type": "path",
           "data": {

src/main/deploy/pathplanner/autos/TuneTranslationPID back.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 6.3,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TuneRotationPID back"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Test",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/VisionAmpSideFarAutoline3Score.auto

@@ -23,18 +23,6 @@
             "pathName": "HighSpeakerToHighNoteLaunch2"
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "VisionLaunch"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch"
-          }
-        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path

@@ -31,7 +31,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 0.35,
-      "rotationDegrees": -169.16565991676583,
+      "rotationDegrees": -167.0,
       "rotateFast": false
     },
     {

src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path

@@ -71,6 +71,12 @@
               "data": {
                 "name": "Intake"
               }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "RetractPivot"
+              }
             }
           ]
         }

src/main/deploy/pathplanner/paths/TuneRotationPID back.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.3,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.3,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.218814272122443,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/TuneTranslationalPID.path

@@ -32,7 +32,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 2.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
@@ -48,5 +48,5 @@
     "rotation": 0,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/bonkRotateForward.path

@@ -3,24 +3,40 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.4,
+        "x": 2.3,
         "y": 7.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.4,
+        "x": 2.1999999999999997,
         "y": 7.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.5,
+        "x": 5.335023775460655,
         "y": 7.0
       },
       "prevControl": {
-        "x": 4.5,
+        "x": 4.835024537004013,
+        "y": 6.999127335817051
+      },
+      "nextControl": {
+        "x": 5.835023013917296,
+        "y": 7.000872664182949
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 2.5999999999999996,
         "y": 7.0
       },
       "nextControl": null,
@@ -30,16 +46,21 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.5,
-      "rotationDegrees": 45.0,
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 1.8407261107289572,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 0.55,
+      "rotationDegrees": 89.63503006133968,
       "rotateFast": false
     }
   ],
   "constraintZones": [],
   "eventMarkers": [
     {
       "name": "New Event Marker",
-      "waypointRelativePos": 0.25,
+      "waypointRelativePos": 0.5,
       "command": {
         "type": "parallel",
         "data": {

src/main/deploy/swerve/controllerproperties.json

@@ -1,8 +1,8 @@
 {
   "angleJoystickRadiusDeadband": 0.5,
   "heading": {
-    "p": 0.4,
+    "p": 0.25,
     "i": 0,
-    "d": 0.15
+    "d": 0
   }
 }

src/main/deploy/swerve/modules/physicalproperties.json

@@ -11,7 +11,7 @@
   },
   "conversionFactor": {
     "angle": 19.2,
-    "drive": 0.0696793458626792
+    "drive": 0.06681204078043
   },
   "rampRate": {
     "drive": 0.25,

src/main/java/frc/team2412/robot/Controls.java

@@ -30,6 +30,7 @@
 import frc.team2412.robot.commands.launcher.SetPivotCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
 import frc.team2412.robot.util.AmpAlign;
+import frc.team2412.robot.util.TrapAlign;
 
 public class Controls {
 	public static class ControlConstants {
@@ -58,7 +59,7 @@ public static class ControlConstants {
 	private final Trigger launcherSubwooferPresetButton;
 	private final Trigger launcherLowerPresetButton;
 	// private final Trigger launcherPodiumPresetButton;
-	// private final Trigger launcherTrapPresetButton;
+	private final Trigger launcherTrapPresetButton;
 	private final Trigger launcherAmpAlignPresetButton;
 	private final Trigger launcherLaunchButton;
 
@@ -76,7 +77,7 @@ public Controls(Subsystems s) {
 		launcherSubwooferPresetButton = codriveController.a();
 		launcherLowerPresetButton = codriveController.y();
 		// launcherPodiumPresetButton = codriveController.povLeft();
-		// launcherTrapPresetButton = codriveController.start();
+		launcherTrapPresetButton = codriveController.start();
 		launcherAmpAlignPresetButton = driveController.y();
 		launcherLaunchButton = codriveController.rightBumper();
 		// intake controls (confirmed with driveteam)
@@ -228,8 +229,8 @@ private void bindLauncherControls() {
 						LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
 						LauncherSubsystem.AMP_AIM_ANGLE));
 
-		// launcherTrapPresetButton.onTrue(
-		// 		TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
+		launcherTrapPresetButton.whileTrue(
+				TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
 		launcherAmpAlignPresetButton.onTrue(
 				Commands.either(
 						AmpAlign.ampPreset(s.drivebaseSubsystem, s.launcherSubsystem),

src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java

@@ -78,7 +78,8 @@ public class DrivebaseSubsystem extends SubsystemBase {
 							: Robot.getInstance().getRobotType() == RobotType.CRANE
 									? new PIDConstants(3.9, 0, 0.2) // crane
 									: new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune
-	private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 0);
+	private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 1);
+	// 7 0 0.2
 	private static final double MAX_AUTO_SPEED =
 			500.0; // this seems to only affect rotation for some reason
 

src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java

@@ -38,9 +38,9 @@ public class LauncherSubsystem extends SubsystemBase {
 	// HARDWARE
 	private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0;
 	private static final double PIVOT_TO_ENCODER_GEARING_RATIO = 1.0 / 2.0;
-	private static final float PIVOT_SOFTSTOP_FORWARD = 0.93f;
+	private static final float PIVOT_SOFTSTOP_FORWARD = 0.99f;
 	private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f;
-	private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.93f;
+	private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.99f;
 	private static final float PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE = 0.38f;
 	private static final float PIVOT_DISABLE_OFFSET = 0.04f;
 	private static final int PIVOT_OFFSET = USE_THROUGHBORE ? 40 : 36;
@@ -51,9 +51,9 @@ public class LauncherSubsystem extends SubsystemBase {
 
 	// ANGLE VALUES
 	public static final int AMP_AIM_ANGLE = 290 + PIVOT_OFFSET;
-	public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET;
+	public static final double SUBWOOFER_AIM_ANGLE = 253.5 + PIVOT_OFFSET;
 	public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET;
-	public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET;
+	public static final double TRAP_AIM_ANGLE = 311.879 + PIVOT_OFFSET;
 	public static final double MANUAL_MODIFIER = 0.02;
 	public static final double RETRACTED_ANGLE = 242 + PIVOT_OFFSET;
 	// offset for FF so parallel to floor is 0
@@ -65,7 +65,7 @@ public class LauncherSubsystem extends SubsystemBase {
 	public static final double ANGLE_TOLERANCE = 5;
 	public static final double RPM_TOLERANCE = 500;
 	// RPM
-	public static final int SPEAKER_SHOOT_SPEED_RPM = 4500;
+	public static final int SPEAKER_SHOOT_SPEED_RPM = 3800;
 	public static final int TRAP_SHOOT_SPEED_RPM = 4500;
 	public static final int LOBBING_RPM = 4700;
 	public static final double ANGLE_MAX_SPEED = 0.3; // percent output

src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java

@@ -13,6 +13,7 @@ public class LimelightSubsystem extends SubsystemBase {
 
 	// POTENTIAL CONSTANTS
 	private GenericEntry GOAL_DISTANCE_FROM_NOTE;
+	private final double AUTO_DETECTION_DISTANCE_FROM_NOTE = 3.5; // meters
 
 	// MEMBERS
 
@@ -69,6 +70,10 @@ private void setPipeline() {
 
 	// METHODS
 
+	public boolean isNoteInFront() {
+		return (hasTargets() && getDistanceFromTargetInches() <= AUTO_DETECTION_DISTANCE_FROM_NOTE);
+	}
+
 	public boolean hasTargets() {
 		return (networkTable.getEntry("tv").getDouble(0) != 0);
 	}

src/main/java/frc/team2412/robot/util/TrapAlign.java

@@ -30,7 +30,7 @@ public class TrapAlign {
 		// trap that faces amp
 		new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-120)),
 		// trap that faces source
-		new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(300)),
+		new Pose2d(new Translation2d(12.15, 3.45), Rotation2d.fromDegrees(120)),
 		// trap that faces mid
 		// DO THIS ONE FIRST
 		// brute force the X lol