swerve field relative behaviour added

CurtinFRC · Nov 22, 2023 · d0fd92b · d0fd92b
1 parent 0ccf05d
commit d0fd92b
Show file tree

Hide file tree

Showing 8 changed files with 62 additions and 11 deletions.
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -28,7 +28,8 @@ void wom::drivetrain::Drivetrain::SetSpeed(wom::drivetrain::TankSpeed speed) {
   _speed = speed;
 }
 
-void wom::drivetrain::Drivetrain::TankControl(double rightSpeed, double leftSpeed) {
+void wom::drivetrain::Drivetrain::TankControl(double rightSpeed,
+                                              double leftSpeed) {
   _config->left1.transmission->SetVoltage(leftSpeed * maxVolts);
   _config->left2.transmission->SetVoltage(leftSpeed * maxVolts);
   _config->left3.transmission->SetVoltage(leftSpeed * maxVolts);

diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -180,3 +180,7 @@ void wom::drivetrain::Swerve::OnUpdate(units::second_t        dt,
       break;
   }
 }
+
+wom::vision::Limelight wom::drivetrain::Swerve::GetLimelight() {
+  return _vision;
+}
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/DrivetrainBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/DrivetrainBehaviours.cpp
@@ -9,7 +9,7 @@ wom::drivetrain::behaviours::TankDrive::TankDrive(
 void wom::drivetrain::behaviours::TankDrive::OnTick(units::second_t dt) {
   wom::drivetrain::TankSpeed speed;
   speed.right = wom::utils::deadzone(_driver.GetRightY());
-  speed.left = wom::utils::deadzone(_driver.GetLeftY());
+  speed.left  = wom::utils::deadzone(_driver.GetLeftY());
   _drivebase->SetSpeed(speed);
   _drivebase->SetState(wom::drivetrain::DrivetrainState::kTank);
 }
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -0,0 +1,19 @@
+#include <drivetrain/behaviours/SwerveBehaviours.h>
+
+wom::drivetrain::behaviours::FieldRelativeSwerveDrive::FieldRelativeSwerveDrive(
+    wom::drivetrain::Swerve *swerve, frc::XboxController &driver)
+    : _swerve(swerve), _driver(driver) {}
+
+void wom::drivetrain::behaviours::FieldRelativeSwerveDrive::OnTick(
+    units::second_t dt) {
+  _swerve->SetState(wom::drivetrain::SwerveState::kFieldRelative);
+  frc::Pose3d currentPose = _swerve->GetLimelight().GetPose();
+  frc::Pose3d desiredPose = frc::Pose3d(
+      currentPose.X() + units::meter_t{_driver.GetRightX()},
+      currentPose.Y() + units::meter_t{_driver.GetRightY()}, currentPose.Z(),
+      frc::Rotation3d(currentPose.Rotation().X(), currentPose.Rotation().Y(),
+                      currentPose.Rotation().Z() +
+                          units::radian_t{std::atan(
+                              (_driver.GetLeftY() / _driver.GetLeftX()))}));
+  _swerve->OnUpdate(dt, _swerve->GetLimelight(), desiredPose);
+}
diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h
@@ -52,10 +52,10 @@ namespace drivetrain {
 
    protected:
    private:
-    DrivetrainConfig    *_config;
-    DrivetrainState      _state;
-    units::volt_t        maxVolts = 9_V;
-    TankSpeed _speed;
+    DrivetrainConfig *_config;
+    DrivetrainState   _state;
+    units::volt_t     maxVolts = 9_V;
+    TankSpeed         _speed;
   };
 }  // namespace drivetrain
 }  // namespace wom
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -82,7 +82,6 @@ namespace drivetrain {
     wom::utils::PIDController<units::meters, units::meters_per_second>
         _movementPID;
 
-
     units::volt_t voltageRotation;
     units::volt_t voltageMovement;
 
@@ -114,8 +113,9 @@ namespace drivetrain {
     explicit Swerve(SwerveConfig config, SwerveState state,
                     wom::vision::Limelight vision);
 
-    SwerveConfig GetConfig();
-    SwerveState  GetState();
+    SwerveConfig           GetConfig();
+    SwerveState            GetState();
+    wom::vision::Limelight GetLimelight();
 
     void SetState(SwerveState state);
     void FieldRelativeControl(frc::Pose3d desiredPose, units::second_t dt);

diff --git a/wombat/src/main/include/drivetrain/behaviours/DrivetrainBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/DrivetrainBehaviours.h
@@ -10,14 +10,15 @@ namespace drivetrain {
   namespace behaviours {
     class TankDrive : public behaviour::Behaviour {
      public:
-      TankDrive(wom::drivetrain::Drivetrain *drivebase, frc::XboxController &driver);
+      TankDrive(wom::drivetrain::Drivetrain *drivebase,
+                frc::XboxController         &driver);
 
       void OnTick(units::second_t dt);
 
      protected:
      private:
       wom::drivetrain::Drivetrain *_drivebase;
-      frc::XboxController &_driver;
+      frc::XboxController         &_driver;
     };
   }  // namespace behaviours
 }  // namespace drivetrain

diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
+
+namespace wom {
+namespace drivetrain {
+  namespace behaviours {
+
+    class FieldRelativeSwerveDrive : public behaviour::Behaviour {
+     public:
+      explicit FieldRelativeSwerveDrive(wom::drivetrain::Swerve *swerve,
+                                        frc::XboxController     &driver);
+
+      void OnTick(units::second_t dt) override;
+
+     private:
+      wom::drivetrain::Swerve *_swerve;
+      frc::XboxController     &_driver;
+    };
+
+  }  // namespace behaviours
+}  // namespace drivetrain
+}  // namespace wom
-Original file line number
+Diff line change
@@ Expand Up / @@ -180,3 +180,7 @@ void wom::drivetrain::Swerve::OnUpdate(units::second_t dt, @@
           break;
       }
     }
+    wom::vision::Limelight wom::drivetrain::Swerve::GetLimelight() {
+      return _vision;
+    }