simmed beambreaks

Author: ethan jones

Diff for: src/main/java/org/sciborgs1155/robot/commands/NoteVisualizer.java

@@ -49,6 +49,9 @@ public class NoteVisualizer implements Logged {
   private static boolean carryingNote = true;
   private static List<Pose3d> pathPosition = new ArrayList<>();
 
+  private static boolean intakeBeam = true;
+  private static boolean feederBeam = true;
+
   // suppliers
   private static Supplier<Pose2d> drive = Pose2d::new;
   private static Supplier<Pose3d> shooter = Pose3d::new;
@@ -99,24 +102,31 @@ public static Command intake() {
                   if (carryingNote) {
                     return Commands.none();
                   }
-                  return Commands.run(
-                      () -> {
-                        Pose2d intakePose = drive.get();
-                        for (Pose3d note : notes) {
-                          double distance =
-                              Math.abs(
-                                  note.getTranslation()
-                                      .toTranslation2d()
-                                      .getDistance(intakePose.getTranslation()));
-                          if (distance > 0.6) {
-                            continue;
-                          }
-                          carryingNote = true;
-                          notes.remove(note);
-                          notesPub.set(notes.toArray(new Pose3d[0]));
-                          break;
-                        }
-                      });
+                  return Commands.runOnce(
+                          () -> {
+                            intakeBeam = false;
+                            Pose2d intakePose = drive.get();
+                            for (Pose3d note : notes) {
+                              double distance =
+                                  Math.abs(
+                                      note.getTranslation()
+                                          .toTranslation2d()
+                                          .getDistance(intakePose.getTranslation()));
+                              if (distance > 0.6) {
+                                continue;
+                              }
+                              notes.remove(note);
+                              notesPub.set(notes.toArray(new Pose3d[0]));
+                              break;
+                            }
+                          })
+                      .andThen(
+                          Commands.waitSeconds(.3),
+                          Commands.runOnce(
+                              () -> {
+                                carryingNote = true;
+                                intakeBeam = true;
+                              }));
                 },
                 Set.of()))
         .unless(Robot::isReal);
@@ -126,6 +136,7 @@ public static Command shoot() {
     return new ScheduleCommand(
             Commands.defer(
                 () -> {
+                  feederBeam = false;
                   var poses = generatePath();
                   if (poses.length == 0) return Commands.runOnce(() -> {});
                   step = 0;
@@ -142,6 +153,7 @@ public static Command shoot() {
                             shotNotePub.set(new Pose3d());
                             notePathPub.set(new Pose3d[0]);
                             carryingNote = false;
+                            feederBeam = true;
                           });
                 },
                 Set.of()))
@@ -154,6 +166,14 @@ public static boolean hasNote() {
     return carryingNote;
   }
 
+  public static boolean intakeBeam() {
+    return intakeBeam;
+  }
+
+  public static boolean feederBeam() {
+    return feederBeam;
+  }
+
   private static Pose3d[] generatePath() {
     double shotVelocity = velocity.getAsDouble();
     ChassisSpeeds driveSpeeds = speeds.get();

Diff for: src/main/java/org/sciborgs1155/robot/feeder/Feeder.java

@@ -1,11 +1,9 @@
 package org.sciborgs1155.robot.feeder;
 
 import static edu.wpi.first.units.Units.Seconds;
-import static org.sciborgs1155.robot.feeder.FeederConstants.DEBOUNCE_TIME;
 import static org.sciborgs1155.robot.feeder.FeederConstants.POWER;
 import static org.sciborgs1155.robot.feeder.FeederConstants.TIMEOUT;
 
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -22,7 +20,7 @@ public class Feeder extends SubsystemBase implements AutoCloseable, Logged {
 
   /** Creates a real or non-existent feeder based on {@link Robot#isReal()}. */
   public static Feeder create() {
-    return Robot.isReal() ? new Feeder(new RealFeeder()) : new Feeder(new NoFeeder());
+    return Robot.isReal() ? new Feeder(new RealFeeder()) : new Feeder(new SimFeeder());
   }
 
   /** Creates a non-existent feeder. */
@@ -32,6 +30,7 @@ public static Feeder none() {
 
   public Feeder(FeederIO feeder) {
     this.feeder = feeder;
+    noteAtShooter().onTrue(Commands.runOnce(() -> System.out.println("note at shooter")));
   }
 
   public Command runFeeder(double power) {
@@ -60,6 +59,7 @@ public Command backward() {
    */
   public Command eject() {
     return Commands.waitUntil(noteAtShooter())
+        .andThen(Commands.runOnce(() -> System.out.println("moved on")))
         .andThen(Commands.waitUntil(noteAtShooter().negate()))
         .deadlineWith(forward())
         .alongWith(NoteVisualizer.shoot())
@@ -76,9 +76,13 @@ public Command retract() {
    * @return A trigger based on the upper feeder beambreak.
    */
   public Trigger noteAtShooter() {
-    return new Trigger(feeder::beambreak)
-        .negate()
-        .debounce(DEBOUNCE_TIME.in(Seconds), DebounceType.kFalling);
+    return new Trigger(feeder::beambreak).negate();
+    // .debounce(DEBOUNCE_TIME.in(Seconds), DebounceType.kFalling);
+  }
+
+  @Log.NT
+  public boolean beambreak() {
+    return feeder.beambreak();
   }
 
   @Log.NT

Diff for: src/main/java/org/sciborgs1155/robot/feeder/SimFeeder.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import monologue.Annotations.Log;
 import org.sciborgs1155.robot.Constants;
+import org.sciborgs1155.robot.commands.NoteVisualizer;
 
 public class SimFeeder implements FeederIO {
   private final DCMotorSim sim =
@@ -22,7 +23,7 @@ public void setPower(double voltage) {
   @Override
   @Log.NT
   public boolean beambreak() {
-    return true;
+    return NoteVisualizer.feederBeam();
   }
 
   @Override

Diff for: src/main/java/org/sciborgs1155/robot/intake/Intake.java

@@ -18,7 +18,7 @@
 
 public class Intake extends SubsystemBase implements Logged, AutoCloseable {
   public static Intake create() {
-    return Robot.isReal() ? new Intake(new RealIntake()) : new Intake(new NoIntake());
+    return Robot.isReal() ? new Intake(new RealIntake()) : new Intake(new SimIntake());
   }
 
   public static Intake none() {

Diff for: src/main/java/org/sciborgs1155/robot/intake/SimIntake.java

@@ -0,0 +1,41 @@
+package org.sciborgs1155.robot.intake;
+
+import static edu.wpi.first.units.Units.Seconds;
+import static org.sciborgs1155.robot.feeder.FeederConstants.*;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import monologue.Annotations.Log;
+import org.sciborgs1155.robot.Constants;
+import org.sciborgs1155.robot.commands.NoteVisualizer;
+
+public class SimIntake implements IntakeIO {
+  private final DCMotorSim sim =
+      new DCMotorSim(LinearSystemId.createDCMotorSystem(kV, kA), DCMotor.getNeoVortex(1), GEARING);
+
+  @Override
+  public void setPower(double voltage) {
+    sim.setInputVoltage(voltage);
+    sim.update(Constants.PERIOD.in(Seconds));
+  }
+
+  @Override
+  @Log.NT
+  public boolean beambreak() {
+    return NoteVisualizer.feederBeam();
+  }
+
+  @Override
+  public double current() {
+    return sim.getCurrentDrawAmps();
+  }
+
+  @Override
+  public boolean seenNote() {
+    return NoteVisualizer.hasNote();
+  }
+
+  @Override
+  public void close() throws Exception {}
+}