From d0fd92b0a99515f7bab9604921bf1bbb7bf6e543 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Wed, 22 Nov 2023 22:17:55 +0800 Subject: [PATCH] swerve field relative behaviour added --- wombat/src/main/cpp/drivetrain/Drivetrain.cpp | 3 ++- .../src/main/cpp/drivetrain/SwerveDrive.cpp | 4 +++ .../behaviours/DrivetrainBehaviours.cpp | 2 +- .../behaviours/SwerveBehaviours.cpp | 19 ++++++++++++++ .../src/main/include/drivetrain/Drivetrain.h | 8 +++--- .../src/main/include/drivetrain/SwerveDrive.h | 6 ++--- .../behaviours/DrivetrainBehaviours.h | 5 ++-- .../drivetrain/behaviours/SwerveBehaviours.h | 26 +++++++++++++++++++ 8 files changed, 62 insertions(+), 11 deletions(-) create mode 100644 wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp create mode 100644 wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp index c5932fa8..064d58eb 100644 --- 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 index 76dbeb9a..23c0d89b 100644 --- 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 index 52ae80d2..9991849e 100644 --- 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 new file mode 100644 index 00000000..1122278e --- /dev/null +++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp @@ -0,0 +1,19 @@ +#include + +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 index 1b886f41..72fe9378 100644 --- 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 index 055bd53d..7ac83a0a 100644 --- 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 _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 index 3451c310..87a2cb40 100644 --- 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 new file mode 100644 index 00000000..0a17e97b --- /dev/null +++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +#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