From a120dd95059ff88fe92eadd897fcb559214b023b Mon Sep 17 00:00:00 2001
From: danielbrownmsm <daniel.brown@msmrockets.org>
Date: Tue, 3 Jan 2023 19:18:31 -0600
Subject: [PATCH] created teleop and auto commands for the swerve drive they do
 not currently work. ticket: #11

---
 .../cpp/commands/swerve/AutoSwerveCommand.cpp | 45 +++++++++++++++++++
 .../commands/swerve/TeleOpSwerveCommand.cpp   | 33 ++++++++++++++
 .../commands/swerve/AutoSwerveCommand.h       | 33 ++++++++++++++
 .../commands/swerve/TeleOpSwerveCommand.h     | 32 +++++++++++++
 4 files changed, 143 insertions(+)
 create mode 100644 src/main/cpp/commands/swerve/AutoSwerveCommand.cpp
 create mode 100644 src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp
 create mode 100644 src/main/include/commands/swerve/AutoSwerveCommand.h
 create mode 100644 src/main/include/commands/swerve/TeleOpSwerveCommand.h

diff --git a/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp b/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp
new file mode 100644
index 0000000..2479614
--- /dev/null
+++ b/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp
@@ -0,0 +1,45 @@
+
+#include "commands/swerve/AutoSwerveCommand.h"
+
+AutoSwerveCommand::AutoSwerveCommand(std::shared_ptr<SwerveDrive> swerve, frc::Translation2d f_pos, frc::Rotation2d f_angle) {
+    swerve_ = swerve;
+    end_pos = f_pos;
+    final_angle = f_angle;
+
+    if (swerve_ != nullptr) {
+        this->AddRequirements(swerve_.get());
+    }
+}
+
+void AutoSwerveCommand::Initialize() {
+    VOKC_CHECK(swerve_ != nullptr);
+
+    VOKC_CALL(swerve_->ResetDriveEncoders());
+    VOKC_CALL(swerve_->ResetSteerEncoders());
+    VOKC_CALL(swerve_->ResetPIDs());
+}
+
+void AutoSwerveCommand::Execute() {
+    VOKC_CHECK(swerve_ != nullptr);
+
+    VOKC_CALL(swerve_->TranslateAuto(end_pos, final_angle));
+}
+
+void AutoSwerveCommand::End(bool executed) {
+    return;
+}
+
+bool AutoSwerveCommand::IsFinished() {
+    // if the swerve subsystem doesn't exist
+    if (swerve_ == nullptr) {
+        return true; // then don't try to do anything on it because it will fail
+    }
+
+    // if the swerve drive is at its setpoint
+    if (swerve_->AtSetpoint()) {
+        return true; // end the command
+    }
+
+    // otherwise, continue
+    return false;
+}
diff --git a/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp
new file mode 100644
index 0000000..56f8e1a
--- /dev/null
+++ b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp
@@ -0,0 +1,33 @@
+
+#include "commands/swerve/TeleOpSwerveCommand.h"
+
+TeleOpSwerveCommand::TeleOpSwerveCommand(std::shared_ptr<SwerveDrive> swerve, std::shared_ptr<frc::Joystick> gamepad) {
+    // Set everything.
+    swerve_ = swerve;
+    gamepad_ = gamepad;
+
+    if (swerve_ != nullptr) {
+        this->AddRequirements(swerve_.get());
+    }
+}
+
+void TeleOpSwerveCommand::Initialize() {
+    VOKC_CHECK(swerve_ != nullptr);
+}
+
+void TeleOpSwerveCommand::Execute() {
+    VOKC_CHECK(swerve_ != nullptr);
+    VOKC_CHECK(gamepad_ != nullptr);
+
+    double throttle = -1.0 * pow(gamepad_->GetRawAxis(1), 3);
+    double turn_power = pow(gamepad_->GetRawAxis(4), 3);
+
+    // TODO actually get correct joystick inputs
+
+    VOKC_CALL(swerve_->TeleOpDrive(throttle, turn_power));
+}
+
+bool TeleOpSwerveCommand::IsFinished() {
+    // this command should never end, unless it is interrupted by another
+    return false;
+}
diff --git a/src/main/include/commands/swerve/AutoSwerveCommand.h b/src/main/include/commands/swerve/AutoSwerveCommand.h
new file mode 100644
index 0000000..8610304
--- /dev/null
+++ b/src/main/include/commands/swerve/AutoSwerveCommand.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "Utils.h"
+#include "subsystems/SwerveDrive.h"
+
+
+/**
+ *
+ */
+class AutoSwerveCommand
+    : public frc2::CommandHelper<frc2::CommandBase, AutoSwerveCommand> {
+public:
+    /**
+     * Creates a new AutoSwerveCommand.
+     * Should be the only autonomous swerve-drive-related command we need
+     *
+     * @param swerve The swerve drive used by this command.
+     */
+    explicit AutoSwerveCommand(std::shared_ptr<SwerveDrive> swerve, frc::Translation2d end_pos, frc::Rotation2d final_angle);
+
+    void Initialize() override;
+    void Execute() override;
+    void End(bool executed) override;
+    bool IsFinished() override;
+
+private:
+    std::shared_ptr<SwerveDrive> swerve_;
+    frc::Translation2d end_pos;
+    frc::Rotation2d final_angle;
+};
diff --git a/src/main/include/commands/swerve/TeleOpSwerveCommand.h b/src/main/include/commands/swerve/TeleOpSwerveCommand.h
new file mode 100644
index 0000000..d62f528
--- /dev/null
+++ b/src/main/include/commands/swerve/TeleOpSwerveCommand.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <frc/Joystick.h>
+
+#include "Utils.h"
+#include "subsystems/SwerveDrive.h"
+
+
+/**
+ * 
+ */
+class TeleOpSwerveCommand
+    : public frc2::CommandHelper<frc2::CommandBase, TeleOpSwerveCommand> {
+public:
+    /**
+     * Creates a new TeleOpSwerveCommand.
+     *
+     * @param swerve The swerve drive subsystem used by this command.
+     */
+    explicit TeleOpSwerveCommand(std::shared_ptr<SwerveDrive> swerve, std::shared_ptr<frc::Joystick> gamepad);
+
+    void Initialize() override;
+    void Execute() override;
+    void End(bool executed) override;
+    bool IsFinished() override;
+
+private:
+    std::shared_ptr<SwerveDrive> swerve_;
+    std::shared_ptr<frc::Joystick> gamepad_;
+};