auto-feeding

simgui-ds.json

@@ -1,12 +1,7 @@
 {
-  "FMS": {
+  "Keyboard 0 Settings": {
     "window": {
-      "visible": false
-    }
-  },
-  "Joysticks": {
-    "window": {
-      "visible": false
+      "visible": true
     }
   },
   "System Joysticks": {

simgui.json

@@ -1,6 +1,9 @@
 {
   "HALProvider": {
     "Addressable LEDs": {
+      "0": {
+        "serpentine": true
+      },
       "window": {
         "visible": true
       }
@@ -100,6 +103,11 @@
           "visible": true
         }
       },
+      "/Robot/pivot/positionVisualizer": {
+        "window": {
+          "visible": true
+        }
+      },
       "/SmartDashboard/VisionSystemSim-main/Sim Field": {
         "bottom": 1476,
         "height": 8.210550308227539,
@@ -160,23 +168,21 @@
             "open": true
           }
         },
-        "feeder": {
-          "open": true
-        },
-        "intake": {
-          "open": true
-        },
         "open": true,
         "pivot": {
+          "Rotation3d##v_/Robot/pivot/rotation": {
+            "open": true
+          },
           "hardware": {
             "open": true
           },
+          "open": true,
           "pid": {
             "open": true
+          },
+          "positionVisualizer": {
+            "open": true
           }
-        },
-        "shooter": {
-          "open": true
         }
       }
     }
@@ -186,8 +192,5 @@
       "open": true
     },
     "visible": true
-  },
-  "NetworkTables View": {
-    "visible": false
   }
 }

src/main/java/org/sciborgs1155/robot/Constants.java

@@ -141,6 +141,10 @@ public static Translation2d amp() {
       return alliance() == Alliance.Blue ? BLUE_AMP : RED_AMP;
     }
 
+    public static Translation2d feedTarget() {
+      return amp().plus(new Translation2d(Inches.of(0), Inches.of(-24)));
+    }
+
     /** Returns whether the provided position is within the boundaries of the field. */
     public static boolean inField(Pose3d pose) {
       return (pose.getX() > 0

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -7,6 +7,7 @@
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*;
 import static org.sciborgs1155.robot.Constants.DEADBAND;
+import static org.sciborgs1155.robot.Constants.Field.feedTarget;
 import static org.sciborgs1155.robot.Constants.PERIOD;
 import static org.sciborgs1155.robot.drive.DriveConstants.MAX_ANGULAR_ACCEL;
 import static org.sciborgs1155.robot.drive.DriveConstants.MAX_SPEED;
@@ -16,6 +17,8 @@
 import static org.sciborgs1155.robot.pivot.PivotConstants.STARTING_ANGLE;
 import static org.sciborgs1155.robot.shooter.ShooterConstants.AMP_VELOCITY;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
@@ -117,6 +120,9 @@ private void configureGameBehavior() {
         () -> log("dist", Shooting.translationToSpeaker(drive.pose().getTranslation()).getNorm()),
         kDefaultPeriod);
 
+    addPeriodic(
+        () -> log("feedTarget", new Pose2d(feedTarget(), new Rotation2d())), kDefaultPeriod);
+
     SmartDashboard.putData(CommandScheduler.getInstance());
     // Log PDH
     SmartDashboard.putData("PDH", pdh);
@@ -263,6 +269,8 @@ private void configureBindings() {
                 .deadlineWith(Commands.idle(shooter)))
         .toggleOnTrue(led.raindrop());
 
+    operator.y().onTrue(shooting.feedToAmp(x, y));
+
     // operator manual shoot (povDown)
     operator.povDown().whileTrue(shooting.shoot(RadiansPerSecond.of(350))).whileTrue(led.rainbow());
 

src/main/java/org/sciborgs1155/robot/commands/Shooting.java

@@ -5,7 +5,9 @@
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 import static java.lang.Math.atan;
+import static java.lang.Math.cos;
 import static java.lang.Math.pow;
+import static java.lang.Math.tan;
 import static org.sciborgs1155.robot.Constants.Field.*;
 import static org.sciborgs1155.robot.pivot.PivotConstants.MAX_ANGLE;
 import static org.sciborgs1155.robot.pivot.PivotConstants.MIN_ANGLE;
@@ -54,6 +56,8 @@ public class Shooting implements Logged {
    */
   public static final DoubleEntry siggysConstant = Tuning.entry("/Robot/Siggy's Constant", 4.42);
 
+  public static final DoubleEntry ethansConstant = Tuning.entry("/Robot/Ethan's Constant", 4.42);
+
   public static final Measure<Distance> MAX_DISTANCE = Meters.of(5.0);
 
   private static final InterpolatingDoubleTreeMap shotVelocityLookup =
@@ -150,6 +154,20 @@ && atYaw(yawFromNoteVelocity(calculateNoteVelocity())))
             pivot.runPivot(() -> pitchFromNoteVelocity(calculateNoteVelocity())));
   }
 
+  public Command feedToAmp(InputStream vx, InputStream vy) {
+    return shoot(
+            () -> calculateFeedNoteVelocity().norm() / RADIUS.in(Meters) * ethansConstant.get(),
+            () ->
+                atYaw(yawFromNoteVelocity(calculateFeedNoteVelocity()))
+                    && pivot.atPosition(pitchFromNoteVelocity(calculateFeedNoteVelocity())))
+        .deadlineWith(
+            drive.drive(
+                vx.scale(0.5),
+                vy.scale(0.5),
+                () -> yawFromNoteVelocity(calculateFeedNoteVelocity())),
+            pivot.runPivot(() -> pitchFromNoteVelocity(calculateFeedNoteVelocity())));
+  }
+
   public static Pose2d robotPoseFacingSpeaker(Translation2d robotTranslation) {
     return new Pose2d(
         robotTranslation,
@@ -163,28 +181,51 @@ public Vector<N3> calculateNoteVelocity() {
   }
 
   public Vector<N3> calculateNoteVelocity(Measure<Time> predictionTime) {
-    return calculateNoteVelocity(
+    Pose2d predictedPose =
+        predictedPose(drive.pose(), drive.getFieldRelativeChassisSpeeds(), predictionTime);
+
+    return calculateNoteVelocity(predictedPose);
+  }
+
+  public Vector<N3> calculateNoteVelocity(Pose2d robotPose) {
+    Translation2d difference = translationToSpeaker(robotPose.getTranslation());
+    double shotVelocity = calculateStationaryVelocity(difference.getNorm());
+    double pitch =
+        calculateStationaryPitch(
+            robotPoseFacingSpeaker(robotPose.getTranslation()), shotVelocity, pivot.position());
+
+    return calculateNoteVelocity(robotPose, difference, shotVelocity, pitch);
+  }
+
+  public Vector<N3> calculateFeedNoteVelocity() {
+    return calculateFeedNoteVelocity(drive.pose());
+  }
+
+  public Vector<N3> calculateFeedNoteVelocity(Measure<Time> predictionTime) {
+    return calculateFeedNoteVelocity(
         predictedPose(drive.pose(), drive.getFieldRelativeChassisSpeeds(), predictionTime));
   }
 
+  public Vector<N3> calculateFeedNoteVelocity(Pose2d robotPose) {
+    double pitch = pivot.position();
+    double shotVelocity = calculateVelocityFromPitch(robotPose, pitch);
+
+    return calculateNoteVelocity(
+        robotPose, translationToFeedTarget(robotPose.getTranslation()), shotVelocity, pitch);
+  }
+
   /**
    * Calculates a vector for the desired note velocity relative to the robot for it to travel into
    * the speaker, accounting for the robot's current motion.
    *
    * @return A 3d vector representing the desired note initial velocity.
    */
-  public Vector<N3> calculateNoteVelocity(Pose2d robotPose) {
+  public Vector<N3> calculateNoteVelocity(
+      Pose2d robotPose, Translation2d difference, double shotVelocity, double pitch) {
     ChassisSpeeds speeds = drive.getFieldRelativeChassisSpeeds();
     Vector<N3> robotVelocity =
         VecBuilder.fill(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, 0);
-    Translation2d difference = translationToSpeaker(robotPose.getTranslation());
-    double shotVelocity = calculateStationaryVelocity(difference.getNorm());
-    Rotation3d noteOrientation =
-        new Rotation3d(
-            0,
-            -calculateStationaryPitch(
-                robotPoseFacingSpeaker(robotPose.getTranslation()), shotVelocity, pivot.position()),
-            difference.getAngle().getRadians());
+    Rotation3d noteOrientation = new Rotation3d(0, -pitch, difference.getAngle().getRadians());
     // rotate unit forward vector by note orientation and scale by our shot velocity
     Vector<N3> noteVelocity =
         new Translation3d(1, 0, 0).rotateBy(noteOrientation).toVector().unit().times(shotVelocity);
@@ -288,6 +329,10 @@ public static Translation2d translationToSpeaker(Translation2d robotTranslation)
     return speaker().toTranslation2d().minus(robotTranslation);
   }
 
+  public static Translation2d translationToFeedTarget(Translation2d robotTranslation) {
+    return feedTarget().minus(robotTranslation);
+  }
+
   public static double calculateStationaryVelocity(double distance) {
     return flywheelToNoteSpeed(shotVelocityLookup.get(distance));
   }
@@ -321,4 +366,16 @@ private static double calculateStationaryPitch(
     }
     return calculateStationaryPitch(robotPose, velocity, pitch, i + 1);
   }
+
+  private static double calculateVelocityFromPitch(Pose2d robotPose, double pitch) {
+    double G = 9.81;
+    Translation3d shooterTranslation =
+        shooterPose(Pivot.transform(-pitch), robotPose).getTranslation();
+    double dist = translationToFeedTarget(shooterTranslation.toTranslation2d()).getNorm();
+    double h = 0;
+    double denom = 2 * pow(cos(pitch), 2) * (h - (dist * tan(pitch)));
+    double numerator = -G * pow(dist, 2);
+
+    return Math.sqrt(numerator / denom);
+  }
 }

src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java

@@ -86,6 +86,8 @@ public static final class Driving {
       public static final Measure<Distance> POSITION_FACTOR = CIRCUMFERENCE.times(GEARING);
       public static final Measure<Velocity<Distance>> VELOCITY_FACTOR = POSITION_FACTOR.per(Minute);
 
+      public static final Measure<Velocity<Distance>> VELOCITY_TOLERANCE = MetersPerSecond.of(.1);
+
       public static final Measure<Current> CURRENT_LIMIT = Amps.of(50);
 
       public static final class PID {