diff --git a/src/main/cpp/io/SwerveDriveIO.cpp b/src/main/cpp/io/SwerveDriveIO.cpp new file mode 100644 index 0000000..f463582 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/main/include/io/SwerveDriveIO.h b/src/main/include/io/SwerveDriveIO.h new file mode 100644 index 0000000..bebeccc --- /dev/null +++ b/src/main/include/io/SwerveDriveIO.h @@ -0,0 +1,110 @@ + +#pragma once + +#include + +#include +#include + +#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_; +}; \ No newline at end of file