working on wierd swerve

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();
+}

src/main/cpp/str/LogitechController.cpp

@@ -0,0 +1,246 @@
+#include "str/LogitechController.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#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);
+}

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<frc::SwerveModuleState, 4> 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());

src/main/include/RobotContainer.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <frc2/command/CommandPtr.h>
+//#include <str/CommandLogitechController.h>
 #include <frc2/command/button/CommandXboxController.h>
 
 #include <functional>
@@ -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<units::second_t()> timeToRumble);
   frc2::CommandPtr RumbleOperator(