Feat/swerve drive support

src/main/cpp/RobotContainer.cpp

@@ -29,6 +29,21 @@ RobotContainer::RobotContainer() {
     drivetrain_ = std::make_shared<Drivetrain>(drivetrain_sw_.get());
     VOKC_CALL(drivetrain_->Init());
 
+
+    // == swerve drive ==
+    std::shared_ptr<SwerveDriveHardwareInterface> swerve_drive_hw;
+    VOKC_CALL(SetupSwerveDriveInterface(hardware_, &swerve_drive_hw));
+
+    // Initialize the software interface
+    swerve_drive_sw_ = std::make_shared<SwerveDriveSoftwareInterface>();
+
+    // Link SwerveDriveIO to hardware / software
+    swerve_drive_io_ = std::make_shared<SwerveDriveIO>(swerve_drive_hw.get(), swerve_drive_sw_.get());
+
+    // Link intake software to the I/O
+    swerve_drive_ = std::make_shared<SwerveDrive>(swerve_drive_sw_.get());
+
+
     // == intake ==
     std::shared_ptr<IntakeHardwareInterface> intake_hw;
     VOKC_CALL(SetupIntakeInterface(hardware_, &intake_hw));

src/main/cpp/SwerveModule.cpp

@@ -0,0 +1,167 @@
+
+#include "SwerveModule.h"
+
+bool SwerveModule::Init(Location loc) {
+    // PID Controller stuff
+    // drive PID gains
+    double drive_kP = RobotParams::GetParam("swerve.drive_pid.kP", 0.0);
+    double drive_kI = RobotParams::GetParam("swerve.drive_pid.kI", 0.001);
+    double drive_kD = RobotParams::GetParam("swerve.drive_pid.kD", 0.0001);
+
+    // steer PID gains
+    double steer_kP = RobotParams::GetParam("swerve.steer_pid.kP", 0.0);
+    double steer_kI = RobotParams::GetParam("swerve.steer_pid.kI", 0.001);
+    double steer_kD = RobotParams::GetParam("swerve.steer_pid.kD", 0.0001);
+
+    // create our PIDs
+    drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD);
+    steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD);
+
+
+    // create a default swerve module state with no speed or angle
+    state = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d());
+
+    // create a default swerve module position with no distance traveled or angle    
+    pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d());
+
+    this->location = loc;
+
+    // physical location on the robot
+    double x_disp = RobotParams::GetParam("swerve.x_disp", 0.3); // in meters
+    double y_disp = RobotParams::GetParam("swerve.y_disp", 0.3); // in meters
+
+    // based on where it needs to be
+    switch(loc) {
+        case Location::LEFT_FRONT:
+            // we only have this to pass it into the kinematics object, because WPILib needs it
+            // positive x is to the front of the robot, positive y is to the left of the robot
+            // this should match the code in the docs pretty well, I think
+            trans = frc::Translation2d(units::meter_t(x_disp), units::meter_t(y_disp));
+            break;
+        case Location::LEFT_BACK:
+            trans = frc::Translation2d(units::meter_t(-x_disp), units::meter_t(y_disp));
+            break;
+        case Location::RIGHT_FRONT:
+            trans = frc::Translation2d(frc::Translation2d(units::meter_t(x_disp), units::meter_t(-y_disp)));
+            break;
+        case Location::RIGHT_BACK:
+            trans = frc::Translation2d(frc::Translation2d(units::meter_t(-x_disp), units::meter_t(-y_disp)));
+            break;
+        default:
+            // we shouldn't have reached here, so throw an error
+            return false;
+    }
+
+    // reset subsystem to initial state
+    this->Reset();
+
+    // Init passed succesffully, return true
+    return true;
+}
+
+bool SwerveModule::Reset() {
+    // reset PIDs
+    this->drive_pid.Reset();
+    this->steer_pid.Reset();
+
+    //TODO reset sensor readings
+    //TODO reset WPILib kinematics stuff
+    return true;
+}
+
+bool SwerveModule::GetLocationOnRobot(frc::Translation2d *trans) {
+    // OKC_CHECK(this->trans != nullptr);
+
+    *trans = this->trans;
+
+    return true;
+}
+
+bool SwerveModule::GetSwerveModulePosition(frc::SwerveModulePosition *pos) {
+    // OKC_CHECK(this->pos != nullptr);
+
+    *pos = this->pos;
+
+    return true;
+}
+
+bool SwerveModule::GetSwerveModuleState(frc::SwerveModuleState *state) {
+    // OKC_CHECK(this->state != nullptr);
+
+    *state = this->state;
+
+    return true;
+}
+
+bool SwerveModule::SetDesiredState(frc::SwerveModuleState state) {
+    // OKC_CHECK(this->state != nullptr);
+
+    this->state = state;
+
+    return true;
+}
+
+bool SwerveModule::GetDriveOutput(double *output) {
+    //TODO check for null pointers
+
+    // set setpoint
+    this->drive_pid.SetSetpoint(this->state.speed.value());
+
+    // calculate output
+    *output = this->drive_pid.Calculate(this->drive_enc_vel);
+
+    return true;
+}
+
+bool SwerveModule::GetSteerOutput(double *output) {
+    // TODO check for null pointers
+
+    // optimize angle
+    this->state = frc::SwerveModuleState::Optimize(this->state, frc::Rotation2d(units::degree_t(this->pos.angle.Degrees())));
+
+    // set setpoint
+    this->steer_pid.SetSetpoint(this->state.angle.Degrees().value());
+
+    *output = this->steer_pid.Calculate(this->steer_enc_vel);
+
+    return true;
+}
+
+bool SwerveModule::SetDrivePID(double kP, double kI, double kD) {
+    //TODO null pointer checks
+    //OKC_CHECK(this->drive_pid != nullptr);
+
+    this->drive_pid.SetPID(kP, kI, kD);
+
+    return true;
+}
+
+bool SwerveModule::SetSteerPID(double kP, double kI, double kD) {
+    //TODO null pointer checks
+    //OKC_CHECK(this->steer_pid != nullptr);
+
+    this->steer_pid.SetPID(kP, kI, kD);
+
+    return true;
+}
+
+
+bool SwerveModule::Update(double drive_enc, double steer_enc, double drive_vel, double steer_vel) {
+    // update the SwerveModulePosition with the given sensor readings
+
+    // 6.75:1 L2 gear ratio
+    // wheel is 4 inch diamete wheel
+    drive_enc = drive_enc / 6.75 * 3.14159265358979323846264338327950288419716939937510582097494459230781 * 4;
+
+    // steering gear ratio of 12.8:1
+    //TODO fix steer encoder readings (like, converting from raw voltage value or whatever to this)
+    steer_enc = steer_enc / 12.8;
+
+    this->pos = frc::SwerveModulePosition(units::meter_t(drive_enc), frc::Rotation2d(units::degree_t(steer_enc)));
+
+    // velocity readings
+    this->drive_enc_vel = drive_vel;
+    this->steer_enc_vel = steer_vel;
+
+    return true;
+}

src/main/cpp/commands/swerve/AutoSwerveCommand.cpp

@@ -0,0 +1,46 @@
+
+#include "commands/swerve/AutoSwerveCommand.h"
+
+AutoSwerveCommand::AutoSwerveCommand(std::shared_ptr<SwerveDrive> swerve, frc::Pose2d f_pos) {
+    swerve_ = swerve;
+    end_pos = f_pos;
+
+    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));
+}
+
+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
+    }
+
+    bool *at;
+
+    // if the swerve drive is at its setpoint
+    if (swerve_->AtSetpoint(at)) {
+        return true; // end the command
+    }
+
+    // otherwise, continue
+    return false;
+}

src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp

@@ -0,0 +1,35 @@
+
+#include "commands/swerve/TeleOpSwerveCommand.h"
+
+TeleOpSwerveCommand::TeleOpSwerveCommand(std::shared_ptr<SwerveDrive> swerve, std::shared_ptr<frc::Joystick> gamepad) {
+    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 drive_power = -this->gamepad_->GetRawAxis(1);
+    double strafe_power = this->gamepad_->GetRawAxis(2);
+    double turn_power = this->gamepad_->GetRawAxis(3);
+
+    VOKC_CALL(swerve_->TeleOpDrive(drive_power, strafe_power, turn_power));
+}
+
+void TeleOpSwerveCommand::End(bool interrupted) {
+    // do nothing lol
+}
+
+bool TeleOpSwerveCommand::IsFinished() {
+    // this command should never end, unless it is interrupted by another
+    return false;
+}

src/main/cpp/hardware/HardwareInterface.cpp

@@ -55,6 +55,40 @@ bool SetupDrivetrainInterface(
     return true;
 }
 
+bool SetupSwerveDriveInterface(
+    std::unique_ptr<HardwareInterface> &hardware,
+    std::shared_ptr<SwerveDriveHardwareInterface> *interface) {
+    OKC_CHECK(interface != nullptr);
+    OKC_CHECK(hardware->actuators != nullptr);
+    OKC_CHECK(hardware->sensors != nullptr);
+
+    // Get actuators interface for swerve drive.
+    std::unique_ptr<ActuatorInterface> &actuators = hardware->actuators;
+    std::unique_ptr<SensorInterface> &sensors = hardware->sensors;
+
+    // set up swerve drive interface.
+    SwerveDriveHardwareInterface swerve_drive_interface = {
+        actuators->left_front_drive_motor.get(),
+        actuators->left_back_drive_motor.get(),
+        actuators->right_front_drive_motor.get(),
+        actuators->right_back_drive_motor.get(),
+        actuators->left_front_steer_motor.get(),
+        actuators->left_back_steer_motor.get(),
+        actuators->right_front_steer_motor.get(),
+        actuators->right_back_steer_motor.get(),
+
+        sensors->ahrs.get(),
+
+        sensors->left_front_steer_encoder.get(),
+        sensors->left_back_steer_encoder.get(),
+        sensors->right_front_steer_encoder.get(),
+        sensors->right_back_steer_encoder.get(),
+    };
+
+    // set the output interface
+    *interface = std::make_shared<SwerveDriveHardwareInterface>(swerve_drive_interface);
+}
+
 bool SetupIntakeInterface(std::unique_ptr<HardwareInterface> &hardware,
                           std::shared_ptr<IntakeHardwareInterface> *interface) {
     OKC_CHECK(interface != nullptr);