Shuttling implemented and map finnished

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index d3cce246a26f57b0792a8bf0c242ed97e73705c5..44fc659eefd48bcfa150396c44205b3749858d31 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -219,33 +219,36 @@ public class Superstructure extends Command {
             stowEverything();
         } else {
             turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
-            
-            // if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
-            //     hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
-            // } else{
-                //hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity);
-            // }
 
-            hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+            boolean shuttling = target.getZ() != FieldConstants.getHubTranslation().getZ(); // if we're aiming upward we are aiming at hub, thus not shuttling
+
+            // shuttling will move the hood so its angles very close to max (less arch)
+            if (shuttling) {
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+            } else {
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+            }
+            
+            
             double x = drivepose.getX(); // compared as meters
             double y = drivepose.getY();
-            System.out.println("X: " + Units.metersToInches(x) + "Y: " + Units.metersToInches(y));
             // if (FieldConstants.underTrench(x, y)) {
-            //     hood.forceHoodDown(true);
             //     System.out.println("Hood forced down");
             // } else {
             //     hood.forceHoodDown(false);
-            //     System.out.println("Hood forced up");
             // }
-            shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
-            Logger.recordOutput("Distance From Target", distanceFromTarget);
-            //shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
-            // if (phaseManager.shouldFeed()) {
-            //     spindexer.maxSpindexer();
-            // } else {
-            //     spindexer.stopSpindexer();
-            // }
+            // different maps for shuttling vs shooting. Less powerful when shuttling.
+            if (shuttling) {
+                shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+            } else {
+                shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+            }
+
+            // record when shuttling
+            Logger.recordOutput("Shuttling", shuttling);
+            // record distance for tuning if needed
+            Logger.recordOutput("Distance From Target", distanceFromTarget);
         }
 
         Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);

diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java
index 0c0da265a3a2a3500cc4f22c4caf229367d04487..1aba33c6bc49459bbbae7fcd1fd830ee169f8b95 100644 (file)
--- a/src/main/java/frc/robot/constants/ShotInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShotInterpolation.java
@@ -43,15 +43,15 @@ public class ShotInterpolation {
         exitVelocityMap.put(11.0, 26.0);
         exitVelocityMap.put(25.0, 25.0* 3.2);
 
+        // currently regresses to y = 1.34959x + 9.79618
+        shooterVelocityMap.put(0.0, 9.55);
         shooterVelocityMap.put(1.00, 11.5);
         shooterVelocityMap.put(1.49, 11.5);
         shooterVelocityMap.put(2.09, 12.5);
         shooterVelocityMap.put(2.95, 13.5);
+        shooterVelocityMap.put(4.07, 15.5);
         shooterVelocityMap.put(5.05, 16.7);
         shooterVelocityMap.put(5.79, 18.0);
-        shooterVelocityMap.put(4.07, 15.5);
-
-        shooterVelocityMap.put(0.0, 9.55);
         shooterVelocityMap.put(25.0, 43.44);
 
         newHoodMap.put(1.00, 78.0);

diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
index f5ad80c512547d45dcf407fb093479fb7dbaa307..1eb358bfc13c650d88773b715a97fec48084e4bd 100644 (file)
--- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java
@@ -1,11 +1,39 @@
 
 // i'm going to get so many lines of code from this :) - Wesley
 public class ShuttleInterpolation {
-    public static final IntpolationDoubleTreeMap timeOfFlightMap = new InterpolationDoubleTreeMap();
-    public static final IntpolationDoubleTreeMap timeOfFlightMap = new InterpolationDoubleTreeMap();
-
+    public static final IntpolationDoubleTreeMap hoodAngleMap = new InterpolationDoubleTreeMap();
+    public static final IntpolationDoubleTreeMap shooterVelocityMap = new InterpolationDoubleTreeMap();
+    /*
+    guide to tuning:
+    modify the left values in the parenthesis to change the distance of the shot.
+    setup the robot at that distance and shoot. Then modifty the right value to be more or less until it hits target perfectly.
+    Repeat
+    OR
+    Run robot, estimate distance, and tweak values when nobodies looking until it works.
+    */
     static {
-        
+        // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618) 
+        // will likely be this that requires tuning.
+        shooterVelocityMap.put(0.0, 6.2075);
+        shooterVelocityMap.put(1.00, 7.475);
+        shooterVelocityMap.put(1.49, 7.475);
+        shooterVelocityMap.put(2.09, 8.125);
+        shooterVelocityMap.put(2.95, 8.775);
+        shooterVelocityMap.put(4.07, 10.075);
+        shooterVelocityMap.put(5.05, 10.855);
+        shooterVelocityMap.put(5.79, 11.7);
+        shooterVelocityMap.put(25.0, 28.236);
+
+        // always shoot at low angle to ground. 
+        newHoodMap.put(0.0, 80.0);
+        newHoodMap.put(1.00, 80.0);
+        newHoodMap.put(1.49, 80.0);
+        newHoodMap.put(2.09, 80.0);
+        newHoodMap.put(2.95, 80.0);
+        newHoodMap.put(5.05, 80.0);
+        newHoodMap.put(5.79, 80.0);
+        newHoodMap.put(4.07, 80.0);
+        newHoodMap.put(27.99, 80.0);
     }
 
 }
\ No newline at end of file