From 57f480728724a96d1ec945425cefa2153c28c051 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Thu, 5 Sep 2024 12:54:10 -0400 Subject: [PATCH] updated note vis --- .vscode/settings.json | 15 +++++++++++++- src/main/cpp/RobotContainer.cpp | 28 +++++++++++++-------------- src/main/cpp/str/NoteVisualizer.cpp | 18 +++++++++++++++++ src/main/deploy/commit.txt | 2 +- src/main/include/str/NoteVisualizer.h | 2 ++ 5 files changed, 49 insertions(+), 16 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0029c6e..084d5a1 100644 --- 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 index cba6c64..76efbbf 100644 --- 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 index c48d071..deacb38 100644 --- a/src/main/cpp/str/NoteVisualizer.cpp +++ b/src/main/cpp/str/NoteVisualizer.cpp @@ -43,6 +43,7 @@ void NoteVisualizer::LaunchNote(frc::Pose3d currentRobotPose, launchedNotes.emplace_back(noteToAdd); launchedNotePoses.emplace_back(noteToAdd.currentPose); + fmt::print("Meow\n"); } void NoteVisualizer::Periodic() { @@ -50,6 +51,7 @@ void NoteVisualizer::Periodic() { 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 ¬e, units::second_t loopTime) { note.currentVelocity.zVel = @@ -86,6 +103,7 @@ void NoteVisualizer::ProjectileMotion(FlyingNote ¬e, 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 index 6d3b0f0..9fa36f2 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -4b09746 \ No newline at end of file +30cedb8 \ No newline at end of file diff --git a/src/main/include/str/NoteVisualizer.h b/src/main/include/str/NoteVisualizer.h index f3801ee..011aaf5 100644 --- 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 ¬e, units::second_t loopTime); + void CleanUp(); units::second_t lastLoopTime;