aimbot changes and auto and fixed note align

hammerhead226 · Apr 19, 2024 · ad32eba · ad32eba
1 parent ec76f8a
commit ad32eba
Show file tree

Hide file tree

Showing 16 changed files with 131 additions and 55 deletions.
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c4-c3.auto b/src/main/deploy/pathplanner/autos/$s!p-c4-c3.auto
@@ -5,7 +5,7 @@
       "x": 1.4194171612308972,
       "y": 4.063660281386183
     },
-    "rotation": -48.81407483474406
+    "rotation": -56.30993247402016
   },
   "command": {
     "type": "sequential",

diff --git a/...ner/autos/Copy of Copy of $s!p-c5-c4.auto → .../deploy/pathplanner/autos/$s!p-c5-c4.auto b/...ner/autos/Copy of Copy of $s!p-c5-c4.auto → .../deploy/pathplanner/autos/$s!p-c5-c4.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.548236363234494,
-      "y": 4.075371117931965
+      "x": 1.4311279977766789,
+      "y": 4.04023860829462
     },
-    "rotation": 0
+    "rotation": -42.27368900609368
   },
   "command": {
     "type": "sequential",

diff --git a/src/main/deploy/pathplanner/autos/Copy of $s!p-c5-c4.auto b/src/main/deploy/pathplanner/autos/Copy of $s!p-c5-c4.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.548236363234494,
-      "y": 4.075371117931965
+      "x": 1.4311279977766789,
+      "y": 4.04023860829462
     },
-    "rotation": 0
+    "rotation": -49.39870535499545
   },
   "command": {
     "type": "sequential",

diff --git a/src/main/deploy/pathplanner/paths/$s!p-c5.path b/src/main/deploy/pathplanner/paths/$s!p-c5.path
@@ -44,7 +44,7 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0.75,
+    "velocity": 0.25,
     "rotation": 0,
     "rotateFast": true
   },

diff --git a/src/main/deploy/pathplanner/paths/$s!score c4.path b/src/main/deploy/pathplanner/paths/$s!score c4.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.7661633639957715,
-        "y": 3.232190886635696
+        "x": 2.71,
+        "y": 3.23
       },
       "prevControl": {
-        "x": 4.218307095672678,
-        "y": 2.014263885874418
+        "x": 4.162143731676909,
+        "y": 2.012072999238722
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/$spc(53)!c3.path b/src/main/deploy/pathplanner/paths/$spc(53)!c3.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.578789979263267,
-        "y": 3.0799500115405367
+        "x": 2.58,
+        "y": 3.23
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.8786928358450146,
-        "y": 2.3187456360647385
+        "x": 4.0777770571233,
+        "y": 2.9979741557200663
       },
       "isLocked": false,
       "linkedName": "$spc(53)score c5 "

diff --git a/src/main/deploy/pathplanner/paths/$spc(53)!p-c5.path b/src/main/deploy/pathplanner/paths/$spc(53)!p-c5.path
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.796716980026477,
-        "y": 2.3772998187926775
+        "x": 3.632765268383603,
+        "y": 2.131372251332233
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.572185241374762,
-        "y": 0.8431802312962678
+        "x": 6.91,
+        "y": 0.91
       },
       "prevControl": {
-        "x": 4.252375228097177,
-        "y": 0.8431802312962678
+        "x": 4.590189986722415,
+        "y": 0.91
       },
       "nextControl": null,
       "isLocked": false,
@@ -44,7 +44,7 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 1.25,
+    "velocity": 0.5,
     "rotation": 0,
     "rotateFast": true
   },

diff --git a/src/main/deploy/pathplanner/paths/$spc(53)!score c5.path b/src/main/deploy/pathplanner/paths/$spc(53)!score c5.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.578789979263267,
-        "y": 3.0799500115405367
+        "x": 2.58,
+        "y": 3.23
       },
       "prevControl": {
-        "x": 3.9304218825729036,
-        "y": 2.4207150845511247
+        "x": 3.9316319033096367,
+        "y": 2.570765073010588
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.3185985259274356,
+        "y": 5.6212015419751244
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.3374413054104268,
+        "y": 5.351852301422149
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6561869414751653,
+        "y": 4.637491272129477
+      },
+      "prevControl": {
+        "x": 2.7076091812668635,
+        "y": 5.04737055123183
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.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/deploy/pathplanner/paths/w$s!p-c5.path b/src/main/deploy/pathplanner/paths/w$s!p-c5.path
@@ -44,7 +44,7 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0.75,
+    "velocity": 0.25,
     "rotation": 0,
     "rotateFast": true
   },

diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 256;
-  public static final String GIT_SHA = "27d4278b8e80b7cc41a7bb8b57cd59c2d074d585";
-  public static final String GIT_DATE = "2024-04-17 15:58:12 EDT";
+  public static final int GIT_REVISION = 257;
+  public static final String GIT_SHA = "ec76f8abd496ea67f7de44c5abb6729289479f21";
+  public static final String GIT_DATE = "2024-04-18 08:12:04 EDT";
   public static final String GIT_BRANCH = "worlds";
-  public static final String BUILD_DATE = "2024-04-17 18:47:16 EDT";
-  public static final long BUILD_UNIX_TIME = 1713394036698L;
+  public static final String BUILD_DATE = "2024-04-18 17:22:17 EDT";
+  public static final long BUILD_UNIX_TIME = 1713475337559L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -120,7 +120,7 @@ public static class PivotConstants {
     public static final double PODIUM_SETPOINT_DEG = 41.45;
     public static final double STOW_SETPOINT_DEG = 50.7;
     public static final double INTAKE_SETPOINT_DEG = 59.0;
-    public static final double AMP_SETPOINT_DEG = 60.0;
+    public static final double AMP_SETPOINT_DEG = 58.6;
     public static final double SUBWOOFER_SETPOINT_DEG = 62.0;
     public static final double REVERSE_SUBWOOFER_SETPOINT_DEG = 118;
 
@@ -130,7 +130,8 @@ public static class PivotConstants {
     public static final double CLIMB_SETPOINT_ONE_DEG = 50;
     public static final double CLIMB_SETPOINT_TWO_DEG = 95;
 
-    public static final double THRESHOLD = 1.5;
+    // public static final double THRESHOLD = 1.5;
+    public static final double THRESHOLD = 1;
 
     public static final double[] PID = {0, 0, 0};
 

diff --git a/src/main/java/frc/robot/commands/AimbotStatic.java b/src/main/java/frc/robot/commands/AimbotStatic.java
@@ -147,7 +147,7 @@ private double calculatePivotAngleDeg(double distanceToSpeakerMeter) {
     // pivotSetpointDeg = (-0.272 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) +
     // 60);
     pivotSetpointDeg =
-        (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 59.4);
+        (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 58.9);
     pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 34, 62);
 
     if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {

diff --git a/src/main/java/frc/robot/commands/AimbotTele.java b/src/main/java/frc/robot/commands/AimbotTele.java
@@ -110,8 +110,8 @@ public void angleShooter() {
       //         calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter)), 3250, 5400);
       double shootingSpeed = calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter));
 
-      shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed);
-    } else shooter.setFlywheelRPMs(5400, 5400);
+      shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed+100);
+    } else shooter.setFlywheelRPMs(5400+100, 5400+200);
     pivot.setPivotGoal(calculatePivotAngleDeg(distanceToSpeakerMeter));
   }
 
@@ -129,7 +129,7 @@ private double calculatePivotAngleDeg(double distanceToSpeakerMeter) {
     // pivotSetpointDeg = (-0.272 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) +
     // 60);
     pivotSetpointDeg =
-        (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 59.4);
+        (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 58.9);
     pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 34, 62);
 
     if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {

diff --git a/src/main/java/frc/robot/commands/ScoreAmp.java b/src/main/java/frc/robot/commands/ScoreAmp.java
@@ -22,7 +22,7 @@ public ScoreAmp(Elevator elevator, Pivot pivot, Shooter shooter, Drive drive) {
         new InstantCommand(() -> elevator.setConstraints(100, 640)),
         new InstantCommand(() -> shooter.setFlywheelRPMs(500, 500), shooter),
         new SetPivotTarget(Constants.PivotConstants.AMP_SETPOINT_DEG, pivot),
-        new SetElevatorTarget(7, 1, elevator));
+        new SetElevatorTarget(6, 1, elevator));
     // addCommands(
     //     new ParallelCommandGroup(
     //         // new AlignToAmp(drive),

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -508,37 +508,60 @@ public double getIntakeLLTy() {
   public Translation2d calculateNotePositionFieldRelative() {
 
     double distInch = (1 / (40 - ((30) * getIntakeLLTy() / 23)) * 1000); // Convert degrees to inch
-    double noteYawAngleDeg = -getIntakeLLTx() - 3; // account for static offset, reverse to be CCW+
-    double radiusInch = distInch / Math.cos(Units.degreesToRadians(noteYawAngleDeg));
+    double noteYawAngleDegCorrected =
+        -getIntakeLLTx() + 4; // account for static offset, reverse to be CCW+
+    double radiusInchCorrected =
+        distInch / Math.cos(Units.degreesToRadians(noteYawAngleDegCorrected));
+
+    double noteYawAngleDegRaw = -getIntakeLLTx(); // account for static offset, reverse to be CCW+
+    double radiusInchRaw = distInch / Math.cos(Units.degreesToRadians(noteYawAngleDegRaw));
+
     Logger.recordOutput("NoteTracking/distInch", distInch);
-    Logger.recordOutput("NoteTracking/noteYawAngleDeg", noteYawAngleDeg);
-    Logger.recordOutput("NoteTracking/radius", radiusInch);
+    Logger.recordOutput("NoteTracking/noteYawAngleDegCorrected", noteYawAngleDegCorrected);
+    Logger.recordOutput("NoteTracking/noteYawAngleDegRaw", noteYawAngleDegRaw);
+    Logger.recordOutput("NoteTracking/radiusCorrected", radiusInchCorrected);
 
     // camera relative -> bot relative -> field relative
-    Translation2d camRelNoteLocT2d =
+    Translation2d camRelNoteLocT2dCorrected =
+        new Translation2d(
+            Units.inchesToMeters(radiusInchCorrected), Rotation2d.fromDegrees(noteYawAngleDegRaw));
+    Logger.recordOutput("NoteTracking/camRelNoteLocT2dCorrected", camRelNoteLocT2dCorrected);
+
+    Translation2d camRelNoteLocT2dRaw =
         new Translation2d(
-            Units.inchesToMeters(radiusInch), Rotation2d.fromDegrees(noteYawAngleDeg));
-    Logger.recordOutput("NoteTracking/camRelNoteLocT2d", camRelNoteLocT2d);
+            Units.inchesToMeters(radiusInchRaw), Rotation2d.fromDegrees(noteYawAngleDegRaw));
 
-    Translation2d roboRelNoteLocT2d =
-        camRelNoteLocT2d
+    Translation2d roboRelNoteLocT2dRaw =
+        camRelNoteLocT2dRaw
             .rotateBy(Rotation2d.fromDegrees(0))
             .plus(new Translation2d(Units.inchesToMeters(12), 0));
-    Logger.recordOutput("NoteTracking/roboRelNoteLocT2d", roboRelNoteLocT2d);
+
+    Translation2d roboRelNoteLocT2dCorrected =
+        camRelNoteLocT2dCorrected
+            .rotateBy(Rotation2d.fromDegrees(0))
+            .plus(new Translation2d(Units.inchesToMeters(12), 0));
+    Logger.recordOutput("NoteTracking/roboRelNoteLocT2dCorrected", roboRelNoteLocT2dCorrected);
     Pose2d pickedRobotPose =
         posePicker(
             Timer.getFPGATimestamp()
                 - LimelightHelpers.getLatency_Pipeline(Constants.LL_INTAKE)
                 - LimelightHelpers.getLatency_Capture(Constants.LL_INTAKE));
-    Translation2d fieldRelNoteLocT2d =
-        roboRelNoteLocT2d
+    Translation2d fieldRelNoteLocT2dCorrected =
+        roboRelNoteLocT2dCorrected
             .rotateBy(pickedRobotPose.getRotation())
             .plus(pickedRobotPose.getTranslation());
-    Logger.recordOutput("NoteTracking/fieldRelNoteLocT2d", fieldRelNoteLocT2d);
+
+    Translation2d fieldRelNoteLocT2dRaw =
+        roboRelNoteLocT2dRaw
+            .rotateBy(pickedRobotPose.getRotation())
+            .plus(pickedRobotPose.getTranslation());
+
+    Logger.recordOutput("NoteTracking/fieldRelNoteLocT2dRaw", fieldRelNoteLocT2dRaw);
+    Logger.recordOutput("NoteTracking/fieldRelNoteLocT2dCorrected", fieldRelNoteLocT2dCorrected);
     Logger.recordOutput(
         "distance from center of robot",
-        Units.metersToInches(fieldRelNoteLocT2d.getDistance(getPose().getTranslation())));
-    return fieldRelNoteLocT2d;
+        Units.metersToInches(fieldRelNoteLocT2dCorrected.getDistance(getPose().getTranslation())));
+    return fieldRelNoteLocT2dCorrected;
   }
 
   public Translation2d getCachedNoteLocation() {