From 36b93f00dc268464cfa682fcbf4073149d631d4f Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Wed, 23 Oct 2024 21:13:54 -0400 Subject: [PATCH] working on wierd swerve --- .../cpp/str/CommandLogitechController.cpp | 81 ++++++ src/main/cpp/str/LogitechController.cpp | 246 ++++++++++++++++++ src/main/cpp/str/SwerveDrive.cpp | 15 +- src/main/include/RobotContainer.h | 2 + .../include/str/CommandLogitechController.h | 75 ++++++ src/main/include/str/LogitechController.h | 117 +++++++++ src/main/include/str/SwerveDrive.h | 4 +- 7 files changed, 533 insertions(+), 7 deletions(-) create mode 100644 src/main/cpp/str/CommandLogitechController.cpp create mode 100644 src/main/cpp/str/LogitechController.cpp create mode 100644 src/main/include/str/CommandLogitechController.h create mode 100644 src/main/include/str/LogitechController.h diff --git a/src/main/cpp/str/CommandLogitechController.cpp b/src/main/cpp/str/CommandLogitechController.cpp new file mode 100644 index 0000000..555db31 --- /dev/null +++ b/src/main/cpp/str/CommandLogitechController.cpp @@ -0,0 +1,81 @@ +#include "str/CommandLogitechController.h" + +using namespace frc2; + +CommandLogitechController::CommandLogitechController(int port) + : CommandGenericHID(port), m_hid{frc::LogitechController(port)} {} + +frc::LogitechController& CommandLogitechController::GetHID() { + return m_hid; +} + +Trigger CommandLogitechController::Thumb(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::kThumb, loop); +} + +Trigger CommandLogitechController::TriggerBtn(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::kTrigger, loop); +} + +Trigger CommandLogitechController::Three(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k3, loop); +} + +Trigger CommandLogitechController::Four(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k4, loop); +} + +Trigger CommandLogitechController::Five(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k5, loop); +} + +Trigger CommandLogitechController::Six(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k6, loop); +} + +Trigger CommandLogitechController::Seven(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k7, loop); +} + +Trigger CommandLogitechController::Eight(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k8, loop); +} + +Trigger CommandLogitechController::Nine(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k9, loop); +} + +Trigger CommandLogitechController::Ten(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k10, loop); +} + +Trigger CommandLogitechController::Eleven(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k11, loop); +} + +Trigger CommandLogitechController::Twelve(frc::EventLoop* loop) const { + return Button(frc::LogitechController::Button::k12, loop); +} + +Trigger CommandLogitechController::Slider(double threshold, + frc::EventLoop* loop) const { + return Trigger(loop, [this, threshold] { + return m_hid.GetSlider() > threshold; + }); +} + +double CommandLogitechController::GetX() const { + return m_hid.GetX(); +} + +double CommandLogitechController::GetY() const { + return m_hid.GetY(); +} + +double CommandLogitechController::GetZ() const { + return m_hid.GetZ(); +} + +double CommandLogitechController::GetSlider() const { + return m_hid.GetSlider(); +} \ No newline at end of file diff --git a/src/main/cpp/str/LogitechController.cpp b/src/main/cpp/str/LogitechController.cpp new file mode 100644 index 0000000..2cce588 --- /dev/null +++ b/src/main/cpp/str/LogitechController.cpp @@ -0,0 +1,246 @@ +#include "str/LogitechController.h" + +#include + +#include "frc/event/BooleanEvent.h" + +using namespace frc; + +LogitechController::LogitechController(int port) : GenericHID(port) {} + +double LogitechController::GetX() const { + return GetRawAxis(Axis::x); +} + +double LogitechController::GetY() const { + return GetRawAxis(Axis::y); +} + +double LogitechController::GetZ() const { + return GetRawAxis(Axis::z); +} + +double LogitechController::GetSlider() const { + return GetRawAxis(Axis::slider); +} + +BooleanEvent LogitechController::Slider(double threshold, EventLoop* loop) const { + return BooleanEvent(loop, [this, threshold] { return this->GetSlider() > threshold; }); +} + +BooleanEvent LogitechController::Slider(EventLoop* loop) const { + return Slider(0.1, loop); +} + +bool LogitechController::GetThumbButton() const { + return GetRawButton(Button::kThumb); +} + +bool LogitechController::GetThumbButtonPressed() { + return GetRawButtonPressed(Button::kThumb); +} + +bool LogitechController::GetThumbButtonReleased() { + return GetRawButtonReleased(Button::kThumb); +} + +BooleanEvent LogitechController::ThumbButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetThumbButton(); }); +} + +bool LogitechController::GetTriggerButton() const { + return GetRawButton(Button::kTrigger); +} + +bool LogitechController::GetTriggerButtonPressed() { + return GetRawButtonPressed(Button::kTrigger); +} + +bool LogitechController::GetTriggerButtonReleased() { + return GetRawButtonReleased(Button::kTrigger); +} + +BooleanEvent LogitechController::TriggerButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetTriggerButton(); }); +} + +bool LogitechController::GetThreeButton() const { + return GetRawButton(Button::k3); +} + +bool LogitechController::GetThreeButtonPressed() { + return GetRawButtonPressed(Button::k3); +} + +bool LogitechController::GetThreeButtonReleased() { + return GetRawButtonReleased(Button::k3); +} + +BooleanEvent LogitechController::ThreeButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetThreeButton(); }); +} + +bool LogitechController::GetFourButton() const { + return GetRawButton(Button::k4); +} + +bool LogitechController::GetFourButtonPressed() { + return GetRawButtonPressed(Button::k4); +} + +bool LogitechController::GetFourButtonReleased() { + return GetRawButtonReleased(Button::k4); +} + +BooleanEvent LogitechController::FourButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetFourButton(); }); +} + +bool LogitechController::GetFiveButton() const { + return GetRawButton(Button::k5); +} + +bool LogitechController::GetFiveButtonPressed() { + return GetRawButtonPressed(Button::k5); +} + +bool LogitechController::GetFiveButtonReleased() { + return GetRawButtonReleased(Button::k5); +} + +BooleanEvent LogitechController::FiveButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetFiveButton(); }); +} + +bool LogitechController::GetSixButton() const { + return GetRawButton(Button::k6); +} + +bool LogitechController::GetSixButtonPressed() { + return GetRawButtonPressed(Button::k6); +} + +bool LogitechController::GetSixButtonReleased() { + return GetRawButtonReleased(Button::k6); +} + +BooleanEvent LogitechController::SixButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetSixButton(); }); +} + +bool LogitechController::GetSevenButton() const { + return GetRawButton(Button::k7); +} + +bool LogitechController::GetSevenButtonPressed() { + return GetRawButtonPressed(Button::k7); +} + +bool LogitechController::GetSevenButtonReleased() { + return GetRawButtonReleased(Button::k7); +} + +BooleanEvent LogitechController::SevenButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetSevenButton(); }); +} + +bool LogitechController::GetEightButton() const { + return GetRawButton(Button::k8); +} + +bool LogitechController::GetEightButtonPressed() { + return GetRawButtonPressed(Button::k8); +} + +bool LogitechController::GetEightButtonReleased() { + return GetRawButtonReleased(Button::k8); +} + +BooleanEvent LogitechController::EightButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetEightButton(); }); +} + +bool LogitechController::GetNineButton() const { + return GetRawButton(Button::k9); +} + +bool LogitechController::GetNineButtonPressed() { + return GetRawButtonPressed(Button::k9); +} + +bool LogitechController::GetNineButtonReleased() { + return GetRawButtonReleased(Button::k9); +} + +BooleanEvent LogitechController::NineButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetNineButton(); }); +} + +bool LogitechController::GetTenButton() const { + return GetRawButton(Button::k10); +} + +bool LogitechController::GetTenButtonPressed() { + return GetRawButtonPressed(Button::k10); +} + +bool LogitechController::GetTenButtonReleased() { + return GetRawButtonReleased(Button::k10); +} + +BooleanEvent LogitechController::TenButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetTenButton(); }); +} + +bool LogitechController::GetElevenButton() const { + return GetRawButton(Button::k11); +} + +bool LogitechController::GetElevenButtonPressed() { + return GetRawButtonPressed(Button::k11); +} + +bool LogitechController::GetElevenButtonReleased() { + return GetRawButtonReleased(Button::k11); +} + +BooleanEvent LogitechController::ElevenButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetElevenButton(); }); +} + +bool LogitechController::GetTwelveButton() const { + return GetRawButton(Button::k12); +} + +bool LogitechController::GetTwelveButtonPressed() { + return GetRawButtonPressed(Button::k12); +} + +bool LogitechController::GetTwelveButtonReleased() { + return GetRawButtonReleased(Button::k12); +} + +BooleanEvent LogitechController::TwelveButton(EventLoop* loop) const { + return BooleanEvent(loop, [this]() { return this->GetTwelveButton(); }); +} + +void LogitechController::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("HID"); + builder.PublishConstString("ControllerType", "Logitech Extreme 3D Pro"); + builder.AddDoubleProperty("X", [this] { return GetX(); }, nullptr); + builder.AddDoubleProperty("Y", [this] { return GetY(); }, nullptr); + builder.AddDoubleProperty("Z", [this] { return GetZ(); }, nullptr); + builder.AddDoubleProperty("Slider", [this] { return GetSlider(); }, nullptr); + builder.AddBooleanProperty("ThumbButton", [this] { return GetThumbButton(); }, nullptr); + builder.AddBooleanProperty("TriggerButton", [this] { return GetTriggerButton(); }, nullptr); + builder.AddBooleanProperty("3", [this] { return GetThreeButton(); }, nullptr); + builder.AddBooleanProperty("4", [this] { return GetFourButton(); }, nullptr); + builder.AddBooleanProperty("5", [this] { return GetFiveButton(); }, nullptr); + builder.AddBooleanProperty("6", [this] { return GetSixButton(); }, nullptr); + builder.AddBooleanProperty("7", [this] { return GetSevenButton(); }, nullptr); + builder.AddBooleanProperty("8", [this] { return GetEightButton(); }, nullptr); + builder.AddBooleanProperty("9", [this] { return GetNineButton(); }, nullptr); + builder.AddBooleanProperty("10", [this] { return GetTenButton(); }, nullptr); + builder.AddBooleanProperty("11", [this] { return GetElevenButton(); }, nullptr); + builder.AddBooleanProperty("12", [this] { return GetTwelveButton(); }, nullptr); +} \ No newline at end of file diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp index 7a1f456..d17fc49 100644 --- a/src/main/cpp/str/SwerveDrive.cpp +++ b/src/main/cpp/str/SwerveDrive.cpp @@ -98,8 +98,13 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel, speedsToSend = frc::ChassisSpeeds::Discretize(speedsToSend, consts::LOOP_PERIOD); - SetModuleStates( - consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend), + fmt::print("x: {}, y: {}, rot: {}\n", speedsToSend.vx, speedsToSend.vy, speedsToSend.omega); + + std::array states = consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend); + + fmt::print("fl speed: {} | fl angle: {}\n", states[0].speed, states[0].angle.Degrees()); + + SetModuleStates(states, true, openLoop, ConvertModuleForcesToTorqueCurrent(xModuleForce, yModuleForce)); } @@ -163,8 +168,8 @@ void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); } else { - frc::DataLogManager::Log( - "WARNING: Vision pose was outside of field! Not adding to estimator!"); + // frc::DataLogManager::Log( + // "WARNING: Vision pose was outside of field! Not adding to estimator!"); } } @@ -200,7 +205,7 @@ void SwerveDrive::UpdateNTEntries() { currentStatesPub.Set(moduleStates); currentPositionsPub.Set(modulePositions); odomPosePub.Set(GetOdomPose()); - lookaheadPub.Set(GetPredictedPose(1_s, 1_s)); + //lookaheadPub.Set(GetPredictedPose(1_s, 1_s)); estimatorPub.Set(GetPose()); isSlippingPub.Set(IsSlipping()); odomUpdateRatePub.Set(odomUpdateRate.value()); diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index e401b31..ed23f0e 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -5,6 +5,7 @@ #pragma once #include +//#include #include #include @@ -35,6 +36,7 @@ class RobotContainer { void ConfigureBindings(); frc2::CommandXboxController driverController{0}; frc2::CommandXboxController operatorController{1}; + //frc2::CommandLogitechController sylaceController{2}; frc2::CommandPtr RumbleDriver(std::function timeToRumble); frc2::CommandPtr RumbleOperator( diff --git a/src/main/include/str/CommandLogitechController.h b/src/main/include/str/CommandLogitechController.h new file mode 100644 index 0000000..f2c8cce --- /dev/null +++ b/src/main/include/str/CommandLogitechController.h @@ -0,0 +1,75 @@ +#pragma once + +#include "str/LogitechController.h" +#include "frc2/command/button/Trigger.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/button/CommandGenericHID.h" + +namespace frc2 { + +class CommandLogitechController : public CommandGenericHID { + public: + /** + * Construct an instance of a controller. + * + * @param port The port index on the Driver Station that the controller is + * plugged into. + */ + explicit CommandLogitechController(int port); + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + frc::LogitechController& GetHID(); + + Trigger Thumb(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger TriggerBtn(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Three(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Four(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Five(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Six(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Seven(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Eight(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Nine(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Ten(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Eleven(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Twelve(frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + Trigger Slider(double threshold = 0.1, + frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + + double GetX() const; + double GetY() const; + double GetZ() const; + double GetSlider() const; + private: + frc::LogitechController m_hid; +}; +} // namespace frc2 diff --git a/src/main/include/str/LogitechController.h b/src/main/include/str/LogitechController.h new file mode 100644 index 0000000..9fee550 --- /dev/null +++ b/src/main/include/str/LogitechController.h @@ -0,0 +1,117 @@ +#pragma once + +#include +#include + +#include "frc/GenericHID.h" + +namespace frc { +class LogitechController : public GenericHID, + public wpi::Sendable, + public wpi::SendableHelper { + public: + explicit LogitechController(int port); + + ~LogitechController() override = default; + + LogitechController(LogitechController&&) = default; + LogitechController& operator=(LogitechController&&) = default; + + double GetX() const; + + double GetY() const; + + double GetZ() const; + + double GetSlider() const; + + BooleanEvent Slider(double threshold, EventLoop* loop) const; + + BooleanEvent Slider(EventLoop* loop) const; + + bool GetThumbButton() const; + bool GetThumbButtonPressed(); + bool GetThumbButtonReleased(); + BooleanEvent ThumbButton(EventLoop* loop) const; + + bool GetTriggerButton() const; + bool GetTriggerButtonPressed(); + bool GetTriggerButtonReleased(); + BooleanEvent TriggerButton(EventLoop* loop) const; + + bool GetThreeButton() const; + bool GetThreeButtonPressed(); + bool GetThreeButtonReleased(); + BooleanEvent ThreeButton(EventLoop* loop) const; + + bool GetFourButton() const; + bool GetFourButtonPressed(); + bool GetFourButtonReleased(); + BooleanEvent FourButton(EventLoop* loop) const; + + bool GetFiveButton() const; + bool GetFiveButtonPressed(); + bool GetFiveButtonReleased(); + BooleanEvent FiveButton(EventLoop* loop) const; + + bool GetSixButton() const; + bool GetSixButtonPressed(); + bool GetSixButtonReleased(); + BooleanEvent SixButton(EventLoop* loop) const; + + bool GetSevenButton() const; + bool GetSevenButtonPressed(); + bool GetSevenButtonReleased(); + BooleanEvent SevenButton(EventLoop* loop) const; + + bool GetEightButton() const; + bool GetEightButtonPressed(); + bool GetEightButtonReleased(); + BooleanEvent EightButton(EventLoop* loop) const; + + bool GetNineButton() const; + bool GetNineButtonPressed(); + bool GetNineButtonReleased(); + BooleanEvent NineButton(EventLoop* loop) const; + + bool GetTenButton() const; + bool GetTenButtonPressed(); + bool GetTenButtonReleased(); + BooleanEvent TenButton(EventLoop* loop) const; + + bool GetElevenButton() const; + bool GetElevenButtonPressed(); + bool GetElevenButtonReleased(); + BooleanEvent ElevenButton(EventLoop* loop) const; + + bool GetTwelveButton() const; + bool GetTwelveButtonPressed(); + bool GetTwelveButtonReleased(); + BooleanEvent TwelveButton(EventLoop* loop) const; + + struct Button { + static constexpr int kTrigger = 1; + static constexpr int kThumb = 2; + static constexpr int k3 = 3; + static constexpr int k4 = 4; + static constexpr int k5 = 5; + static constexpr int k6 = 6; + static constexpr int k7 = 7; + static constexpr int k8 = 8; + static constexpr int k9 = 9; + static constexpr int k10 = 10; + static constexpr int k11 = 11; + static constexpr int k12 = 12; + }; + + struct Axis { + static constexpr int x = 0; + static constexpr int y = 1; + static constexpr int z = 2; + static constexpr int slider = 3; + }; + + void InitSendable(wpi::SendableBuilder& builder) override; +}; + +} // namespace frc diff --git a/src/main/include/str/SwerveDrive.h b/src/main/include/str/SwerveDrive.h index c6924c8..95c9560 100644 --- a/src/main/include/str/SwerveDrive.h +++ b/src/main/include/str/SwerveDrive.h @@ -149,8 +149,8 @@ class SwerveDrive { std::array modulePositions; std::array moduleStates; - std::array xModuleForce; - std::array yModuleForce; + std::array xModuleForce{}; + std::array yModuleForce{}; frc::SwerveDriveOdometry<4> odom{consts::swerve::physical::KINEMATICS, frc::Rotation2d{0_deg}, modulePositions};