created swerve drive IO per #11

still has a lot of functionality to be written and planned, but the basic skeleton is there

src/main/cpp/io/SwerveDriveIO.cpp

@@ -0,0 +1,108 @@
+
+#include "io/SwerveDriveIO.h"
+
+void SwerveDriveIO::Periodic() {
+    // Process all the inputs and outputs to/from high level software.
+    VOKC_CALL(ProcessIO());
+}
+
+void SwerveDriveIO::SimulationPeriodic() {
+    // SimulationPeriodic
+}
+
+bool SwerveDriveIO::ProcessIO() {
+    OKC_CHECK(sw_interface_ != nullptr);
+    OKC_CHECK(hw_interface_ != nullptr);
+
+    // Set the software outputs
+    // If the swerve drive configuration needs to be updated, update it.
+    if (sw_interface_->update_config) {
+        OKC_CALL(UpdateDriveConfig(sw_interface_->swerve_drive_config));
+
+        // Lower the update flag
+        sw_interface_->update_config = false;
+    }
+
+    // If the encoders should be reset, reset them
+    if (sw_interface_->reset_drive_encoders) {
+        OKC_CALL(ResetDriveEncoders());
+
+        // Lower the encoder reset flag
+        sw_interface_->reset_drive_encoders = false;
+    }
+
+    if (sw_interface_->reset_steer_encoders) {
+        OKC_CALL(ResetSteerEncoders());
+
+        // Lower the encoder reset flag
+        sw_interface_->reset_steer_encoders = false;
+    }
+
+    // If the navX should be reset, reset it.
+    if (sw_interface_->reset_gyro) {
+        hw_interface_->ahrs->Reset();
+
+        // Lower the navX reset flag
+        sw_interface_->reset_gyro = false;
+    }
+
+    // Set the drive outputs based on the drive mode.
+    //TODO
+
+    // Get the hardware sensor values.
+    // navX IMU:
+    sw_interface_->imu_yaw = hw_interface_->ahrs->GetAngle();
+
+    // Encoders
+    //TODO
+
+    return true;
+}
+
+bool SwerveDriveIO::UpdateDriveConfig(SwerveDriveConfig &config) {
+    OKC_CHECK(hw_interface_ != nullptr);
+
+    // Get the configuration
+    double open_loop_ramp_drive = config.open_loop_ramp_rate_drive;
+    double open_loop_ramp_steer = config.open_loop_ramp_rate_steer;
+    double max_output_drive = config.max_output_drive;
+    double max_output_steer = config.max_output_steer;
+
+    // Apply the configuration
+    // Open Loop Ramp Rate
+    hw_interface_->left_front_drive_motor->SetOpenLoopRampRate(open_loop_ramp_drive);
+    hw_interface_->left_back_drive_motor->SetOpenLoopRampRate(open_loop_ramp_drive);
+    hw_interface_->right_front_drive_motor->SetOpenLoopRampRate(open_loop_ramp_drive);
+    hw_interface_->right_back_drive_motor->SetOpenLoopRampRate(open_loop_ramp_drive);
+
+    hw_interface_->left_front_steer_motor->SetOpenLoopRampRate(open_loop_ramp_steer);
+    hw_interface_->left_back_steer_motor->SetOpenLoopRampRate(open_loop_ramp_steer);
+    hw_interface_->right_front_steer_motor->SetOpenLoopRampRate(open_loop_ramp_steer);
+    hw_interface_->right_back_steer_motor->SetOpenLoopRampRate(open_loop_ramp_steer);
+
+    // Swerve drive Max Output
+    //TODO drive motors
+    //TODO steer motors
+
+    return true;
+}
+
+bool SwerveDriveIO::ResetDriveEncoders() {
+    OKC_CHECK(hw_interface_ != nullptr);
+
+    // internal encoders
+    hw_interface_->left_front_drive_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->left_back_drive_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->right_front_drive_motor->GetEncoder().SetPosition(0.0);
+    hw_interface_->right_back_drive_motor->GetEncoder().SetPosition(0.0);
+
+    return true;
+}
+
+bool SwerveDriveIO::ResetSteerEncoders() {
+    OKC_CHECK(hw_interface_ != nullptr);
+
+    //TODO
+
+    return true;
+}

src/main/include/io/SwerveDriveIO.h

@@ -0,0 +1,110 @@
+
+#pragma once
+
+#include <memory>
+
+#include <frc2/command/SubsystemBase.h>
+#include <rev/CANSparkMax.h>
+
+#include "AHRS.h"
+#include "Utils.h"
+
+
+typedef struct swerve_drive_config_t {
+    double max_output_drive;
+    double open_loop_ramp_rate_drive;
+
+    double max_output_steer;
+    double open_loop_ramp_rate_steer;
+} SwerveDriveConfig;
+
+typedef struct swerve_drive_hardware_interface_t {
+    // motors
+    rev::CANSparkMax *const left_front_drive_motor;
+    rev::CANSparkMax *const left_back_drive_motor;
+
+    rev::CANSparkMax *const right_front_drive_motor;
+    rev::CANSparkMax *const right_back_drive_motor;
+
+    rev::CANSparkMax *const left_front_steer_motor;
+    rev::CANSparkMax *const left_back_steer_motor;
+
+    rev::CANSparkMax *const right_front_steer_motor;
+    rev::CANSparkMax *const right_back_steer_motor;
+
+    // AHRS
+    AHRS *const ahrs;
+
+} SwerveDriveHardwareInterface;
+
+typedef struct swerve_drive_software_interface_t {
+    // SW INPUTS
+    // IMU yaw angle
+    double imu_yaw;
+
+    // Encoders
+    double left_front_drive_motor_enc;
+    double left_back_drive_motor_enc;
+
+    double right_front_drive_motor_enc;
+    double right_back_drive_motor_enc;
+
+    double left_front_steer_motor_enc;
+    double left_back_steer_motor_enc;
+
+    double right_front_steer_motor_enc;
+    double right_back_steer_motor_enc;
+
+
+    // SW OUTPUTS
+    // Configure swerve drive variables
+    SwerveDriveConfig drive_config;
+    bool update_config;
+
+    // Input squaring
+    bool square_inputs;
+
+    // Holonomic drive inputs
+    double drive_power;
+    double strafe_power;
+    double turn_power;
+
+    // auto inputs
+    double translation_x;
+    double translation_y;
+    double rotation;
+
+    // Reset flags
+    bool reset_drive_encoders;
+    bool reset_steer_encoders;
+    bool reset_gyro;
+
+} SwerveDriveSoftwareInterface;
+
+class SwerveDriveIO : public frc2::SubsystemBase {
+public:
+    SwerveDriveIO(SwerveDriveHardwareInterface *hw_interface,
+                 SwerveDriveSoftwareInterface *sw_interface)
+        : hw_interface_(hw_interface), sw_interface_(sw_interface) {}
+
+    /**
+     * Will be called periodically whenever the CommandScheduler runs.
+     */
+    void Periodic() override;
+
+    /**
+     * Will be called periodically whenever the CommandScheduler runs during
+     * simulation.
+     */
+    void SimulationPeriodic() override;
+
+    bool ProcessIO();
+
+private:
+    bool UpdateDriveConfig(SwerveDriveConfig &config);
+    bool ResetDriveEncoders();
+    bool ResetSteerEncoders();
+
+    SwerveDriveHardwareInterface *const hw_interface_;
+    SwerveDriveSoftwareInterface *const sw_interface_;
+};