From 28d4f573e71075bb31632e6375b3e307ec1db1e4 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Wed, 2 Oct 2024 11:45:31 -0400 Subject: [PATCH] commenting out all subsystems as we add them --- src/main/cpp/Robot.cpp | 60 ++++++++++----------- src/main/cpp/RobotContainer.cpp | 51 +++++++++--------- src/main/cpp/subsystems/SwerveSubsystem.cpp | 4 +- src/main/deploy/commit.txt | 2 +- src/main/include/RobotContainer.h | 20 +++---- 5 files changed, 69 insertions(+), 68 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 7222218..e1292db 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -32,41 +32,41 @@ void Robot::RobotPeriodic() { units::second_t loopTime = now - lastTotalLoopTime; loopTimePub.Set((1 / loopTime).value()); - m_container.GetNoteVisualizer().DisplayRobotNote( - m_container.GetFeederSubsystem().HasNote(), - m_container.GetSwerveSubsystem().GetRobotPose()); + // m_container.GetNoteVisualizer().DisplayRobotNote( + // m_container.GetFeederSubsystem().HasNote(), + // m_container.GetSwerveSubsystem().GetRobotPose()); frc2::CommandScheduler::GetInstance().Run(); - m_container.GetNoteVisualizer().Periodic(); - UpdateVision(); - m_container.GetSwerveSubsystem().CalculateFoundNotePose(m_container.GetVision().GetDistanceToNote(), m_container.GetVision().GetAngleToNote()); + //m_container.GetNoteVisualizer().Periodic(); + //UpdateVision(); + //m_container.GetSwerveSubsystem().CalculateFoundNotePose(m_container.GetVision().GetDistanceToNote(), m_container.GetVision().GetAngleToNote()); lastTotalLoopTime = now; } void Robot::UpdateVision() { - auto visionEstimates = m_container.GetVision().GetCameraEstimatedPoses(); - auto stdDevs = m_container.GetVision().GetPoseStdDevs(visionEstimates); - - frc::Pose3d pose = - frc::Pose3d{m_container.GetSwerveSubsystem().GetRobotPose()}; - - cameraLocations[0] = pose.TransformBy(consts::vision::FL_ROBOT_TO_CAM); - cameraLocations[1] = pose.TransformBy(consts::vision::FR_ROBOT_TO_CAM); - cameraLocations[2] = pose.TransformBy(consts::vision::BL_ROBOT_TO_CAM); - cameraLocations[3] = pose.TransformBy(consts::vision::BR_ROBOT_TO_CAM); - cameraLocations[4] = pose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM); - - cameraLocationsPub.Set(cameraLocations); - - int i = 0; - for (const auto &est : visionEstimates) { - if (est.has_value()) { - // m_container.GetSwerveSubsystem().AddVisionMeasurement(est.value().estimatedPose.ToPose2d(), - // est.value().timestamp, stdDevs[i].value()); - } - i++; - } + // auto visionEstimates = m_container.GetVision().GetCameraEstimatedPoses(); + // auto stdDevs = m_container.GetVision().GetPoseStdDevs(visionEstimates); + + // frc::Pose3d pose = + // frc::Pose3d{m_container.GetSwerveSubsystem().GetRobotPose()}; + + // cameraLocations[0] = pose.TransformBy(consts::vision::FL_ROBOT_TO_CAM); + // cameraLocations[1] = pose.TransformBy(consts::vision::FR_ROBOT_TO_CAM); + // cameraLocations[2] = pose.TransformBy(consts::vision::BL_ROBOT_TO_CAM); + // cameraLocations[3] = pose.TransformBy(consts::vision::BR_ROBOT_TO_CAM); + // cameraLocations[4] = pose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM); + + // cameraLocationsPub.Set(cameraLocations); + + // int i = 0; + // for (const auto &est : visionEstimates) { + // if (est.has_value()) { + // // m_container.GetSwerveSubsystem().AddVisionMeasurement(est.value().estimatedPose.ToPose2d(), + // // est.value().timestamp, stdDevs[i].value()); + // } + // i++; + // } } void Robot::DisabledInit() {} @@ -111,8 +111,8 @@ void Robot::SimulationPeriodic() { // frc::sim::BatterySim::Calculate({m_container.GetSwerveSubsystem().GetSimulatedCurrentDraw()}); // frc::sim::RoboRioSim::SetVInVoltage(battVoltage); - m_container.GetVision().SimulationPeriodic( - m_container.GetSwerveSubsystem().GetOdomPose()); + // m_container.GetVision().SimulationPeriodic( + // m_container.GetSwerveSubsystem().GetOdomPose()); } #ifndef RUNNING_FRC_TESTS diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index b25236b..1c33a40 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -50,14 +50,14 @@ void RobotContainer::ConfigureBindings() { // 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{-4_in, 0_in, 13_in}, - frc::Rotation3d{0_deg, -50_deg, 0_deg}}, - 41.71_fps); - })); + // controller.A().OnTrue(frc2::cmd::RunOnce([this] { + // noteVisualizer.LaunchNote( + // frc::Pose3d{swerveSubsystem.GetRobotPose()}, + // swerveSubsystem.GetRobotRelativeSpeed(), + // frc::Transform3d{frc::Translation3d{-4_in, 0_in, 13_in}, + // frc::Rotation3d{0_deg, -50_deg, 0_deg}}, + // 41.71_fps); + // })); // controller.A().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kForward)); // controller.B().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kReverse)); @@ -69,13 +69,13 @@ void RobotContainer::ConfigureBindings() { // 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)); @@ -115,27 +115,28 @@ void RobotContainer::ConfigureBindings() { } frc2::Command *RobotContainer::GetAutonomousCommand() { - return autos.GetSelectedCommand(); + //return autos.GetSelectedCommand(); + return nullptr; } SwerveSubsystem &RobotContainer::GetSwerveSubsystem() { return swerveSubsystem; } -ShooterSubsystem &RobotContainer::GetShooterSubsystem() { - return shooterSubsystem; -} +// ShooterSubsystem &RobotContainer::GetShooterSubsystem() { +// return shooterSubsystem; +// } IntakeSubsystem &RobotContainer::GetIntakeSubsystem() { return intakeSubsystem; } -FeederSubsystem &RobotContainer::GetFeederSubsystem() { - return feederSubsystem; -} +// FeederSubsystem &RobotContainer::GetFeederSubsystem() { +// return feederSubsystem; +// } -str::Vision &RobotContainer::GetVision() { return vision; } +// str::Vision &RobotContainer::GetVision() { return vision; } -str::NoteVisualizer &RobotContainer::GetNoteVisualizer() { - return noteVisualizer; -} +// str::NoteVisualizer &RobotContainer::GetNoteVisualizer() { +// return noteVisualizer; +// } diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index ffbc0f1..133d236 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -29,8 +29,8 @@ SwerveSubsystem::SwerveSubsystem() consts::swerve::pathplanning::ROTATION_D})) { SetName("SwerveSubsystem"); frc::SmartDashboard::PutData(this); - SetupPathplanner(); - LoadChoreoTrajectories(); + //SetupPathplanner(); + //LoadChoreoTrajectories(); } void SwerveSubsystem::UpdateSwerveOdom() { swerveDrive.UpdateSwerveOdom(); } diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 1a6872b..3aacab5 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -c83f2be \ No newline at end of file +9255fe7 \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 43a64ab..91e9a8b 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -20,23 +20,23 @@ class RobotContainer { frc2::Command *GetAutonomousCommand(); SwerveSubsystem &GetSwerveSubsystem(); - ShooterSubsystem &GetShooterSubsystem(); + // ShooterSubsystem &GetShooterSubsystem(); IntakeSubsystem &GetIntakeSubsystem(); - FeederSubsystem &GetFeederSubsystem(); - str::Vision &GetVision(); - str::NoteVisualizer &GetNoteVisualizer(); + // FeederSubsystem &GetFeederSubsystem(); + //str::Vision &GetVision(); + //str::NoteVisualizer &GetNoteVisualizer(); private: void ConfigureBindings(); frc2::CommandXboxController controller{0}; SwerveSubsystem swerveSubsystem; - ShooterSubsystem shooterSubsystem; + // ShooterSubsystem shooterSubsystem; IntakeSubsystem intakeSubsystem; - FeederSubsystem feederSubsystem; - str::Vision vision; - str::NoteVisualizer noteVisualizer; + // FeederSubsystem feederSubsystem; + // str::Vision vision; + // str::NoteVisualizer noteVisualizer; - Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem, - feederSubsystem}; + // Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem, + // feederSubsystem}; };