mid

Signed-off-by: KP <[email protected]>
DurhamAcademy · Apr 6, 2024 · 5766f36 · 5766f36
1 parent bdc20bf
commit 5766f36
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 18 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -411,7 +411,7 @@ private void configureButtonBindings() {
                                         .withTimeout(4.0)
                         );
                 driverController
-                        .povUp()
+                        .rightBumper()
                         .whileTrue(
                                 sequence(
                                         FeederCommands.feedToShooter(feeder)

diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java
@@ -14,7 +14,7 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
-import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.*;
 import static edu.wpi.first.wpilibj.DriverStation.Alliance.Blue;
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 import static java.lang.Math.abs;
@@ -46,11 +46,12 @@ public static double inverseInterpolate(Double startValue, Double endValue, Doub
     }
 
     static LoggedDashboardNumber height = new LoggedDashboardNumber("Shooter Tweak Height", 2.13);
+    static LoggedDashboardNumber fartherIn = new LoggedDashboardNumber("Shoot Farther In", Inches.of(0).in(Meters));
 
     public static Pose3d getSpeakerPos() {
         return (DriverStation.getAlliance().orElse(Blue).equals(Blue)) ?
-                new Pose3d(0.24, 5.550, height.get(), new Rotation3d()) :
-                new Pose3d(16.27, 5.550, height.get(), new Rotation3d());
+                new Pose3d(0.24-fartherIn.get(), 5.50, height.get(), new Rotation3d()) :
+                new Pose3d(16.27+fartherIn.get(), 5.50, height.get(), new Rotation3d());
     }
 
     public static Pose3d getSourcePos() {
@@ -70,11 +71,12 @@ public static Command diagShot(Shooter shooter) {
     private static double getDistance(Pose3d pose3d) {
         return pose3d.toPose2d().getTranslation().getNorm();
     }
-
+//0.35 offset
     public static void construct() {
         distanceToAngle.clear();
         distanceToAngle.put(0.0, 0.0);
-//        distanceToAngle.put(1.633, 0.2);
+//        distanceToAngle.put(1.0, -0.38);
+//        distanceToAngle.put(1.633, -0.2);
 //        distanceToAngle.put(2.0, 0.2);
 //        distanceToAngle.put(2.29, 0.0);
 //        distanceToAngle.put(1.1, -0.2941);
@@ -84,13 +86,12 @@ public static void construct() {
         distanceToAngle.put(1000.0, 0.0);
 
         distanceToRPM.clear();
-        distanceToRPM.put(0.0, 2000.0);
-        distanceToRPM.put(0.894, 3000.0);
+        distanceToRPM.put(0.0, 3500.0);
+        distanceToRPM.put(0.894, 3500.0);
         distanceToRPM.put(2.52, 3750.0); //GOOD VALUES
-        distanceToRPM.put(3.506, 4000.0);
-        distanceToRPM.put(4.25, 4200.0);
-        distanceToRPM.put(1000.0,
-                4000.0);
+//        distanceToRPM.put(3.506, 4000.0);
+        distanceToRPM.put(4.25, 4000.0);
+        distanceToRPM.put(1000.0, 4000.0);
     }
 
     public static LoggedDashboardBoolean retractAfterShot = new LoggedDashboardBoolean("Aim/Retract After Shooting", true);
@@ -248,7 +249,7 @@ public static Command newAmpShoot(Shooter shooter){
 
     public static Command ampSpin(Shooter shooter){
         return run(() -> {
-            shooter.setTargetShooterAngle(Rotation2d.fromRadians(0.925));
+            shooter.setTargetShooterAngle(Rotation2d.fromRadians(1.114));
             shooter.shooterRunVelocity(600);
         });
     }

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -350,6 +350,7 @@ public void periodic() {
                                 if (visionInputs[j].cameraResult.getTargets().size()>visionInputs[i].cameraResult.getTargets().size() && visionInputs[j].timestampSeconds != timestampSeconds[j])
                                     mult += 0.5;
                             }
+                            mult += 2;
                             Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 5", mult);
                             Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier", mult);
                             visionMatrix = new Matrix<>(Nat.N3(), Nat.N1(), new double[]{8 * mult, 8 * mult, 12 * mult});

diff --git a/src/main/java/frc/robot/subsystems/drive/VisionIO.java b/src/main/java/frc/robot/subsystems/drive/VisionIO.java
@@ -83,8 +83,13 @@ public void fromLog(LogTable table) {
       latencyMillis = table.get("LatencyMillis", latencyMillis);
       timestampSeconds = table.get("TimestampSeconds", timestampSeconds);
       name = table.get("Name", name);
-      table.get("CameraResultData").getRaw();
-      cameraResult = PhotonPipelineResult.serde.unpack(new Packet(table.get("CameraResultData").getRaw()));
+      LogTable.LogValue cameraResultData = table.get("CameraResultData");
+      if (cameraResultData != null) {
+        cameraResultData.getRaw();
+        cameraResult = PhotonPipelineResult.serde.unpack(new Packet(cameraResultData.getRaw()));
+      } else {
+        connected = false;
+      }
 
       cameraResult.setTimestampSeconds(timestampSeconds);
     }

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -180,8 +180,8 @@ public void periodic() {
     }
 
     public void resetToLimitAngle(){
-        hoodOffsetAngle = new Rotation2d(hoodInputs.motorPositionRad - 1.98875);
-        hoodFB.reset(hoodInputs.motorPositionRad - 1.98875);
+        hoodOffsetAngle = new Rotation2d(hoodInputs.motorPositionRad - (1.98875));
+        hoodFB.reset(hoodInputs.motorPositionRad - (1.98875+.16));
     }
 
     public void resetToStartingAngle() {
@@ -190,7 +190,7 @@ public void resetToStartingAngle() {
     }
 
     public void resetWhileZeroing() {
-        hoodOffsetAngle = new Rotation2d(hoodInputs.motorPositionRad - 2.225);
+        hoodOffsetAngle = new Rotation2d(hoodInputs.motorPositionRad - (2.225));
         hoodFB.reset(hoodInputs.motorPositionRad - hoodOffsetAngle.getRadians());
     }