updated note vis

frc2053 · Sep 5, 2024 · 57f4807 · 57f4807
1 parent 30cedb8
commit 57f4807
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 16 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -97,6 +97,19 @@
     "xstring": "cpp",
     "xtr1common": "cpp",
     "xutility": "cpp",
-    "shared_mutex": "cpp"
+    "shared_mutex": "cpp",
+    "charconv": "cpp",
+    "format": "cpp",
+    "ios": "cpp",
+    "locale": "cpp",
+    "xfacet": "cpp",
+    "xiosbase": "cpp",
+    "xlocale": "cpp",
+    "xlocbuf": "cpp",
+    "xlocinfo": "cpp",
+    "xlocmes": "cpp",
+    "xlocmon": "cpp",
+    "xlocnum": "cpp",
+    "xloctime": "cpp"
   }
 }
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -28,19 +28,19 @@ void RobotContainer::ConfigureBindings() {
       },
       true));
 
-  //controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp());
-  controller.LeftBumper().WhileTrue(swerveSubsystem.XPattern());
-  controller.Back().WhileTrue(
-      swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse));
-  controller.Start().WhileTrue(
-      swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward));
+  controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp());
+  // controller.LeftBumper().WhileTrue(swerveSubsystem.XPattern());
+  // controller.Back().WhileTrue(
+  //     swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse));
+  // controller.Start().WhileTrue(
+  //     swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward));
 
   controller.A().OnTrue(frc2::cmd::RunOnce([this] {
     noteVisualizer.LaunchNote(
         frc::Pose3d{swerveSubsystem.GetRobotPose()},
         swerveSubsystem.GetRobotRelativeSpeed(),
-        frc::Transform3d{frc::Translation3d{0_m, 0_m, 12_in},
-                         frc::Rotation3d{0_deg, -30_deg, 0_deg}},
+        frc::Transform3d{frc::Translation3d{-4_in, 0_in, 13_in},
+                         frc::Rotation3d{0_deg, -50_deg, 0_deg}},
         41.71_fps);
   }));
 
@@ -49,14 +49,14 @@ void RobotContainer::ConfigureBindings() {
   // controller.X().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kForward));
   // controller.Y().WhileTrue(swerveSubsystem.SysIdDriveDynamicTorque(frc2::sysid::Direction::kReverse));
 
-  controller.POVDown().OnTrue(swerveSubsystem.TuneSteerPID([this] { return controller.Start().Get(); }));
-  controller.POVUp().OnTrue(swerveSubsystem.TuneDrivePID([this] { return controller.Start().Get(); }));
+  // controller.POVDown().OnTrue(swerveSubsystem.TuneSteerPID([this] { return controller.Start().Get(); }));
+  // controller.POVUp().OnTrue(swerveSubsystem.TuneDrivePID([this] { return controller.Start().Get(); }));
 
-  controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; }));
-  controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));
+  // controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; }));
+  // controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 
-  controller.LeftTrigger().WhileTrue(intakeSubsystem.IntakeNote());
-  controller.RightTrigger().WhileTrue(intakeSubsystem.PoopNote());
+  // controller.LeftTrigger().WhileTrue(intakeSubsystem.IntakeNote());
+  // controller.RightTrigger().WhileTrue(intakeSubsystem.PoopNote());
 
   // controller.A().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kForward));
   // controller.B().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse));

diff --git a/src/main/cpp/str/NoteVisualizer.cpp b/src/main/cpp/str/NoteVisualizer.cpp
@@ -43,13 +43,15 @@ void NoteVisualizer::LaunchNote(frc::Pose3d currentRobotPose,
 
   launchedNotes.emplace_back(noteToAdd);
   launchedNotePoses.emplace_back(noteToAdd.currentPose);
+  fmt::print("Meow\n");
 }
 
 void NoteVisualizer::Periodic() {
   units::second_t now = frc::Timer::GetFPGATimestamp();
   units::second_t loopTime = now - lastLoopTime;
 
   UpdateLaunchedNotes(loopTime);
+  CleanUp();
 
   stagedNotesPub.Set(initialNoteLocations);
   launchedNotesPub.Set(launchedNotePoses);
@@ -67,6 +69,21 @@ void NoteVisualizer::UpdateLaunchedNotes(units::second_t loopTime) {
   }
 }
 
+void NoteVisualizer::CleanUp() {
+  int i = 0;
+  for(auto mit = launchedNotes.begin(); mit != launchedNotes.end(); )
+  {   
+    if(mit->shouldClean) {
+      mit = launchedNotes.erase(mit);
+      launchedNotePoses.erase(launchedNotePoses.begin() + i);
+    }
+    else {
+      mit++;
+    }
+    i++;
+  }
+}
+
 void NoteVisualizer::ProjectileMotion(FlyingNote &note,
                                       units::second_t loopTime) {
   note.currentVelocity.zVel =
@@ -86,6 +103,7 @@ void NoteVisualizer::ProjectileMotion(FlyingNote &note,
     note.currentVelocity.xVel = 0_mps;
     note.currentVelocity.yVel = 0_mps;
     note.currentVelocity.zVel = 0_mps;
+    note.shouldClean = true;
   }
 
   note.currentPose = newPose;

diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-4b09746
+30cedb8
diff --git a/src/main/include/str/NoteVisualizer.h b/src/main/include/str/NoteVisualizer.h
@@ -28,6 +28,7 @@ struct FlyingNote {
   NoteVelocity initialVelocity{};
   NoteVelocity currentVelocity{};
   frc::Pose3d currentPose{};
+  bool shouldClean{false};
 };
 
 class NoteVisualizer {
@@ -42,6 +43,7 @@ class NoteVisualizer {
 private:
   void UpdateLaunchedNotes(units::second_t loopTime);
   void ProjectileMotion(FlyingNote &note, units::second_t loopTime);
+  void CleanUp();
 
   units::second_t lastLoopTime;