[Robot] Lobbing tune

SouthwestRoboticsProgramming · Mar 24, 2024 · 9ad90d6 · 9ad90d6
1 parent a649c9b
commit 9ad90d6
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 2 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java b/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java
@@ -1,5 +1,6 @@
 package com.swrobotics.robot.commands;
 
+import com.swrobotics.lib.net.NTEntry;
 import com.swrobotics.lib.net.NTUtil;
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.config.NTData;
@@ -11,6 +12,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public final class AimTowardsLobCommand extends Command {
@@ -53,7 +55,12 @@ public void execute() {
         double flyTime = getFlyTime(currentAim);
         double missAmount = flyTime * robotVelocity.getY();
 
-        double correctionRad = -Math.atan2(missAmount, currentAim.distanceToSpeaker()) + OFFSET;
+        // For some reason it's different per alliance???
+        NTEntry<Double> offset = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue
+                ? NTData.SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE
+                : NTData.SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_RED;
+
+        double correctionRad = -Math.atan2(missAmount, currentAim.distanceToSpeaker()) + Math.toRadians(offset.get());
 
         double setpointAngle = MathUtil.wrap(angleToTarget.getRadians() + correctionRad, -Math.PI, Math.PI);
         double currentAngle = MathUtil.wrap(robotPose.getRotation().getRadians(), -Math.PI, Math.PI);

diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java
@@ -119,6 +119,8 @@ public final class NTData {
     public static final NTEntry<Double> SHOOTER_LOB_POWER_COEFFICIENT = new NTDouble("Shooter/Lob/Power Coefficient", 1.25).setPersistent(); // To go from real velocity to flywheel velocity
     public static final NTEntry<Double> SHOOTER_LOB_TALL_HEIGHT_METERS = new NTDouble("Shooter/Lob/Tall Lob Height (m)", 4).setPersistent();
     public static final NTEntry<Double> SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1).setPersistent();
+    public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE = new NTDouble("Shooter/Lob/Drive Angle Correction Blue (deg)", -25).setPersistent();
+    public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_RED = new NTDouble("Shooter/Lob/Drive Angle Correction Red (deg)", -25).setPersistent();
     public static final NTEntry<Double> SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25);
 
     private NTData() {

diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
@@ -230,7 +230,7 @@ public void periodic() {
             robot.intake.setReverse(false);
             robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop
 
-            robot.drive.setEstimatorIgnoreVision(DriverStation.isAutonomous());
+//            robot.drive.setEstimatorIgnoreVision(DriverStation.isAutonomous());
 
             return;
         }