From e2e4ac850a30d3a846afb7ac50752b97e4bd9b20 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 24 Dec 2022 11:08:36 -0600 Subject: [PATCH 01/36] add swerve drive motors to actuator interface --- src/main/include/hardware/ActuatorInterface.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/include/hardware/ActuatorInterface.h b/src/main/include/hardware/ActuatorInterface.h index e05179e..17856b6 100644 --- a/src/main/include/hardware/ActuatorInterface.h +++ b/src/main/include/hardware/ActuatorInterface.h @@ -33,6 +33,19 @@ typedef struct actuator_interface_t { std::unique_ptr right_motor_2; std::unique_ptr right_motor_3; + // swerve drive motors + std::unique_ptr left_front_drive_motor; + std::unique_ptr left_back_drive_motor; + + std::unique_ptr right_front_drive_motor; + std::unique_ptr right_back_drive_motor; + + std::unique_ptr left_front_steer_motor; + std::unique_ptr left_back_steer_motor; + + std::unique_ptr right_front_steer_motor; + std::unique_ptr right_back_steer_motor; + // intake motors std::unique_ptr intake_position_motor; std::unique_ptr intake_motor; From 4d65374a6559c07d10c0fbf6cb1a2ddc0ba3b598 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 24 Dec 2022 11:08:51 -0600 Subject: [PATCH 02/36] add swerve drive absolute steering encoders to sensor interface --- src/main/include/hardware/SensorInterface.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/include/hardware/SensorInterface.h b/src/main/include/hardware/SensorInterface.h index 75df1b1..c67e9db 100644 --- a/src/main/include/hardware/SensorInterface.h +++ b/src/main/include/hardware/SensorInterface.h @@ -6,6 +6,7 @@ #include "AHRS.h" #include #include +#include // == sensor ports == #define DEPLOY_LIMIT_SWITCH 2 @@ -18,4 +19,9 @@ typedef struct sensors_t { // intake limit switches std::unique_ptr deploy_limit_switch; std::unique_ptr retracted_limit_switch; + + std::unique_ptr left_front_steer_encoder; + std::unique_ptr left_back_steer_encoder; + std::unique_ptr right_front_steer_encoder; + std::unique_ptr right_back_steer_encoder; } SensorInterface; From e8767c6b2b1514906151efba9adf6c3930482dac Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 24 Dec 2022 11:14:19 -0600 Subject: [PATCH 03/36] create swerve drive hardware interface per #11 --- src/main/cpp/hardware/HardwareInterface.cpp | 29 +++++++++++++++++++ src/main/include/hardware/HardwareInterface.h | 14 +++++++++ 2 files changed, 43 insertions(+) diff --git a/src/main/cpp/hardware/HardwareInterface.cpp b/src/main/cpp/hardware/HardwareInterface.cpp index fe8c0da..f132ed1 100644 --- a/src/main/cpp/hardware/HardwareInterface.cpp +++ b/src/main/cpp/hardware/HardwareInterface.cpp @@ -44,6 +44,35 @@ bool SetupDrivetrainInterface( return true; } +bool SetupSwerveDriveInterface( + std::unique_ptr &hardware, + std::shared_ptr *interface) { + OKC_CHECK(interface != nullptr); + OKC_CHECK(hardware->actuators != nullptr); + OKC_CHECK(hardware->sensors != nullptr); + + // Get actuators interface for swerve drive. + std::unique_ptr &actuators = hardware->actuators; + std::unique_ptr &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(), + }; + + // set the output interface + *interface = std::make_shared(swerve_drive_interface); +} + bool SetupIntakeInterface( std::unique_ptr &hardware, std::shared_ptr *interface) { diff --git a/src/main/include/hardware/HardwareInterface.h b/src/main/include/hardware/HardwareInterface.h index 89c8c20..2c84482 100644 --- a/src/main/include/hardware/HardwareInterface.h +++ b/src/main/include/hardware/HardwareInterface.h @@ -9,6 +9,7 @@ // Subsystem I/O #include "io/DrivetrainIO.h" +#include "io/SwerveDriveIO.h" #include "io/IntakeIO.h" typedef struct hardware_t { @@ -36,6 +37,19 @@ bool SetupDrivetrainInterface( std::unique_ptr &hardware, std::shared_ptr *interface); +/** + * @brief Link the Swerve drive to the hardware interfaces. + * + * @param interface + * @return true + * @return false + */ +bool SetupSwerveDriveInterface( + std::unique_ptr &hardware, + std::shared_ptr *interface); + + + /** * @brief Link the Intake to the hardware interfaces. * From b5f4cb80b49543e00e021c57c44cf430160a76f5 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 24 Dec 2022 11:24:35 -0600 Subject: [PATCH 04/36] 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 | 108 +++++++++++++++++++++++++++ src/main/include/io/SwerveDriveIO.h | 110 ++++++++++++++++++++++++++++ 2 files changed, 218 insertions(+) create mode 100644 src/main/cpp/io/SwerveDriveIO.cpp create mode 100644 src/main/include/io/SwerveDriveIO.h 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 From 8cc81373ee40e5c09f99a337eb986587e50f4fbc Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 24 Dec 2022 11:25:11 -0600 Subject: [PATCH 05/36] fixes bug with drive config from last commit issue: #11 --- src/main/cpp/io/SwerveDriveIO.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/io/SwerveDriveIO.cpp b/src/main/cpp/io/SwerveDriveIO.cpp index f463582..380c43d 100644 --- a/src/main/cpp/io/SwerveDriveIO.cpp +++ b/src/main/cpp/io/SwerveDriveIO.cpp @@ -17,7 +17,7 @@ bool SwerveDriveIO::ProcessIO() { // 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)); + OKC_CALL(UpdateDriveConfig(sw_interface_->drive_config)); // Lower the update flag sw_interface_->update_config = false; From 3244231ed2e7659b7de4a037991c1a9dd4678de4 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 26 Dec 2022 22:01:04 -0600 Subject: [PATCH 06/36] adds swerve drive parameters to the parameters.toml file per #11 --- src/main/deploy/parameters.toml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 67c916b..14d6efd 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -74,3 +74,9 @@ camera_angle = 0 # deg Kp = 0.03 Ki = 0 Kd = 0.001 + +[swerve] +# assumes our modules are located in a square, everything should be the same distance from the center +# units are in meters +x_disp = 0.3 +y_disp = 0.3 \ No newline at end of file From 60a2d43c3c356dd1b152d598745aec308e201600 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 26 Dec 2022 22:01:54 -0600 Subject: [PATCH 07/36] add swerve drive to RobotContainer per #11 --- src/main/cpp/RobotContainer.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index cd76d90..7a31187 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -29,6 +29,21 @@ RobotContainer::RobotContainer() { drivetrain_ = std::make_shared(drivetrain_sw_.get()); VOKC_CALL(drivetrain_->Init()); + + // == swerve drive == + std::shared_ptr swerve_drive_hw; + VOKC_CALL(SetupSwerveDriveInterface(hardware_, &swerve_drive_hw)); + + // Initialize the software interface + swerve_drive_sw_ = std::make_shared(); + + // Link SwerveDriveIO to hardware / software + swerve_drive_io_ = std::make_shared(swerve_drive_hw.get(), swerve_drive_sw_.get()); + + // Link intake software to the I/O + swerve_drive_ = std::make_shared(swerve_drive_sw_.get()); + + // == intake == std::shared_ptr intake_hw; VOKC_CALL(SetupIntakeInterface(hardware_, &intake_hw)); From 3b0fe4ff64a3263e6d2556b25bfd1f0c1bf2e598 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 26 Dec 2022 22:02:05 -0600 Subject: [PATCH 08/36] add swerve drive to RobotContainer header per #11 --- src/main/include/RobotContainer.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index dcba6f6..2035608 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -16,6 +16,8 @@ #include "hardware/HardwareInterface.h" #include "io/DrivetrainIO.h" #include "subsystems/Drivetrain.h" +#include "io/SwerveDriveIO.h" +#include "subsystems/SwerveDrive.h" #include "io/IntakeIO.h" #include "subsystems/Intake.h" @@ -44,14 +46,17 @@ class RobotContainer { // Hardware I/O interfaces std::shared_ptr drivetrain_io_; + std::shared_ptr swerve_drive_io_; std::shared_ptr intake_io_; // Robot software interfaces. std::shared_ptr drivetrain_sw_; + std::shared_ptr swerve_drive_sw_; std::shared_ptr intake_sw_; // Subsystems std::shared_ptr drivetrain_; + std::shared_ptr swerve_drive_; std::shared_ptr intake_; // Commands From e8fe72edf3f0949f5b865e9a4c9d223016e5be55 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 19:22:33 -0600 Subject: [PATCH 09/36] created SwerveModule classes, need to be fleshed out --- src/main/cpp/SwerveModule.cpp | 2 + src/main/cpp/subsystems/SwerveDrive.cpp | 219 ++++++++++++++++++++++ src/main/include/SwerveModule.h | 14 ++ src/main/include/subsystems/SwerveDrive.h | 86 +++++++++ 4 files changed, 321 insertions(+) create mode 100644 src/main/cpp/SwerveModule.cpp create mode 100644 src/main/cpp/subsystems/SwerveDrive.cpp create mode 100644 src/main/include/SwerveModule.h create mode 100644 src/main/include/subsystems/SwerveDrive.h diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp new file mode 100644 index 0000000..83ff37f --- /dev/null +++ b/src/main/cpp/SwerveModule.cpp @@ -0,0 +1,2 @@ +#include "SwerveModule.h" + diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000..feff4ad --- /dev/null +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,219 @@ + +#include "subsystems/SwerveDrive.h" + +bool SwerveDrive::Init() { + // Initialize Shuffleboard from parameters. + OKC_CALL(InitShuffleboard()); + + double x_disp = RobotParams::GetParam("swerve.x_disp", 0.3); // in meters + double y_disp = RobotParams::GetParam("swerve.y_disp", 0.3); // in meters + + // !! IMPORTANT ASSUMPTION/PRACTICE/WHATEVER !! + // the order of swerve stuff should always be in: + // left front, left back, right front, right back + // for the sake of consistency and also if you want the swerve drive to actually work + // because that should be how the argumetns are passed in, and however they're passed in, + // that's how they gonna get passed out + + //TODO define displacements (the Translation2d stuff) + + //TODO define SwerveKinematics object + + //TODO define SwerveOdometry object + + //TODO define SwerveModuleStates objects + + + + // Reset everything + OKC_CALL(ResetDriveEncoders()); + OKC_CALL(ResetSteerEncoders()); + OKC_CALL(ResetGyro()); + return true; +} + +void SwerveDrive::Periodic() { + // Update shuffleboard + VOKC_CALL(UpdateShuffleboard()); + + // update odometry + swerve_odometry.update(-this->GetHeading()); // negate heading because odometry expects counterclockwise to be positive, but the NavX is not +} + +void SwerveDrive::SimulationPeriodic() { + // SimulationPeriodic +} + +bool SwerveDrive::SetSpeedModifierDrive(const double &speed_mod) { + speed_modifier_drive = speed_mod; + + return true; +} + +bool SwerveDrive::SetSpeedModifierSteer(const double &speed_mod) { + speed_modifier_steer = speed_mod; + + return true; +} + +bool SwerveDrive::SetOpenLoopRampDrive(const double &open_loop_ramp) { + OKC_CHECK(interface_ != nullptr); + + interface_->drive_config.open_loop_ramp_rate_drive = open_loop_ramp; + return true; +} + +bool SwerveDrive::SetOpenLoopRampSteer(const double &open_loop_ramp) { + OKC_CHECK(interface_ != nullptr); + + interface_->drive_config.open_loop_ramp_rate_steer = open_loop_ramp; + return true; +} + +bool SwerveDrive::SetMaxOutputDrive(const double &max_output) { + OKC_CHECK(interface_ != nullptr); + + interface_->drive_config.max_output_drive = max_output; + return true; +} + +bool SwerveDrive::SetMaxOutputSteer(const double &max_output) { + OKC_CHECK(interface_ != nullptr); + + interface_->drive_config.max_output_steer = max_output; + return true; +} + +bool SwerveDrive::TeleOpDrive(const double &drive, const double &strafe, const double &turn) { + //TODO + return true; +} + + +bool SwerveDrive::TranslateAuto(const double &x, const double &y, const double &rot) { + //TODO + return true; +} + +bool SwerveDrive::TurnToHeading(const double &heading) { + //TODO + return true; +} + +bool SwerveDrive::GetHeading(double *heading) { + OKC_CHECK(heading != nullptr); + OKC_CHECK(interface_ != nullptr); + + heading = &interface_->imu_yaw; + + return true; +} + +bool SwerveDrive::ResetDriveEncoders() { + OKC_CHECK(interface_ != nullptr); + + interface_->reset_drive_encoders = true; + return true; +} + +bool SwerveDrive::ResetSteerEncoders() { + OKC_CHECK(interface_ != nullptr); + + interface_->reset_steer_encoders = true; + return true; +} + + +bool SwerveDrive::ResetGyro() { + OKC_CHECK(interface_ != nullptr); + + interface_->reset_gyro = true; + return true; +} + + +bool SwerveDrive::InitShuffleboard() { + // Get parameters + // Update dashboard. + return true; +} + +bool SwerveDrive::UpdateShuffleboard() { + // If competition mode isn't set to true, then allow the PID gains to be + // tuned. + bool is_competition = RobotParams::GetParam("competition", false); + if (!is_competition) { + // Update encoder UI + double encoder_tmp = 0.0; + OKC_CALL(GetLeftEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_left_ticks.SetDouble(encoder_tmp); + OKC_CALL(GetRightEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_right_ticks.SetDouble(encoder_tmp); + OKC_CALL(GetEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_total_ticks.SetDouble(encoder_tmp); + + // Heading UI + double heading_tmp = 0.0; + OKC_CALL(GetHeading(&heading_tmp)); + SwerveDriveUI::nt_heading.SetDouble(heading_tmp); + + // Distance UI + double dist_err = dist_pid_.GetPositionError(); + SwerveDriveUI::nt_dist_error.SetDouble(dist_err); + + // Update the PID Gains if write mode is true. + if (SwerveDriveUI::nt_write_mode.GetBoolean(false)) { + // Get the current PID parameter values + double dist_p = + RobotParams::GetParam("drivetrain.distance_pid.Kp", 0); + double dist_i = + RobotParams::GetParam("drivetrain.distance_pid.Ki", 0); + double dist_d = + RobotParams::GetParam("drivetrain.distance_pid.Kp", 0); + + double heading_p = + RobotParams::GetParam("drivetrain.heading_pid.Kp", 0); + double heading_i = + RobotParams::GetParam("drivetrain.heading_pid.Ki", 0); + double heading_d = + RobotParams::GetParam("drivetrain.heading_pid.Kp", 0); + + double turn_p = RobotParams::GetParam("drivetrain.turn_pid.Kp", 0); + double turn_i = RobotParams::GetParam("drivetrain.turn_pid.Ki", 0); + double turn_d = RobotParams::GetParam("drivetrain.turn_pid.Kp", 0); + + // Get the values from shuffleboard. + dist_p = SwerveDriveUI::nt_dist_kp.GetDouble(dist_p); + dist_i = SwerveDriveUI::nt_dist_ki.GetDouble(dist_i); + dist_d = SwerveDriveUI::nt_dist_kd.GetDouble(dist_d); + + heading_p = SwerveDriveUI::nt_heading_kp.GetDouble(heading_p); + heading_i = SwerveDriveUI::nt_heading_ki.GetDouble(heading_i); + heading_d = SwerveDriveUI::nt_heading_kd.GetDouble(heading_d); + + turn_p = SwerveDriveUI::nt_turn_kp.GetDouble(turn_p); + turn_i = SwerveDriveUI::nt_turn_ki.GetDouble(turn_i); + turn_d = SwerveDriveUI::nt_turn_kd.GetDouble(turn_d); + + // Distance PID + dist_pid_.SetPID(dist_p, dist_i, dist_d); + heading_pid_.SetPID(heading_p, heading_i, heading_d); + turn_pid_.SetPID(turn_p, turn_i, turn_d); + } + + // Allow saving parameters in non-competition modes + if (SwerveDriveUI::nt_save.GetBoolean(true)) { + // Save the parameters. + OKC_CALL(RobotParams::SaveParameters(RobotParams::param_file)); + SwerveDriveUI::nt_save.SetBoolean(false); + } + } + + // Resetting the Gyro needs to always be available. + if (SwerveDriveUI::nt_reset_gyro.GetBoolean(false)) { + interface_->reset_gyro = true; + SwerveDriveUI::nt_reset_gyro.SetBoolean(false); + } + + return true; +} \ No newline at end of file diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h new file mode 100644 index 0000000..5f57a71 --- /dev/null +++ b/src/main/include/SwerveModule.h @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +class SwerveModule { +public: + bool SetState(frc::SwerveModuleState &state); + bool GetState(frc::SwerveModuleState *state); + bool GetPosition(frc::SwerveModulePosition *pos); +private: + frc::SwerveModuleState state; + frc::SwerveModulePosition pos; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000..66a97fe --- /dev/null +++ b/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,86 @@ +// Copyright (c) Team OKC Robotics + +#pragma once + +#define _USE_MATH_DEFINES +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "Parameters.h" +#include "UserInterface.h" +#include "Utils.h" +#include "io/SwerveDriveIO.h" + +class SwerveDrive : public frc2::SubsystemBase { +public: + // TODO: put the actual constants in for the PID gains. + SwerveDrive(SwerveDriveSoftwareInterface *interface) + : interface_(interface), swerve_kinematics(frc::Translation2d(/*TODO parameter loading, etc*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), + swerve_odometry(&swerve_kinematics, 0, /*TODO*/), + left_front_module, left_back_module, right_front_module, right_back_module) {} + ~SwerveDrive() {} + + bool Init(); + + /** + * 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 SetSpeedModifierDrive(const double &speed_mod); + bool SetSpeedModifierSteer(const double &speed_mod); + bool SetOpenLoopRampDrive(const double &open_loop_ramp); + bool SetOpenLoopRampSteer(const double &open_loop_ramp); + bool SetMaxOutputDrive(const double &max_output); + bool SetMaxOutputSteer(const double &max_output); + + bool TeleOpDrive(const double &drive, const double &strafe, const double &turn); + + bool TranslateAuto(const double &x, const double &y, const double &rot); + bool TurnToHeading(const double &heading); + bool GetHeading(double *heading); + + bool ResetDriveEncoders(); + bool ResetSteerEncoders(); + bool ResetGyro(); + +private: + // Shuffleboard functions + bool InitShuffleboard(); + bool UpdateShuffleboard(); + + // software interface + SwerveDriveSoftwareInterface *const interface_; + + // kinematics + frc::SwerveDriveKinematics<4> swerve_kinematics; + + // odometry + frc::SwerveDriveOdometry<4> swerve_odometry; + + // swerve modules + frc::SwerveModuleState left_front_module; + frc::SwerveModuleState left_back_module; + frc::SwerveModuleState right_front_module; + frc::SwerveModuleState right_back_module; + + // Speed modifier (the joystick input is multiplied by this value) + double speed_modifier_drive = 0.75; + double speed_modifier_steer = 0.75; + + // Open loop ramp rate + double open_loop_ramp_ = 0.5; +}; From 696c850c45d94192149ef394fd8461bbbd9c429b Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 20:28:03 -0600 Subject: [PATCH 10/36] finised adding WPILib swerve support to SwerveDrive subsystem of course, it doesn't do much right now, but the framework is there for writing all the set_output etc. adds SwerveModulePosition, SwerveModuleState, SwerveKinematics, and SwerveOdometry also removes all the drivetrian shuffleboard code (because this file was copied from the Drivetrain subsystem) ticket: #11 --- src/main/cpp/subsystems/SwerveDrive.cpp | 77 ++++++++--------------- src/main/include/subsystems/SwerveDrive.h | 29 ++++++--- 2 files changed, 47 insertions(+), 59 deletions(-) diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index feff4ad..bbc4626 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -1,5 +1,7 @@ #include "subsystems/SwerveDrive.h" +#include "units/length.h" +#include "frc/geometry/Rotation2d.h" bool SwerveDrive::Init() { // Initialize Shuffleboard from parameters. @@ -36,8 +38,20 @@ void SwerveDrive::Periodic() { // Update shuffleboard VOKC_CALL(UpdateShuffleboard()); + // get the heading + double heading = 0; + VOKC_CALL(this->GetHeading(&heading)); + + // update modules + //TODO figure out the conversion between steer motor encoder reading and actual reading in degees + modules[0] = frc::SwerveModulePosition(units::meter_t(this->interface_->left_front_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->left_front_steer_motor_enc))); + modules[1] = frc::SwerveModulePosition(units::meter_t(this->interface_->left_back_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->left_back_steer_motor_enc))); + modules[2] = frc::SwerveModulePosition(units::meter_t(this->interface_->right_front_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->right_front_steer_motor_enc))); + modules[3] = frc::SwerveModulePosition(units::meter_t(this->interface_->right_back_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->right_back_steer_motor_enc))); + + // update odometry - swerve_odometry.update(-this->GetHeading()); // negate heading because odometry expects counterclockwise to be positive, but the NavX is not + swerve_odometry.Update(frc::Rotation2d(units::degree_t(heading)), modules); // negate heading because odometry expects counterclockwise to be positive, but the NavX is not } void SwerveDrive::SimulationPeriodic() { @@ -134,7 +148,11 @@ bool SwerveDrive::ResetGyro() { bool SwerveDrive::InitShuffleboard() { // Get parameters + //TODO + // Update dashboard. + //TODO + return true; } @@ -143,63 +161,19 @@ bool SwerveDrive::UpdateShuffleboard() { // tuned. bool is_competition = RobotParams::GetParam("competition", false); if (!is_competition) { - // Update encoder UI - double encoder_tmp = 0.0; - OKC_CALL(GetLeftEncoderAverage(&encoder_tmp)); - SwerveDriveUI::nt_left_ticks.SetDouble(encoder_tmp); - OKC_CALL(GetRightEncoderAverage(&encoder_tmp)); - SwerveDriveUI::nt_right_ticks.SetDouble(encoder_tmp); - OKC_CALL(GetEncoderAverage(&encoder_tmp)); - SwerveDriveUI::nt_total_ticks.SetDouble(encoder_tmp); - + /*// Update encoder UI + //TODO + // Heading UI double heading_tmp = 0.0; OKC_CALL(GetHeading(&heading_tmp)); SwerveDriveUI::nt_heading.SetDouble(heading_tmp); // Distance UI - double dist_err = dist_pid_.GetPositionError(); - SwerveDriveUI::nt_dist_error.SetDouble(dist_err); - + //TODO + // Update the PID Gains if write mode is true. - if (SwerveDriveUI::nt_write_mode.GetBoolean(false)) { - // Get the current PID parameter values - double dist_p = - RobotParams::GetParam("drivetrain.distance_pid.Kp", 0); - double dist_i = - RobotParams::GetParam("drivetrain.distance_pid.Ki", 0); - double dist_d = - RobotParams::GetParam("drivetrain.distance_pid.Kp", 0); - - double heading_p = - RobotParams::GetParam("drivetrain.heading_pid.Kp", 0); - double heading_i = - RobotParams::GetParam("drivetrain.heading_pid.Ki", 0); - double heading_d = - RobotParams::GetParam("drivetrain.heading_pid.Kp", 0); - - double turn_p = RobotParams::GetParam("drivetrain.turn_pid.Kp", 0); - double turn_i = RobotParams::GetParam("drivetrain.turn_pid.Ki", 0); - double turn_d = RobotParams::GetParam("drivetrain.turn_pid.Kp", 0); - - // Get the values from shuffleboard. - dist_p = SwerveDriveUI::nt_dist_kp.GetDouble(dist_p); - dist_i = SwerveDriveUI::nt_dist_ki.GetDouble(dist_i); - dist_d = SwerveDriveUI::nt_dist_kd.GetDouble(dist_d); - - heading_p = SwerveDriveUI::nt_heading_kp.GetDouble(heading_p); - heading_i = SwerveDriveUI::nt_heading_ki.GetDouble(heading_i); - heading_d = SwerveDriveUI::nt_heading_kd.GetDouble(heading_d); - - turn_p = SwerveDriveUI::nt_turn_kp.GetDouble(turn_p); - turn_i = SwerveDriveUI::nt_turn_ki.GetDouble(turn_i); - turn_d = SwerveDriveUI::nt_turn_kd.GetDouble(turn_d); - - // Distance PID - dist_pid_.SetPID(dist_p, dist_i, dist_d); - heading_pid_.SetPID(heading_p, heading_i, heading_d); - turn_pid_.SetPID(turn_p, turn_i, turn_d); - } + //TODO // Allow saving parameters in non-competition modes if (SwerveDriveUI::nt_save.GetBoolean(true)) { @@ -213,6 +187,7 @@ bool SwerveDrive::UpdateShuffleboard() { if (SwerveDriveUI::nt_reset_gyro.GetBoolean(false)) { interface_->reset_gyro = true; SwerveDriveUI::nt_reset_gyro.SetBoolean(false); + }*/ } return true; diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h index 66a97fe..8e00732 100644 --- a/src/main/include/subsystems/SwerveDrive.h +++ b/src/main/include/subsystems/SwerveDrive.h @@ -11,6 +11,8 @@ #include #include #include +#include +#include "frc/geometry/Rotation2d.h" #include #include "Parameters.h" @@ -22,9 +24,9 @@ class SwerveDrive : public frc2::SubsystemBase { public: // TODO: put the actual constants in for the PID gains. SwerveDrive(SwerveDriveSoftwareInterface *interface) - : interface_(interface), swerve_kinematics(frc::Translation2d(/*TODO parameter loading, etc*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), - swerve_odometry(&swerve_kinematics, 0, /*TODO*/), - left_front_module, left_back_module, right_front_module, right_back_module) {} + : interface_(interface), swerve_kinematics(frc::Translation2d(/*TODO parameter loading, etc*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/)), + left_front_module(), left_back_module(), right_front_module(), right_back_module(), + swerve_odometry(swerve_kinematics, frc::Rotation2d(), modules) {} ~SwerveDrive() {} bool Init(); @@ -65,17 +67,28 @@ class SwerveDrive : public frc2::SubsystemBase { // software interface SwerveDriveSoftwareInterface *const interface_; + + // swerve modules states + frc::SwerveModuleState left_front_module; + frc::SwerveModuleState left_back_module; + frc::SwerveModuleState right_front_module; + frc::SwerveModuleState right_back_module; + + // swerve module positions + frc::SwerveModulePosition left_front_pos; + frc::SwerveModulePosition left_back_pos; + frc::SwerveModulePosition right_front_pos; + frc::SwerveModulePosition right_back_pos; + + // swerve module list + wpi::array modules = {left_front_pos, left_back_pos, right_front_pos, right_back_pos}; + // kinematics frc::SwerveDriveKinematics<4> swerve_kinematics; // odometry frc::SwerveDriveOdometry<4> swerve_odometry; - // swerve modules - frc::SwerveModuleState left_front_module; - frc::SwerveModuleState left_back_module; - frc::SwerveModuleState right_front_module; - frc::SwerveModuleState right_back_module; // Speed modifier (the joystick input is multiplied by this value) double speed_modifier_drive = 0.75; From a5846ddcfb5cf5c366ccf833e8e8feee23d838d4 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 20:28:18 -0600 Subject: [PATCH 11/36] fixes swerve drive initialization in RobotContainer --- src/main/include/RobotContainer.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 2035608..134785b 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -46,17 +46,17 @@ class RobotContainer { // Hardware I/O interfaces std::shared_ptr drivetrain_io_; - std::shared_ptr swerve_drive_io_; + std::shared_ptr swerve_drive_io_; std::shared_ptr intake_io_; // Robot software interfaces. std::shared_ptr drivetrain_sw_; - std::shared_ptr swerve_drive_sw_; + std::shared_ptr swerve_drive_sw_; std::shared_ptr intake_sw_; // Subsystems std::shared_ptr drivetrain_; - std::shared_ptr swerve_drive_; + std::shared_ptr swerve_drive_; std::shared_ptr intake_; // Commands From b1e56d18631c72ae9d3806e57ea8247a7777a2ac Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 20:51:19 -0600 Subject: [PATCH 12/36] fixes swerve drive subsystem constructor --- src/main/include/subsystems/SwerveDrive.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h index 8e00732..bbb160c 100644 --- a/src/main/include/subsystems/SwerveDrive.h +++ b/src/main/include/subsystems/SwerveDrive.h @@ -13,6 +13,7 @@ #include #include #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Pose2d.h" #include #include "Parameters.h" @@ -26,7 +27,7 @@ class SwerveDrive : public frc2::SubsystemBase { SwerveDrive(SwerveDriveSoftwareInterface *interface) : interface_(interface), swerve_kinematics(frc::Translation2d(/*TODO parameter loading, etc*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/)), left_front_module(), left_back_module(), right_front_module(), right_back_module(), - swerve_odometry(swerve_kinematics, frc::Rotation2d(), modules) {} + swerve_odometry(swerve_kinematics, frc::Rotation2d(), frc::Pose2d()) {} ~SwerveDrive() {} bool Init(); From 688361aa3e22515f379de3f0a55b087df39401cc Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 20:51:47 -0600 Subject: [PATCH 13/36] instantiates swerve drive kinematics and swerve drive odometry ticket: #11 --- src/main/cpp/subsystems/SwerveDrive.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index bbc4626..f36ed8a 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -1,7 +1,6 @@ #include "subsystems/SwerveDrive.h" #include "units/length.h" -#include "frc/geometry/Rotation2d.h" bool SwerveDrive::Init() { // Initialize Shuffleboard from parameters. @@ -17,13 +16,14 @@ bool SwerveDrive::Init() { // because that should be how the argumetns are passed in, and however they're passed in, // that's how they gonna get passed out - //TODO define displacements (the Translation2d stuff) + // define SwerveKinematics object + swerve_kinematics = frc::SwerveDriveKinematics<4>(frc::Translation2d(x_disp, y_disp), frc::Translation2d(-x_disp, y_disp), frc::Translation2d(x_disp, -y_disp), frc::Translation2d(-x_disp, -y_disp)); - //TODO define SwerveKinematics object + // define SwerveOdometry object + swerve_odometry = frc::SwerveDriveOdometry(swerve_kinematics, frc::Rotation2d(), frc::Pose2d()); - //TODO define SwerveOdometry object - - //TODO define SwerveModuleStates objects + // define SwerveModuleStates objects + //TODO From 32f96289acd59822c38830580e104ed6d9c4cd0e Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Fri, 30 Dec 2022 20:57:53 -0600 Subject: [PATCH 14/36] remove ServeModule class as it was never written and it turns out we don't actually need it ticket: #11 --- src/main/cpp/SwerveModule.cpp | 2 -- src/main/include/SwerveModule.h | 14 -------------- 2 files changed, 16 deletions(-) delete mode 100644 src/main/cpp/SwerveModule.cpp delete mode 100644 src/main/include/SwerveModule.h diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp deleted file mode 100644 index 83ff37f..0000000 --- a/src/main/cpp/SwerveModule.cpp +++ /dev/null @@ -1,2 +0,0 @@ -#include "SwerveModule.h" - diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h deleted file mode 100644 index 5f57a71..0000000 --- a/src/main/include/SwerveModule.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include -#include - -class SwerveModule { -public: - bool SetState(frc::SwerveModuleState &state); - bool GetState(frc::SwerveModuleState *state); - bool GetPosition(frc::SwerveModulePosition *pos); -private: - frc::SwerveModuleState state; - frc::SwerveModulePosition pos; -}; \ No newline at end of file From f43a4561afc5166e5b923e92bf8203f8001995d6 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 2 Jan 2023 16:24:07 -0600 Subject: [PATCH 15/36] added swerve drive PID constants --- src/main/deploy/parameters.toml | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index a840b04..6d127eb 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -83,4 +83,14 @@ Kd = 0.001 # assumes our modules are located in a square, everything should be the same distance from the center # units are in meters x_disp = 0.3 -y_disp = 0.3 \ No newline at end of file +y_disp = 0.3 + +[swerve.drive_pid] +kP = 0.0 +kI = 0.001 +kD = 0.0001 + +[swerve.steer_pid] +kP = 0.0 +kI = 0.001 +kD = 0.0001 \ No newline at end of file From ded7317698986f67720aeed5530523fcb1b3b4e2 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 2 Jan 2023 16:25:30 -0600 Subject: [PATCH 16/36] fixed SwerveDrive.h default constructors, added PID controllers --- src/main/include/subsystems/SwerveDrive.h | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h index bbb160c..25a4fb9 100644 --- a/src/main/include/subsystems/SwerveDrive.h +++ b/src/main/include/subsystems/SwerveDrive.h @@ -14,10 +14,13 @@ #include #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Pose2d.h" +#include "frc/geometry/Translation2d.h" #include +#include "units/length.h" +#include "units/velocity.h" #include "Parameters.h" -#include "UserInterface.h" +#include "ui/UserInterface.h" #include "Utils.h" #include "io/SwerveDriveIO.h" @@ -25,9 +28,11 @@ class SwerveDrive : public frc2::SubsystemBase { public: // TODO: put the actual constants in for the PID gains. SwerveDrive(SwerveDriveSoftwareInterface *interface) - : interface_(interface), swerve_kinematics(frc::Translation2d(/*TODO parameter loading, etc*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/), frc::Translation2d(/*TODO*/)), + : interface_(interface), swerve_kinematics(frc::Translation2d(units::meter_t(0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(0.3), units::meter_t(-0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(-0.3))), left_front_module(), left_back_module(), right_front_module(), right_back_module(), - swerve_odometry(swerve_kinematics, frc::Rotation2d(), frc::Pose2d()) {} + swerve_odometry(swerve_kinematics, frc::Rotation2d(), modules, frc::Pose2d()), + left_front_drive_pid(0, 0.001, 0.0001), left_back_drive_pid(0, 0.001, 0.0001), right_front_drive_pid(0, 0.001, 0.0001), right_back_drive_pid(0, 0.001, 0.0001), + left_front_steer_pid(0, 0.001, 0.0001), left_back_steer_pid(0, 0.001, 0.0001), right_front_steer_pid(0, 0.001, 0.0001), right_back_steer_pid(0, 0.001, 0.0001) {} ~SwerveDrive() {} bool Init(); @@ -82,6 +87,7 @@ class SwerveDrive : public frc2::SubsystemBase { frc::SwerveModulePosition right_back_pos; // swerve module list + wpi::array outputs = {left_front_module, left_back_module, right_front_module, right_back_module}; wpi::array modules = {left_front_pos, left_back_pos, right_front_pos, right_back_pos}; // kinematics @@ -90,6 +96,17 @@ class SwerveDrive : public frc2::SubsystemBase { // odometry frc::SwerveDriveOdometry<4> swerve_odometry; + // PID Controllers + frc::PIDController left_front_drive_pid; + frc::PIDController left_back_drive_pid; + frc::PIDController right_front_drive_pid; + frc::PIDController right_back_drive_pid; + + frc::PIDController left_front_steer_pid; + frc::PIDController left_back_steer_pid; + frc::PIDController right_front_steer_pid; + frc::PIDController right_back_steer_pid; + // Speed modifier (the joystick input is multiplied by this value) double speed_modifier_drive = 0.75; From 076b7625af7f2f658e60324dc065e7a91a9ed5a5 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 2 Jan 2023 16:26:10 -0600 Subject: [PATCH 17/36] created actual swerve I/O motor outputs --- src/main/include/io/SwerveDriveIO.h | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/include/io/SwerveDriveIO.h b/src/main/include/io/SwerveDriveIO.h index bebeccc..109518b 100644 --- a/src/main/include/io/SwerveDriveIO.h +++ b/src/main/include/io/SwerveDriveIO.h @@ -64,15 +64,18 @@ typedef struct swerve_drive_software_interface_t { // 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; + // motor outputs + double left_front_drive_motor_output; + double left_back_drive_motor_output; + + double right_front_drive_motor_output; + double right_back_drive_motor_output; + + double left_front_steer_motor_output; + double left_back_steer_motor_output; + + double right_front_steer_motor_output; + double right_back_steer_motor_output; // Reset flags bool reset_drive_encoders; From a0aad188fd820ffd4785d3204bef0ff93774cd9d Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 2 Jan 2023 16:31:39 -0600 Subject: [PATCH 18/36] added drive and steer PID controllers, fleshed out TeleOpDrive method ticket: #11 --- src/main/cpp/subsystems/SwerveDrive.cpp | 68 ++++++++++++++++++++++--- 1 file changed, 61 insertions(+), 7 deletions(-) diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index f36ed8a..0460708 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -1,6 +1,6 @@ #include "subsystems/SwerveDrive.h" -#include "units/length.h" + bool SwerveDrive::Init() { // Initialize Shuffleboard from parameters. @@ -9,6 +9,29 @@ bool SwerveDrive::Init() { double x_disp = RobotParams::GetParam("swerve.x_disp", 0.3); // in meters double y_disp = RobotParams::GetParam("swerve.y_disp", 0.3); // in meters + // 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); + + left_front_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); + left_back_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); + right_front_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); + right_back_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); + + left_front_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); + left_back_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); + right_front_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); + right_back_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); + + + // !! IMPORTANT ASSUMPTION/PRACTICE/WHATEVER !! // the order of swerve stuff should always be in: // left front, left back, right front, right back @@ -17,15 +40,24 @@ bool SwerveDrive::Init() { // that's how they gonna get passed out // define SwerveKinematics object - swerve_kinematics = frc::SwerveDriveKinematics<4>(frc::Translation2d(x_disp, y_disp), frc::Translation2d(-x_disp, y_disp), frc::Translation2d(x_disp, -y_disp), frc::Translation2d(-x_disp, -y_disp)); + swerve_kinematics = frc::SwerveDriveKinematics<4>(frc::Translation2d(units::meter_t(x_disp), units::meter_t(y_disp)), frc::Translation2d(units::meter_t(-x_disp), units::meter_t(y_disp)), frc::Translation2d(units::meter_t(x_disp), units::meter_t(-y_disp)), frc::Translation2d(units::meter_t(-x_disp), units::meter_t(-y_disp))); // define SwerveOdometry object - swerve_odometry = frc::SwerveDriveOdometry(swerve_kinematics, frc::Rotation2d(), frc::Pose2d()); + swerve_odometry = frc::SwerveDriveOdometry(swerve_kinematics, frc::Rotation2d(), modules, frc::Pose2d()); // define SwerveModuleStates objects - //TODO - - + // everything starts at a default of 0 m/s and 0 degrees + left_front_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); + left_back_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); + right_front_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); + right_back_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); + + // define SwerveModulePosition objects + // everything starts at a default of 0m and 0 degrees + left_front_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); + left_back_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); + right_front_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); + right_back_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); // Reset everything OKC_CALL(ResetDriveEncoders()); @@ -99,7 +131,29 @@ bool SwerveDrive::SetMaxOutputSteer(const double &max_output) { } bool SwerveDrive::TeleOpDrive(const double &drive, const double &strafe, const double &turn) { - //TODO + // get outputs from the kinematics object based on joystick inputs + outputs = swerve_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds(units::meters_per_second_t(drive), units::meters_per_second_t(strafe), units::radians_per_second_t(turn))); + + for(int i = 0; i < outputs.size(); i++) { + // optimize the angle of the wheel so we never turn more than 90 degrees or something like that + // basically we avoid unnecessary turning because positive negative angle stuff + outputs.at(i) = frc::SwerveModuleState::Optimize(modules.at(i).angle.Radians(), outputs.at(i).angle.Radians()); + } + + // set all the outputs in the interface + interface_->left_front_drive_motor_output = outputs.at(0).speed; + interface_->left_back_drive_motor_output = outputs.at(0).angle; + + interface_->right_front_drive_motor_output = outputs.at(1).speed; + interface_->right_back_drive_motor_output = outputs.at(1).angle; + + interface_->left_front_steer_motor_output = outputs.at(2).speed; + interface_->left_back_steer_motor_output = outputs.at(2).angle; + + interface_->right_front_steer_motor_output = outputs.at(3).speed; + interface_->right_back_steer_motor_output = outputs.at(3).angle; + + // return true return true; } From 9db4fa142f3ebcbe92679424506f21f780e96d3d Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 11:25:09 -0600 Subject: [PATCH 19/36] created swerve module class --- src/main/cpp/SwerveModule.cpp | 146 ++++++++++++++++++++++++++++++++ src/main/include/SwerveModule.h | 66 +++++++++++++++ 2 files changed, 212 insertions(+) create mode 100644 src/main/cpp/SwerveModule.cpp create mode 100644 src/main/include/SwerveModule.h diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp new file mode 100644 index 0000000..fddd606 --- /dev/null +++ b/src/main/cpp/SwerveModule.cpp @@ -0,0 +1,146 @@ + +#include "SwerveModule.h" + +bool SwerveModule::Init() { + // 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()); + + + // 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::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 + 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; +} \ No newline at end of file diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h new file mode 100644 index 0000000..80576c7 --- /dev/null +++ b/src/main/include/SwerveModule.h @@ -0,0 +1,66 @@ + +#pragma once + +#define _USE_MATH_DEFINES +#include +#include + +#include +#include +#include +#include +#include +#include +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Translation2d.h" +#include +#include "units/length.h" +#include "units/velocity.h" +#include "units/math.h" + +#include "Parameters.h" +#include "ui/UserInterface.h" +#include "Utils.h" +#include "io/SwerveDriveIO.h" + +enum Location { + LEFT_FRONT, + LEFT_BACK, + RIGHT_FRONT, + RIGHT_BACK +}; + +class SwerveModule { +public: + SwerveModule(); + ~SwerveModule(); + + bool Init(); + + bool GetSwerveModulePosition(frc::SwerveModulePosition *pos); + bool GetSwerveModuleState(frc::SwerveModuleState *state); + + bool GetLocationOnRobot(frc::Translation2d *loc); + + bool SetDesiredState(frc::SwerveModuleState state); + bool GetDriveOutput(double *output); // PID + bool GetSteerOutput(double *output); // PID, optimize angle + + bool Update(double drive_enc, double steer_enc, double drive_vel, double steer_vel); + bool Reset(); + +private: + frc::PIDController drive_pid; + frc::PIDController steer_pid; + + frc::SwerveModuleState state; + frc::SwerveModulePosition pos; + + frc::Translation2d trans; + + Location loc; + + double drive_enc_vel; + double steer_enc_vel; +}; \ No newline at end of file From 06b330de05b19436a97c8f6de49179063f1e7606 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 12:44:10 -0600 Subject: [PATCH 20/36] fix swerve module not actually having a way to set its physical location --- src/main/cpp/SwerveModule.cpp | 4 +++- src/main/include/SwerveModule.h | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp index fddd606..35933db 100644 --- a/src/main/cpp/SwerveModule.cpp +++ b/src/main/cpp/SwerveModule.cpp @@ -1,7 +1,7 @@ #include "SwerveModule.h" -bool SwerveModule::Init() { +bool SwerveModule::Init(Location loc) { // PID Controller stuff // drive PID gains double drive_kP = RobotParams::GetParam("swerve.drive_pid.kP", 0.0); @@ -24,6 +24,7 @@ bool SwerveModule::Init() { // 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 @@ -134,6 +135,7 @@ bool SwerveModule::Update(double drive_enc, double steer_enc, double drive_vel, 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))); diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h index 80576c7..110ed85 100644 --- a/src/main/include/SwerveModule.h +++ b/src/main/include/SwerveModule.h @@ -36,7 +36,7 @@ class SwerveModule { SwerveModule(); ~SwerveModule(); - bool Init(); + bool Init(Location loc); bool GetSwerveModulePosition(frc::SwerveModulePosition *pos); bool GetSwerveModuleState(frc::SwerveModuleState *state); @@ -59,7 +59,7 @@ class SwerveModule { frc::Translation2d trans; - Location loc; + Location location; double drive_enc_vel; double steer_enc_vel; From 5b66c7b779501181099391edf2b6c7f6e7dfdbc0 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 12:45:51 -0600 Subject: [PATCH 21/36] added velocity readings to swerve software interface added actually getting encoder readings TODO: add absolute encoder support, everything right now is all the internal NEO encoders ticket: #11 --- src/main/cpp/io/SwerveDriveIO.cpp | 31 +++++++++++++++++++++++++++-- src/main/include/io/SwerveDriveIO.h | 12 +++++++++-- 2 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/io/SwerveDriveIO.cpp b/src/main/cpp/io/SwerveDriveIO.cpp index 380c43d..f5cb881 100644 --- a/src/main/cpp/io/SwerveDriveIO.cpp +++ b/src/main/cpp/io/SwerveDriveIO.cpp @@ -46,7 +46,10 @@ bool SwerveDriveIO::ProcessIO() { sw_interface_->reset_gyro = false; } - // Set the drive outputs based on the drive mode. + // Set the drive outputs. + //TODO + + // set the steer outputs. //TODO // Get the hardware sensor values. @@ -54,7 +57,31 @@ bool SwerveDriveIO::ProcessIO() { sw_interface_->imu_yaw = hw_interface_->ahrs->GetAngle(); // Encoders - //TODO + // position + // drive + sw_interface_->left_front_drive_motor_enc = hw_interface_->left_front_drive_motor->GetEncoder().GetPosition(); + sw_interface_->left_back_drive_motor_enc = hw_interface_->left_back_drive_motor->GetEncoder().GetPosition(); + sw_interface_->right_front_drive_motor_enc = hw_interface_->right_front_drive_motor->GetEncoder().GetPosition(); + sw_interface_->right_back_drive_motor_enc = hw_interface_->right_back_drive_motor->GetEncoder().GetPosition(); + + // steer + sw_interface_->left_front_steer_motor_enc = hw_interface_->left_front_steer_motor->GetEncoder().GetPosition(); + sw_interface_->left_back_steer_motor_enc = hw_interface_->left_back_steer_motor->GetEncoder().GetPosition(); + sw_interface_->right_front_steer_motor_enc = hw_interface_->right_front_steer_motor->GetEncoder().GetPosition(); + sw_interface_->right_back_steer_motor_enc = hw_interface_->right_back_steer_motor->GetEncoder().GetPosition(); + + // velocity + // drive + sw_interface_->left_front_drive_enc_vel = hw_interface_->left_front_drive_motor->GetEncoder().GetVelocity(); + sw_interface_->left_back_drive_enc_vel = hw_interface_->left_back_drive_motor->GetEncoder().GetVelocity(); + sw_interface_->right_front_drive_enc_vel = hw_interface_->right_front_drive_motor->GetEncoder().GetVelocity(); + sw_interface_->right_back_drive_enc_vel = hw_interface_->right_back_drive_motor->GetEncoder().GetVelocity(); + + // steer + sw_interface_->left_front_steer_enc_vel = hw_interface_->left_front_steer_motor->GetEncoder().GetVelocity(); + sw_interface_->left_back_steer_enc_vel = hw_interface_->left_back_steer_motor->GetEncoder().GetVelocity(); + sw_interface_->right_front_steer_enc_vel = hw_interface_->right_front_steer_motor->GetEncoder().GetVelocity(); + sw_interface_->right_back_steer_enc_vel = hw_interface_->right_back_steer_motor->GetEncoder().GetVelocity(); return true; } diff --git a/src/main/include/io/SwerveDriveIO.h b/src/main/include/io/SwerveDriveIO.h index 109518b..90b48c4 100644 --- a/src/main/include/io/SwerveDriveIO.h +++ b/src/main/include/io/SwerveDriveIO.h @@ -45,16 +45,24 @@ typedef struct swerve_drive_software_interface_t { // 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; + // encoder velocities + double left_front_drive_enc_vel; + double left_back_drive_enc_vel; + double right_front_drive_enc_vel; + double right_back_drive_enc_vel; + + double left_front_steer_enc_vel; + double left_back_steer_enc_vel; + double right_front_steer_enc_vel; + double right_back_steer_enc_vel; // SW OUTPUTS // Configure swerve drive variables From 20f607e677233c77bbe33f0b5810f790be58515f Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 13:13:39 -0600 Subject: [PATCH 22/36] fix destructor --- src/main/include/subsystems/SwerveDrive.h | 57 +++++++++-------------- 1 file changed, 21 insertions(+), 36 deletions(-) diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h index 25a4fb9..afd1b91 100644 --- a/src/main/include/subsystems/SwerveDrive.h +++ b/src/main/include/subsystems/SwerveDrive.h @@ -9,30 +9,23 @@ #include #include #include -#include -#include #include #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Pose2d.h" -#include "frc/geometry/Translation2d.h" #include -#include "units/length.h" -#include "units/velocity.h" #include "Parameters.h" #include "ui/UserInterface.h" #include "Utils.h" #include "io/SwerveDriveIO.h" +#include "SwerveModule.h" class SwerveDrive : public frc2::SubsystemBase { public: - // TODO: put the actual constants in for the PID gains. SwerveDrive(SwerveDriveSoftwareInterface *interface) - : interface_(interface), swerve_kinematics(frc::Translation2d(units::meter_t(0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(0.3), units::meter_t(-0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(-0.3))), - left_front_module(), left_back_module(), right_front_module(), right_back_module(), - swerve_odometry(swerve_kinematics, frc::Rotation2d(), modules, frc::Pose2d()), - left_front_drive_pid(0, 0.001, 0.0001), left_back_drive_pid(0, 0.001, 0.0001), right_front_drive_pid(0, 0.001, 0.0001), right_back_drive_pid(0, 0.001, 0.0001), - left_front_steer_pid(0, 0.001, 0.0001), left_back_steer_pid(0, 0.001, 0.0001), right_front_steer_pid(0, 0.001, 0.0001), right_back_steer_pid(0, 0.001, 0.0001) {} + : interface_(interface), left_front_module(), left_back_module(), right_front_module(), right_back_module(), + swerve_kinematics(frc::Translation2d(units::meter_t(0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(0.3), units::meter_t(-0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(-0.3))), + swerve_odometry(swerve_kinematics, frc::Rotation2d(), positions, frc::Pose2d()) {} ~SwerveDrive() {} bool Init(); @@ -73,22 +66,26 @@ class SwerveDrive : public frc2::SubsystemBase { // software interface SwerveDriveSoftwareInterface *const interface_; - - // swerve modules states - frc::SwerveModuleState left_front_module; - frc::SwerveModuleState left_back_module; - frc::SwerveModuleState right_front_module; - frc::SwerveModuleState right_back_module; + // swerve modules + SwerveModule left_front_module; + SwerveModule left_back_module; + SwerveModule right_front_module; + SwerveModule right_back_module; // swerve module positions - frc::SwerveModulePosition left_front_pos; - frc::SwerveModulePosition left_back_pos; - frc::SwerveModulePosition right_front_pos; - frc::SwerveModulePosition right_back_pos; + frc::Translation2d *left_front_loc; + frc::Translation2d *left_back_loc; + frc::Translation2d *right_front_loc; + frc::Translation2d *right_back_loc; + + // swerve module states + frc::SwerveModulePosition *left_front_pos; + frc::SwerveModulePosition *left_back_pos; + frc::SwerveModulePosition *right_front_pos; + frc::SwerveModulePosition *right_back_pos; - // swerve module list - wpi::array outputs = {left_front_module, left_back_module, right_front_module, right_back_module}; - wpi::array modules = {left_front_pos, left_back_pos, right_front_pos, right_back_pos}; + // swerve module positions + wpi::array positions = {*left_front_pos, *left_back_pos, *right_front_pos, *right_back_pos}; // kinematics frc::SwerveDriveKinematics<4> swerve_kinematics; @@ -96,18 +93,6 @@ class SwerveDrive : public frc2::SubsystemBase { // odometry frc::SwerveDriveOdometry<4> swerve_odometry; - // PID Controllers - frc::PIDController left_front_drive_pid; - frc::PIDController left_back_drive_pid; - frc::PIDController right_front_drive_pid; - frc::PIDController right_back_drive_pid; - - frc::PIDController left_front_steer_pid; - frc::PIDController left_back_steer_pid; - frc::PIDController right_front_steer_pid; - frc::PIDController right_back_steer_pid; - - // Speed modifier (the joystick input is multiplied by this value) double speed_modifier_drive = 0.75; double speed_modifier_steer = 0.75; From 219fa286fa759354563b6e5c14d022704efea132 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 13:30:00 -0600 Subject: [PATCH 23/36] set outputs in sw_interface --- src/main/cpp/io/SwerveDriveIO.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/io/SwerveDriveIO.cpp b/src/main/cpp/io/SwerveDriveIO.cpp index f5cb881..e647974 100644 --- a/src/main/cpp/io/SwerveDriveIO.cpp +++ b/src/main/cpp/io/SwerveDriveIO.cpp @@ -1,5 +1,6 @@ #include "io/SwerveDriveIO.h" +#include "Utils.h" void SwerveDriveIO::Periodic() { // Process all the inputs and outputs to/from high level software. @@ -47,10 +48,16 @@ bool SwerveDriveIO::ProcessIO() { } // Set the drive outputs. - //TODO + hw_interface_->left_front_drive_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_drive, this->sw_interface_->drive_config.max_output_drive, &this->sw_interface_->left_front_drive_motor_output)); + hw_interface_->left_back_drive_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_drive, this->sw_interface_->drive_config.max_output_drive, &this->sw_interface_->left_back_drive_motor_output)); + hw_interface_->right_front_drive_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_drive, this->sw_interface_->drive_config.max_output_drive, &this->sw_interface_->right_front_drive_motor_output)); + hw_interface_->right_back_drive_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_drive, this->sw_interface_->drive_config.max_output_drive, &this->sw_interface_->right_back_drive_motor_output)); // set the steer outputs. - //TODO + hw_interface_->left_front_steer_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_steer, this->sw_interface_->drive_config.max_output_steer, &this->sw_interface_->left_front_steer_motor_output)); + hw_interface_->left_back_steer_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_steer, this->sw_interface_->drive_config.max_output_steer, &this->sw_interface_->left_back_steer_motor_output)); + hw_interface_->right_front_steer_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_steer, this->sw_interface_->drive_config.max_output_steer, &this->sw_interface_->right_front_steer_motor_output)); + hw_interface_->right_back_steer_motor->Set(TeamOKC::Clamp(-this->sw_interface_->drive_config.max_output_steer, this->sw_interface_->drive_config.max_output_steer, &this->sw_interface_->right_back_steer_motor_output)); // Get the hardware sensor values. // navX IMU: @@ -107,10 +114,6 @@ bool SwerveDriveIO::UpdateDriveConfig(SwerveDriveConfig &config) { 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; } From 1ee7e1e55f4077879ceb6dbee01ec4e1968f7e9c Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 18:40:41 -0600 Subject: [PATCH 24/36] added absolute encoders to hardware interface --- src/main/cpp/hardware/HardwareInterface.cpp | 5 +++++ src/main/include/hardware/SensorInterface.h | 1 + 2 files changed, 6 insertions(+) diff --git a/src/main/cpp/hardware/HardwareInterface.cpp b/src/main/cpp/hardware/HardwareInterface.cpp index a171c63..91d962d 100644 --- a/src/main/cpp/hardware/HardwareInterface.cpp +++ b/src/main/cpp/hardware/HardwareInterface.cpp @@ -78,6 +78,11 @@ bool SetupSwerveDriveInterface( 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 diff --git a/src/main/include/hardware/SensorInterface.h b/src/main/include/hardware/SensorInterface.h index 7367388..f4c9803 100644 --- a/src/main/include/hardware/SensorInterface.h +++ b/src/main/include/hardware/SensorInterface.h @@ -22,6 +22,7 @@ typedef struct sensors_t { std::unique_ptr deploy_limit_switch; std::unique_ptr retracted_limit_switch; + // swerve drive steer encoders std::unique_ptr left_front_steer_encoder; std::unique_ptr left_back_steer_encoder; std::unique_ptr right_front_steer_encoder; From a120dd95059ff88fe92eadd897fcb559214b023b Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 19:18:31 -0600 Subject: [PATCH 25/36] 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 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 swerve, std::shared_ptr 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 +#include + +#include "Utils.h" +#include "subsystems/SwerveDrive.h" + + +/** + * + */ +class AutoSwerveCommand + : public frc2::CommandHelper { +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 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 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 +#include +#include + +#include "Utils.h" +#include "subsystems/SwerveDrive.h" + + +/** + * + */ +class TeleOpSwerveCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new TeleOpSwerveCommand. + * + * @param swerve The swerve drive subsystem used by this command. + */ + explicit TeleOpSwerveCommand(std::shared_ptr swerve, std::shared_ptr gamepad); + + void Initialize() override; + void Execute() override; + void End(bool executed) override; + bool IsFinished() override; + +private: + std::shared_ptr swerve_; + std::shared_ptr gamepad_; +}; From 2a95107fc9687a2c1b189a199cede8890701c5d6 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 19:19:08 -0600 Subject: [PATCH 26/36] add steering absolute encoders to swerve drive i/o. ticket: #11 --- src/main/include/io/SwerveDriveIO.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/include/io/SwerveDriveIO.h b/src/main/include/io/SwerveDriveIO.h index 90b48c4..6950047 100644 --- a/src/main/include/io/SwerveDriveIO.h +++ b/src/main/include/io/SwerveDriveIO.h @@ -8,7 +8,7 @@ #include "AHRS.h" #include "Utils.h" - +#include "frc/AnalogInput.h" typedef struct swerve_drive_config_t { double max_output_drive; @@ -35,6 +35,12 @@ typedef struct swerve_drive_hardware_interface_t { // AHRS AHRS *const ahrs; + // Steer encoders + frc::AnalogInput *const left_front_steer_encoder; + frc::AnalogInput *const left_back_steer_encoder; + frc::AnalogInput *const right_front_steer_encoder; + frc::AnalogInput *const right_back_steer_encoder; + } SwerveDriveHardwareInterface; typedef struct swerve_drive_software_interface_t { From b6f1e0b3f220da7d42d05365be3783a84274b6e3 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 19:19:49 -0600 Subject: [PATCH 27/36] fix swerve module constructor --- src/main/include/SwerveModule.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h index 110ed85..fbb607f 100644 --- a/src/main/include/SwerveModule.h +++ b/src/main/include/SwerveModule.h @@ -33,8 +33,9 @@ enum Location { class SwerveModule { public: - SwerveModule(); - ~SwerveModule(); + SwerveModule() + : drive_pid(0.0, 0.001, 0.0001), steer_pid(0.0, 0.001, 0.0001), state(), pos(), trans(), location() {}; + ~SwerveModule() {} bool Init(Location loc); From 2088ee6600308d7aeb540fe6fc0d2e5709b41899 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 3 Jan 2023 19:28:57 -0600 Subject: [PATCH 28/36] added swerve drive shuffleboard values to the UI class ticket: #11 --- src/main/cpp/ui/UserInterface.cpp | 35 ++++++++++++++++++++++++++++- src/main/include/ui/UserInterface.h | 31 +++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index c9d0bd9..5f406bc 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/src/main/cpp/ui/UserInterface.cpp @@ -73,4 +73,37 @@ namespace ShooterUI { nt::GenericEntry &nt_low_goal = nt_tab.Add("Low Goal Preset", 0).GetEntry(); nt::GenericEntry &nt_far_shot = nt_tab.Add("Far Shot Preset", 0).GetEntry(); -} // namespace ShooterUI \ No newline at end of file +} // namespace ShooterUI + +namespace SwerveDriveUI { + // Get the tab + frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("SwerveDrive"); + + // Add all the defaults + // Write mode + nt::GenericEntry &nt_write_mode = + nt_tab.Add("Write Mode", false).GetEntry(); + + // Encoder + nt::GenericEntry &nt_left_ticks = nt_tab.Add("left avg dist", 0).GetEntry(); + nt::GenericEntry &nt_right_ticks = nt_tab.Add("right avg dist", 0).GetEntry(); + nt::GenericEntry &nt_total_ticks = nt_tab.Add("avg dist", 0).GetEntry(); + + // Distance PID + nt::GenericEntry &nt_dist_kp = nt_tab.Add("Distance kP", 0).GetEntry(); + nt::GenericEntry &nt_dist_ki = nt_tab.Add("Distance kI", 0).GetEntry(); + nt::GenericEntry &nt_dist_kd = nt_tab.Add("Distance kD", 0).GetEntry(); + + // Steer PID + nt::GenericEntry &nt_steer_kp = nt_tab.Add("Steer kP", 0).GetEntry(); + nt::GenericEntry &nt_steer_ki = nt_tab.Add("Steer kI", 0).GetEntry(); + nt::GenericEntry &nt_steer_kd = nt_tab.Add("Steer kD", 0).GetEntry(); + + // Gyro + nt::GenericEntry &nt_heading = nt_tab.Add("Heading", 0).GetEntry(); + nt::GenericEntry &nt_reset_gyro = + nt_tab.Add("Reset Gyro", false).GetEntry(); + + // Save parameters button + nt::GenericEntry &nt_save = nt_tab.Add("Save", false).GetEntry(); +} // namespace SwerveDriveUI \ No newline at end of file diff --git a/src/main/include/ui/UserInterface.h b/src/main/include/ui/UserInterface.h index 64ddd33..5c9fcd7 100644 --- a/src/main/include/ui/UserInterface.h +++ b/src/main/include/ui/UserInterface.h @@ -72,3 +72,34 @@ namespace ShooterUI { extern nt::GenericEntry &nt_low_goal; extern nt::GenericEntry &nt_far_shot; } // namespace ShooterUI + +namespace SwerveDriveUI { + // Get the tab + extern frc::ShuffleboardTab &nt_tab; + + // Add all the defaults + // Write mode + extern nt::GenericEntry &nt_write_mode; + + // Encoder + extern nt::GenericEntry &nt_left_avg; + extern nt::GenericEntry &nt_right_avg; + extern nt::GenericEntry &nt_avg_dist; + + // Distance PID + extern nt::GenericEntry &nt_dist_kp; + extern nt::GenericEntry &nt_dist_ki; + extern nt::GenericEntry &nt_dist_kd; + + // Steer PID + extern nt::GenericEntry &nt_steer_kp; + extern nt::GenericEntry &nt_steer_ki; + extern nt::GenericEntry &nt_steer_kd; + + // Gyro + extern nt::GenericEntry &nt_heading; + extern nt::GenericEntry &nt_reset_gyro; + + // Save drivetrain parameters + extern nt::GenericEntry &nt_save; +} // namespace SwerveDriveUI \ No newline at end of file From f98b32b4c68bb4b3f32264346195cea5bd71609e Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 18:42:43 -0600 Subject: [PATCH 29/36] make AutoSwerveCommand use Pose2d instead of a Translation2d and a Rotation2d --- src/main/cpp/commands/swerve/AutoSwerveCommand.cpp | 9 +++++---- src/main/include/commands/swerve/AutoSwerveCommand.h | 5 ++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp b/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp index 2479614..c6b5616 100644 --- a/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp +++ b/src/main/cpp/commands/swerve/AutoSwerveCommand.cpp @@ -1,10 +1,9 @@ #include "commands/swerve/AutoSwerveCommand.h" -AutoSwerveCommand::AutoSwerveCommand(std::shared_ptr swerve, frc::Translation2d f_pos, frc::Rotation2d f_angle) { +AutoSwerveCommand::AutoSwerveCommand(std::shared_ptr swerve, frc::Pose2d f_pos) { swerve_ = swerve; end_pos = f_pos; - final_angle = f_angle; if (swerve_ != nullptr) { this->AddRequirements(swerve_.get()); @@ -22,7 +21,7 @@ void AutoSwerveCommand::Initialize() { void AutoSwerveCommand::Execute() { VOKC_CHECK(swerve_ != nullptr); - VOKC_CALL(swerve_->TranslateAuto(end_pos, final_angle)); + VOKC_CALL(swerve_->TranslateAuto(end_pos)); } void AutoSwerveCommand::End(bool executed) { @@ -35,8 +34,10 @@ bool AutoSwerveCommand::IsFinished() { 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()) { + if (swerve_->AtSetpoint(at)) { return true; // end the command } diff --git a/src/main/include/commands/swerve/AutoSwerveCommand.h b/src/main/include/commands/swerve/AutoSwerveCommand.h index 8610304..69e80a9 100644 --- a/src/main/include/commands/swerve/AutoSwerveCommand.h +++ b/src/main/include/commands/swerve/AutoSwerveCommand.h @@ -19,7 +19,7 @@ class AutoSwerveCommand * * @param swerve The swerve drive used by this command. */ - explicit AutoSwerveCommand(std::shared_ptr swerve, frc::Translation2d end_pos, frc::Rotation2d final_angle); + explicit AutoSwerveCommand(std::shared_ptr swerve, frc::Pose2d end_pos); void Initialize() override; void Execute() override; @@ -28,6 +28,5 @@ class AutoSwerveCommand private: std::shared_ptr swerve_; - frc::Translation2d end_pos; - frc::Rotation2d final_angle; + frc::Pose2d end_pos; }; From 5982eecf4f7ce96c79ef9f652a9bd24808f218bd Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 18:43:02 -0600 Subject: [PATCH 30/36] add SetPID methods to SwerveModule --- src/main/cpp/SwerveModule.cpp | 19 +++++++++++++++++++ src/main/include/SwerveModule.h | 10 +++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp index 35933db..01e65c9 100644 --- a/src/main/cpp/SwerveModule.cpp +++ b/src/main/cpp/SwerveModule.cpp @@ -127,6 +127,25 @@ bool SwerveModule::GetSteerOutput(double *output) { 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 diff --git a/src/main/include/SwerveModule.h b/src/main/include/SwerveModule.h index fbb607f..6fe10d0 100644 --- a/src/main/include/SwerveModule.h +++ b/src/main/include/SwerveModule.h @@ -18,6 +18,8 @@ #include "units/length.h" #include "units/velocity.h" #include "units/math.h" +#include "units/voltage.h" +#include "frc/controller/SimpleMotorFeedforward.h" #include "Parameters.h" #include "ui/UserInterface.h" @@ -34,7 +36,7 @@ enum Location { class SwerveModule { public: SwerveModule() - : drive_pid(0.0, 0.001, 0.0001), steer_pid(0.0, 0.001, 0.0001), state(), pos(), trans(), location() {}; + : drive_pid(0.0, 0.001, 0.0001), steer_pid(0.0, 0.001, 0.0001), state(), pos(), trans(), location()/*, drive_ff(units::volt_t(0.0), units::volt_t(0.0) * units::second_t(1.0) / units::meter_t(1.0), units::volts_seconds_squared_per_meter(0.0)), steer_ff(0, 0, 0)*/ {}; ~SwerveModule() {} bool Init(Location loc); @@ -48,6 +50,9 @@ class SwerveModule { bool GetDriveOutput(double *output); // PID bool GetSteerOutput(double *output); // PID, optimize angle + bool SetDrivePID(double kP, double kI, double kD); + bool SetSteerPID(double kP, double kI, double kD); + bool Update(double drive_enc, double steer_enc, double drive_vel, double steer_vel); bool Reset(); @@ -55,6 +60,9 @@ class SwerveModule { frc::PIDController drive_pid; frc::PIDController steer_pid; + frc::SimpleMotorFeedforward drive_ff; + frc::SimpleMotorFeedforward steer_ff; + frc::SwerveModuleState state; frc::SwerveModulePosition pos; From 7e782da4c38865bc0f5d0c2f4b6fa114e5db8efd Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 18:43:38 -0600 Subject: [PATCH 31/36] add a bunch of auto methods and variables and an enum to SwerveDrive subsystem --- src/main/include/subsystems/SwerveDrive.h | 43 ++++++++++++++++++++--- 1 file changed, 38 insertions(+), 5 deletions(-) diff --git a/src/main/include/subsystems/SwerveDrive.h b/src/main/include/subsystems/SwerveDrive.h index afd1b91..045ee6d 100644 --- a/src/main/include/subsystems/SwerveDrive.h +++ b/src/main/include/subsystems/SwerveDrive.h @@ -20,12 +20,19 @@ #include "io/SwerveDriveIO.h" #include "SwerveModule.h" +enum AutoStage { + TURN_TO_GOAL, + DRIVE_TO_GOAL, + TURN_TO_FINAL_HEADING, + FINISHED +}; + class SwerveDrive : public frc2::SubsystemBase { public: SwerveDrive(SwerveDriveSoftwareInterface *interface) : interface_(interface), left_front_module(), left_back_module(), right_front_module(), right_back_module(), swerve_kinematics(frc::Translation2d(units::meter_t(0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(0.3)), frc::Translation2d(units::meter_t(0.3), units::meter_t(-0.3)), frc::Translation2d(units::meter_t(-0.3), units::meter_t(-0.3))), - swerve_odometry(swerve_kinematics, frc::Rotation2d(), positions, frc::Pose2d()) {} + swerve_odometry(swerve_kinematics, frc::Rotation2d(), positions, frc::Pose2d()), position() {} ~SwerveDrive() {} bool Init(); @@ -49,13 +56,29 @@ class SwerveDrive : public frc2::SubsystemBase { bool SetMaxOutputSteer(const double &max_output); bool TeleOpDrive(const double &drive, const double &strafe, const double &turn); + bool DumbTeleOpDrive(const double &drive, const double &strafe, const double &turn); - bool TranslateAuto(const double &x, const double &y, const double &rot); - bool TurnToHeading(const double &heading); + bool TranslateAuto(frc::Pose2d pos); + bool TurnToHeading(frc::Pose2d pos); + bool InitAuto(frc::Pose2d pos); + bool TurnToGoal(frc::Pose2d pos); + bool SetDrive(const double &power); + bool DriveToGoal(frc::Pose2d pos); + bool TurnToGoalHeading(frc::Pose2d pos); + bool TranslateAutoLockHeading(frc::Pose2d pos); + + bool GetLeftDriveEncoderAverage(double *avg); + bool GetRightDriveEncoderAverage(double *avg); + bool GetDriveEncoderAverage(double *avg); + bool GetLeftSteerEncoderAverage(double *avg); + bool GetRightSteerEncoderAverage(double *avg); bool GetHeading(double *heading); + + bool AtSetpoint(bool *at); bool ResetDriveEncoders(); bool ResetSteerEncoders(); + bool ResetPIDs(); bool ResetGyro(); private: @@ -93,10 +116,20 @@ class SwerveDrive : public frc2::SubsystemBase { // odometry frc::SwerveDriveOdometry<4> swerve_odometry; + // position + frc::Pose2d position; + // Speed modifier (the joystick input is multiplied by this value) double speed_modifier_drive = 0.75; double speed_modifier_steer = 0.75; - // Open loop ramp rate - double open_loop_ramp_ = 0.5; + // max output + double max_output_drive = 1; + double max_output_steer = 1; + + // if the robot has reached the autonomous setpoint + bool at_setpoint = false; + + double heading_to_goal; + double distance_to_goal; }; From 71159af8f94715ea71abfcdad0a2972c7584dd79 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 18:44:07 -0600 Subject: [PATCH 32/36] remove tele-op input cubing, add strafe_power and fix it --- src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp index 56f8e1a..76facd9 100644 --- a/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp +++ b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp @@ -2,7 +2,6 @@ #include "commands/swerve/TeleOpSwerveCommand.h" TeleOpSwerveCommand::TeleOpSwerveCommand(std::shared_ptr swerve, std::shared_ptr gamepad) { - // Set everything. swerve_ = swerve; gamepad_ = gamepad; @@ -19,12 +18,11 @@ 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); + double drive_power = -this->gamepad_->GetRawAxis(1); + double strafe_power = this->gamepad_->GetRawAxis(2); + double turn_power = this->gamepad_->GetRawAxis(3); - // TODO actually get correct joystick inputs - - VOKC_CALL(swerve_->TeleOpDrive(throttle, turn_power)); + VOKC_CALL(swerve_->TeleOpDrive(drive_power, strafe_power, turn_power)); } bool TeleOpSwerveCommand::IsFinished() { From e2b7042ce9f346b996dca0aea09d439cf387adac Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 18:46:08 -0600 Subject: [PATCH 33/36] add shuffleboard, SwerveModule class usage, and a bunch of auton suff to SwerveDrive subsystem also fixed some WPILib swerve odom and kinem stuff yeah, mega commit, I know --- src/main/cpp/subsystems/SwerveDrive.cpp | 406 +++++++++++++++++++----- 1 file changed, 326 insertions(+), 80 deletions(-) diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index 0460708..8cda18f 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -1,37 +1,13 @@ #include "subsystems/SwerveDrive.h" +#include "SwerveModule.h" bool SwerveDrive::Init() { // Initialize Shuffleboard from parameters. OKC_CALL(InitShuffleboard()); - double x_disp = RobotParams::GetParam("swerve.x_disp", 0.3); // in meters - double y_disp = RobotParams::GetParam("swerve.y_disp", 0.3); // in meters - - // 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); - - left_front_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); - left_back_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); - right_front_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); - right_back_drive_pid = frc::PIDController(drive_kP, drive_kI, drive_kD); - - left_front_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); - left_back_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); - right_front_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); - right_back_steer_pid = frc::PIDController(steer_kP, steer_kI, steer_kD); - - - + // !! IMPORTANT ASSUMPTION/PRACTICE/WHATEVER !! // the order of swerve stuff should always be in: // left front, left back, right front, right back @@ -39,25 +15,38 @@ bool SwerveDrive::Init() { // because that should be how the argumetns are passed in, and however they're passed in, // that's how they gonna get passed out + // initialize swerve modules + left_front_module.Init(LEFT_FRONT); + left_back_module.Init(LEFT_BACK); + right_front_module.Init(RIGHT_FRONT); + right_back_module.Init(RIGHT_BACK); + + // create location objects + left_front_module.GetLocationOnRobot(left_front_loc); + left_back_module.GetLocationOnRobot(left_back_loc); + right_front_module.GetLocationOnRobot(right_front_loc); + right_back_module.GetLocationOnRobot(right_back_loc); + // define SwerveKinematics object - swerve_kinematics = frc::SwerveDriveKinematics<4>(frc::Translation2d(units::meter_t(x_disp), units::meter_t(y_disp)), frc::Translation2d(units::meter_t(-x_disp), units::meter_t(y_disp)), frc::Translation2d(units::meter_t(x_disp), units::meter_t(-y_disp)), frc::Translation2d(units::meter_t(-x_disp), units::meter_t(-y_disp))); + swerve_kinematics = frc::SwerveDriveKinematics<4>(*left_front_loc, *left_back_loc, *right_front_loc, *right_back_loc); - // define SwerveOdometry object - swerve_odometry = frc::SwerveDriveOdometry(swerve_kinematics, frc::Rotation2d(), modules, frc::Pose2d()); + // create position objects + left_front_module.GetSwerveModulePosition(left_front_pos); + left_back_module.GetSwerveModulePosition(left_back_pos); + right_front_module.GetSwerveModulePosition(right_front_pos); + right_back_module.GetSwerveModulePosition(right_back_pos); + + positions = wpi::array(*left_front_pos, *left_back_pos, *right_front_pos, *right_back_pos); + + // define SwerveOdometry object with default 0 starting parameters + swerve_odometry = frc::SwerveDriveOdometry(swerve_kinematics, frc::Rotation2d(), positions, frc::Pose2d()); - // define SwerveModuleStates objects - // everything starts at a default of 0 m/s and 0 degrees - left_front_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); - left_back_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); - right_front_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); - right_back_module = frc::SwerveModuleState(units::meters_per_second_t(0.0), frc::Rotation2d()); + // our internal position object + position = frc::Pose2d(); - // define SwerveModulePosition objects - // everything starts at a default of 0m and 0 degrees - left_front_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); - left_back_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); - right_front_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); - right_back_pos = frc::SwerveModulePosition(units::meter_t(0.0), frc::Rotation2d()); + // setpoint + at_setpoint = false; + // Reset everything OKC_CALL(ResetDriveEncoders()); @@ -75,15 +64,22 @@ void SwerveDrive::Periodic() { VOKC_CALL(this->GetHeading(&heading)); // update modules - //TODO figure out the conversion between steer motor encoder reading and actual reading in degees - modules[0] = frc::SwerveModulePosition(units::meter_t(this->interface_->left_front_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->left_front_steer_motor_enc))); - modules[1] = frc::SwerveModulePosition(units::meter_t(this->interface_->left_back_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->left_back_steer_motor_enc))); - modules[2] = frc::SwerveModulePosition(units::meter_t(this->interface_->right_front_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->right_front_steer_motor_enc))); - modules[3] = frc::SwerveModulePosition(units::meter_t(this->interface_->right_back_drive_motor_enc), frc::Rotation2d(units::degree_t(this->interface_->right_back_steer_motor_enc))); + left_front_module.Update(this->interface_->left_front_drive_motor_enc, this->interface_->left_front_steer_motor_enc, this->interface_->left_front_drive_enc_vel, this->interface_->left_front_steer_enc_vel); + left_back_module.Update(this->interface_->left_back_drive_motor_enc, this->interface_->left_back_steer_motor_enc, this->interface_->left_back_drive_enc_vel, this->interface_->left_back_steer_enc_vel); + right_front_module.Update(this->interface_->right_front_drive_motor_enc, this->interface_->right_front_steer_motor_enc, this->interface_->right_front_drive_enc_vel, this->interface_->right_front_steer_enc_vel); + right_back_module.Update(this->interface_->right_back_drive_motor_enc, this->interface_->right_back_steer_motor_enc, this->interface_->right_back_drive_enc_vel, this->interface_->right_back_steer_enc_vel); + // update module positions + left_front_module.GetSwerveModulePosition(left_front_pos); + left_back_module.GetSwerveModulePosition(left_back_pos); + right_front_module.GetSwerveModulePosition(right_front_pos); + right_back_module.GetSwerveModulePosition(right_back_pos); // update odometry - swerve_odometry.Update(frc::Rotation2d(units::degree_t(heading)), modules); // negate heading because odometry expects counterclockwise to be positive, but the NavX is not + swerve_odometry.Update(frc::Rotation2d(units::degree_t(-heading)), positions); // negate heading because odometry expects counterclockwise to be positive, but the NavX is not + + // update our position object + position = swerve_odometry.GetPose(); } void SwerveDrive::SimulationPeriodic() { @@ -132,39 +128,162 @@ bool SwerveDrive::SetMaxOutputSteer(const double &max_output) { bool SwerveDrive::TeleOpDrive(const double &drive, const double &strafe, const double &turn) { // get outputs from the kinematics object based on joystick inputs - outputs = swerve_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds(units::meters_per_second_t(drive), units::meters_per_second_t(strafe), units::radians_per_second_t(turn))); + auto outputs = swerve_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds(units::meters_per_second_t(drive), units::meters_per_second_t(strafe), units::radians_per_second_t(turn))); - for(int i = 0; i < outputs.size(); i++) { - // optimize the angle of the wheel so we never turn more than 90 degrees or something like that - // basically we avoid unnecessary turning because positive negative angle stuff - outputs.at(i) = frc::SwerveModuleState::Optimize(modules.at(i).angle.Radians(), outputs.at(i).angle.Radians()); - } + // set desired state in all the modules (set setpoints for PIDs) + left_front_module.SetDesiredState(outputs[0]); + left_back_module.SetDesiredState(outputs[1]); + right_front_module.SetDesiredState(outputs[2]); + right_back_module.SetDesiredState(outputs[3]); // set all the outputs in the interface - interface_->left_front_drive_motor_output = outputs.at(0).speed; - interface_->left_back_drive_motor_output = outputs.at(0).angle; + // drive outputs + left_front_module.GetDriveOutput(&this->interface_->left_front_drive_motor_output); + left_back_module.GetDriveOutput(&this->interface_->left_back_drive_motor_output); + + right_front_module.GetDriveOutput(&this->interface_->right_front_drive_motor_output); + right_back_module.GetDriveOutput(&this->interface_->right_back_drive_motor_output); + + // steer outputs + left_front_module.GetSteerOutput(&this->interface_->left_front_steer_motor_output); + left_back_module.GetSteerOutput(&this->interface_->left_back_steer_motor_output); + + right_front_module.GetSteerOutput(&this->interface_->right_front_steer_motor_output); + right_back_module.GetSteerOutput(&this->interface_->right_back_steer_motor_output); + + // if we've made it here, we haven't errored, so return true + return true; +} - interface_->right_front_drive_motor_output = outputs.at(1).speed; - interface_->right_back_drive_motor_output = outputs.at(1).angle; +/** + * In case the WPILib kinematics stuff doesn't work out + */ +bool SwerveDrive::DumbTeleOpDrive(const double &drive, const double &strafe, const double &turn) { + // given: drive power, strafe power, turn power + // 1. figure out what angle the drive and strafe thing is at + // 2. figure out how much turn power there is + // 3. figure out magnitude of drive/strafe + // 4. figure out magnitude of turn + // 5. add turn stuff together + // 6. set steer encoders to turn stuff + // 7. set drive power to drive magnitude + + // so that's: + // 1. atan(strafe / drive) + // 2. turn/45 // degrees. when turn is 1, the value should be 45 degrees. when turn is 0, the value should be 0 degrees. so that's 1:45, so just div by 45? + // 3. sqrt(drive**2 + strafe**2) + // 4. I think this is just straight [turn] + // 5. steer = turn+angle_from_step_1 + // 6. PID + // 7. set_output->(result_of_step_3) + + double drive_strafe_angle = atan(strafe / drive); // TODO figure out how this is gonna work with negative/positive/turning/stuff + double turn_angle = turn / 45; - interface_->left_front_steer_motor_output = outputs.at(2).speed; - interface_->left_back_steer_motor_output = outputs.at(2).angle; + double drive_strafe_magnitude = sqrt(pow(drive, 2) + pow(strafe, 2)); // TODO don't forget to copy the signs because just squaring gets rid of negatives + double turn_magnitude = turn; - interface_->right_front_steer_motor_output = outputs.at(3).speed; - interface_->right_back_steer_motor_output = outputs.at(3).angle; + double turn_power = turn_angle + turn; // ??? + //TODO - // return true return true; } +bool SwerveDrive::InitAuto(frc::Pose2d pos) { + // 1. figure out angle between here and there + this->heading_to_goal = atan((position.X().value() - pos.X().value()) / (position.Y().value() - pos.Y().value())); + + // 2. figure out distance + this->distance_to_goal = sqrt(pow(position.X().value() + pos.X(), 2) + pow(position.Y().value() + pos.Y(), 2)); +} + +bool SwerveDrive::DriveToGoal(frc::Pose2d pos) { + // 1. set steering to goal angle + // 2. PID the drive power until we're at the position + + // it's just distance formula between given pos and our odometry position + // double drive_power = this->dist_pid.Calculate(sqrt(pow(position.X().value() + pos.X(), 2) + pow(position.Y().value() + pos.Y(), 2))); + double drive_power = 0; + + left_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(drive_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + left_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(drive_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + right_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(drive_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + right_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(drive_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + + return true; +} + +bool SwerveDrive::TurnToHeading(frc::Pose2d pos) { + // 2. PID the drive power until we're at the heading + // this->heading_pid.SetSetpoint(pos.Rotation().Degrees()); + // double steer_power = this->heading_pid.Calculate(position.Rotation().Degrees()); + + double steer_power = 0; + + // 1. set steering to 45 (well, for half of them) degrees (cut the corners of the robot, basically) + // left front needs to be 135 degrees (because x axis goes from robot back through robot front positive) + left_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(135)))); + // left back needs to be 45 degrees + left_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(45)))); + // right front is 45 + right_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(45)))); + // right back is 135 + right_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(135)))); -bool SwerveDrive::TranslateAuto(const double &x, const double &y, const double &rot) { - //TODO return true; } -bool SwerveDrive::TurnToHeading(const double &heading) { +bool SwerveDrive::TranslateAutoLockHeading(frc::Pose2d pos) { + // 1. set steering to goal angle + // 2. PID the drive power until we're at the position + // that's literally it + + // this->heading_pid.SetSetpoint(this->heading_to_goal); + // double steer_power = this->heading_pid.Calculate(position.Rotation().Degrees()); + + double steer_power = 0; + + left_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + left_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + right_front_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + right_back_module.SetDesiredState(frc::SwerveModuleState(units::meters_per_second_t(steer_power), frc::Rotation2d(units::degree_t(heading_to_goal)))); + + + return true; +} + +bool SwerveDrive::TranslateAuto(frc::Pose2d pos) { + //TODO + // given: initial pos, final pos + // 1. figure out angle between here and there + // 2. figure out distance + // 3. turn to angle + // 4. drive the distance + // 5. figure out difference between current and final angle + // 6. turn to final angle + // 7. set AtSetpoint to true + /** + * 1. atan((position.x - pos.x) / (position.y - pos.y)) + * 2. sqrt((poxition.x - pos.x) ** 2 + (position.y - pos.y) ** 2) + * 3. PID + * 4. #2 + * 5. pos.rot - getHeading() + * 6. PID + * 7. at_setpoint = true; + */ + + double heading_to_goal = atan((position.X().value() - pos.X().value()) / (position.Y().value() - pos.Y().value())); + double distance_to_goal = sqrt(pow(position.X().value() + pos.X().value(), 2) + pow(position.Y().value() + pos.Y().value(), 2)); + //TODO + //TODO + + double heading_to_goal_heading = pos.Rotation().Degrees().value() - position.Rotation().Degrees().value(); + + //TODO + + at_setpoint = true; + return true; } @@ -177,6 +296,90 @@ bool SwerveDrive::GetHeading(double *heading) { return true; } +bool SwerveDrive::GetLeftDriveEncoderAverage(double *avg) { + OKC_CHECK(interface_ != nullptr); + + double tmp = 0; + + tmp += interface_->left_front_drive_motor_enc; + tmp += interface_->left_back_drive_motor_enc; + + *avg = (tmp / 2); + + return true; +} + +bool SwerveDrive::GetRightDriveEncoderAverage(double *avg) { + OKC_CHECK(interface_ != nullptr); + + double tmp = 0; + + tmp += interface_->right_front_drive_motor_enc; + tmp += interface_->right_back_drive_motor_enc; + + *avg = (tmp / 2); + + return true; +} + +bool SwerveDrive::GetDriveEncoderAverage(double *avg) { + OKC_CHECK(interface_ != nullptr); + + double left = 0; + double right = 0; + + this->GetLeftDriveEncoderAverage(&left); + this->GetRightDriveEncoderAverage(&right); + + *avg = (left + right) / 2; + + return true; +} + +bool SwerveDrive::GetLeftSteerEncoderAverage(double *avg) { + OKC_CHECK(interface_ != nullptr); + + double tmp = 0; + + tmp += interface_->right_front_steer_motor_enc; + tmp += interface_->right_back_steer_motor_enc; + + *avg = (tmp / 2); + + return true; +} + +bool SwerveDrive::GetRightSteerEncoderAverage(double *avg) { + OKC_CHECK(interface_ != nullptr); + + double tmp = 0; + + tmp += interface_->right_front_steer_motor_enc; + tmp += interface_->right_back_steer_motor_enc; + + *avg = (tmp / 2); + + return true; +} + +bool SwerveDrive::AtSetpoint(bool *at) { + OKC_CHECK(interface_ != nullptr); + + //TODO + + return true; +} + +bool SwerveDrive::ResetPIDs() { + left_front_module.Reset(); + left_back_module.Reset(); + right_front_module.Reset(); + right_back_module.Reset(); + + return true; +} + + bool SwerveDrive::ResetDriveEncoders() { OKC_CHECK(interface_ != nullptr); @@ -202,32 +405,76 @@ bool SwerveDrive::ResetGyro() { bool SwerveDrive::InitShuffleboard() { // Get parameters - //TODO + double dist_p = RobotParams::GetParam("swerve.distance_pid.kP", 0); + double dist_i = RobotParams::GetParam("swerve.distance_pid.kI", 0); + double dist_d = RobotParams::GetParam("swerve.distance_pid.kD", 0); + + double steer_p = RobotParams::GetParam("swerve.steer_pid.kP", 0); + double steer_i = RobotParams::GetParam("swerve.steer_pid.kI", 0); + double steer_d = RobotParams::GetParam("swerve.steer_pid.kD", 0); // Update dashboard. - //TODO + SwerveDriveUI::nt_dist_kp.SetDouble(dist_p); + SwerveDriveUI::nt_dist_ki.SetDouble(dist_i); + SwerveDriveUI::nt_dist_kd.SetDouble(dist_d); + + SwerveDriveUI::nt_steer_kp.SetDouble(steer_p); + SwerveDriveUI::nt_steer_ki.SetDouble(steer_i); + SwerveDriveUI::nt_steer_kd.SetDouble(steer_d); return true; } bool SwerveDrive::UpdateShuffleboard() { + // Update encoder UI + double encoder_tmp = 0.0; + VOKC_CALL(this->GetLeftDriveEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_left_avg.SetDouble(encoder_tmp); + VOKC_CALL(GetRightDriveEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_right_avg.SetDouble(encoder_tmp); + VOKC_CALL(GetDriveEncoderAverage(&encoder_tmp)); + SwerveDriveUI::nt_avg_dist.SetDouble(encoder_tmp); + + // Heading UI + double heading_tmp = 0.0; + VOKC_CALL(GetHeading(&heading_tmp)); + SwerveDriveUI::nt_heading.SetDouble(heading_tmp); + // If competition mode isn't set to true, then allow the PID gains to be // tuned. bool is_competition = RobotParams::GetParam("competition", false); if (!is_competition) { - /*// Update encoder UI - //TODO - - // Heading UI - double heading_tmp = 0.0; - OKC_CALL(GetHeading(&heading_tmp)); - SwerveDriveUI::nt_heading.SetDouble(heading_tmp); - - // Distance UI - //TODO - // Update the PID Gains if write mode is true. - //TODO + if (DrivetrainUI::nt_write_mode.GetBoolean(false)) { + // Get the current PID parameter values + double dist_p = RobotParams::GetParam("swerve.distance_pid.Kp", 0); + double dist_i = RobotParams::GetParam("swerve.distance_pid.Ki", 0); + double dist_d = RobotParams::GetParam("swerve.distance_pid.Kp", 0); + + double steer_p = RobotParams::GetParam("swerve.steer_pid.Kp", 0); + double steer_i = RobotParams::GetParam("swerve.steer_pid.Ki", 0); + double steer_d = RobotParams::GetParam("swerve.steer_pid.Kp", 0); + + // Get the values from shuffleboard. + dist_p = SwerveDriveUI::nt_dist_kp.GetDouble(dist_p); + dist_i = SwerveDriveUI::nt_dist_ki.GetDouble(dist_i); + dist_d = SwerveDriveUI::nt_dist_kd.GetDouble(dist_d); + + steer_p = SwerveDriveUI::nt_steer_kp.GetDouble(steer_p); + steer_i = SwerveDriveUI::nt_steer_ki.GetDouble(steer_i); + steer_d = SwerveDriveUI::nt_steer_kd.GetDouble(steer_d); + + // Update PIDs with values + VOKC_CALL(left_front_module.SetDrivePID(dist_p, dist_i, dist_d)); + VOKC_CALL(left_back_module.SetDrivePID(dist_p, dist_i, dist_d)); + VOKC_CALL(right_front_module.SetDrivePID(dist_p, dist_i, dist_d)); + VOKC_CALL(right_back_module.SetDrivePID(dist_p, dist_i, dist_d)); + + VOKC_CALL(left_front_module.SetSteerPID(steer_p, steer_i, steer_d)); + VOKC_CALL(left_back_module.SetSteerPID(steer_p, steer_i, steer_d)); + VOKC_CALL(right_front_module.SetSteerPID(steer_p, steer_i, steer_d)); + VOKC_CALL(right_back_module.SetSteerPID(steer_p, steer_i, steer_d)); + } // Allow saving parameters in non-competition modes if (SwerveDriveUI::nt_save.GetBoolean(true)) { @@ -241,7 +488,6 @@ bool SwerveDrive::UpdateShuffleboard() { if (SwerveDriveUI::nt_reset_gyro.GetBoolean(false)) { interface_->reset_gyro = true; SwerveDriveUI::nt_reset_gyro.SetBoolean(false); - }*/ } return true; From 8b613045f96e9b407b69372520dd71947fd684db Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 19:16:48 -0600 Subject: [PATCH 34/36] made code build --- .../commands/swerve/TeleOpSwerveCommand.cpp | 4 +++ src/main/cpp/subsystems/SwerveDrive.cpp | 30 ++++++++++--------- src/main/cpp/ui/UserInterface.cpp | 6 ++-- src/main/deploy/parameters.toml | 2 +- 4 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp index 76facd9..39ddc64 100644 --- a/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp +++ b/src/main/cpp/commands/swerve/TeleOpSwerveCommand.cpp @@ -25,6 +25,10 @@ void TeleOpSwerveCommand::Execute() { 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; diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index 8cda18f..bd591c3 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -194,7 +194,9 @@ bool SwerveDrive::InitAuto(frc::Pose2d pos) { this->heading_to_goal = atan((position.X().value() - pos.X().value()) / (position.Y().value() - pos.Y().value())); // 2. figure out distance - this->distance_to_goal = sqrt(pow(position.X().value() + pos.X(), 2) + pow(position.Y().value() + pos.Y(), 2)); + this->distance_to_goal = sqrt(pow(position.X().value() + pos.X().value(), 2) + pow(position.Y().value() + pos.Y().value(), 2)); + + return true; } bool SwerveDrive::DriveToGoal(frc::Pose2d pos) { @@ -428,16 +430,16 @@ bool SwerveDrive::InitShuffleboard() { bool SwerveDrive::UpdateShuffleboard() { // Update encoder UI double encoder_tmp = 0.0; - VOKC_CALL(this->GetLeftDriveEncoderAverage(&encoder_tmp)); + this->GetLeftDriveEncoderAverage(&encoder_tmp); SwerveDriveUI::nt_left_avg.SetDouble(encoder_tmp); - VOKC_CALL(GetRightDriveEncoderAverage(&encoder_tmp)); + GetRightDriveEncoderAverage(&encoder_tmp); SwerveDriveUI::nt_right_avg.SetDouble(encoder_tmp); - VOKC_CALL(GetDriveEncoderAverage(&encoder_tmp)); + GetDriveEncoderAverage(&encoder_tmp); SwerveDriveUI::nt_avg_dist.SetDouble(encoder_tmp); // Heading UI double heading_tmp = 0.0; - VOKC_CALL(GetHeading(&heading_tmp)); + GetHeading(&heading_tmp); SwerveDriveUI::nt_heading.SetDouble(heading_tmp); // If competition mode isn't set to true, then allow the PID gains to be @@ -465,15 +467,15 @@ bool SwerveDrive::UpdateShuffleboard() { steer_d = SwerveDriveUI::nt_steer_kd.GetDouble(steer_d); // Update PIDs with values - VOKC_CALL(left_front_module.SetDrivePID(dist_p, dist_i, dist_d)); - VOKC_CALL(left_back_module.SetDrivePID(dist_p, dist_i, dist_d)); - VOKC_CALL(right_front_module.SetDrivePID(dist_p, dist_i, dist_d)); - VOKC_CALL(right_back_module.SetDrivePID(dist_p, dist_i, dist_d)); - - VOKC_CALL(left_front_module.SetSteerPID(steer_p, steer_i, steer_d)); - VOKC_CALL(left_back_module.SetSteerPID(steer_p, steer_i, steer_d)); - VOKC_CALL(right_front_module.SetSteerPID(steer_p, steer_i, steer_d)); - VOKC_CALL(right_back_module.SetSteerPID(steer_p, steer_i, steer_d)); + left_front_module.SetDrivePID(dist_p, dist_i, dist_d); + left_back_module.SetDrivePID(dist_p, dist_i, dist_d); + right_front_module.SetDrivePID(dist_p, dist_i, dist_d); + right_back_module.SetDrivePID(dist_p, dist_i, dist_d); + + left_front_module.SetSteerPID(steer_p, steer_i, steer_d); + left_back_module.SetSteerPID(steer_p, steer_i, steer_d); + right_front_module.SetSteerPID(steer_p, steer_i, steer_d); + right_back_module.SetSteerPID(steer_p, steer_i, steer_d); } // Allow saving parameters in non-competition modes diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index 5f406bc..9051d9c 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/src/main/cpp/ui/UserInterface.cpp @@ -85,9 +85,9 @@ namespace SwerveDriveUI { nt_tab.Add("Write Mode", false).GetEntry(); // Encoder - nt::GenericEntry &nt_left_ticks = nt_tab.Add("left avg dist", 0).GetEntry(); - nt::GenericEntry &nt_right_ticks = nt_tab.Add("right avg dist", 0).GetEntry(); - nt::GenericEntry &nt_total_ticks = nt_tab.Add("avg dist", 0).GetEntry(); + nt::GenericEntry &nt_left_avg = nt_tab.Add("left avg dist", 0).GetEntry(); + nt::GenericEntry &nt_right_avg = nt_tab.Add("right avg dist", 0).GetEntry(); + nt::GenericEntry &nt_avg_dist = nt_tab.Add("avg dist", 0).GetEntry(); // Distance PID nt::GenericEntry &nt_dist_kp = nt_tab.Add("Distance kP", 0).GetEntry(); diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 6d127eb..b3badb8 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -91,6 +91,6 @@ kI = 0.001 kD = 0.0001 [swerve.steer_pid] -kP = 0.0 +kP = 0.01 kI = 0.001 kD = 0.0001 \ No newline at end of file From 02cab8d0edc4de750a6a060a5a73d800a75657bc Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 21:02:35 -0600 Subject: [PATCH 35/36] imported into beta-7 --- build.gradle | 2 +- src/main/deploy/parameters.toml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 63c5fa4..1ad254e 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ import org.gradle.internal.os.OperatingSystem plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index b3badb8..df1b86a 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -82,8 +82,8 @@ Kd = 0.001 [swerve] # assumes our modules are located in a square, everything should be the same distance from the center # units are in meters -x_disp = 0.3 -y_disp = 0.3 +x_disp = 0.216 +y_disp = 0.216 [swerve.drive_pid] kP = 0.0 From 0f84f34fb14a1a6745eb79240970fcd38cdc8b0f Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Thu, 5 Jan 2023 21:31:11 -0600 Subject: [PATCH 36/36] fix shuffleboard changing to pointers because WPILib --- src/main/cpp/subsystems/SwerveDrive.cpp | 42 ++++++++++++------------- src/main/cpp/ui/UserInterface.cpp | 26 +++++++-------- src/main/include/ui/UserInterface.h | 26 +++++++-------- 3 files changed, 47 insertions(+), 47 deletions(-) diff --git a/src/main/cpp/subsystems/SwerveDrive.cpp b/src/main/cpp/subsystems/SwerveDrive.cpp index bd591c3..e4e7c41 100644 --- a/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/src/main/cpp/subsystems/SwerveDrive.cpp @@ -416,13 +416,13 @@ bool SwerveDrive::InitShuffleboard() { double steer_d = RobotParams::GetParam("swerve.steer_pid.kD", 0); // Update dashboard. - SwerveDriveUI::nt_dist_kp.SetDouble(dist_p); - SwerveDriveUI::nt_dist_ki.SetDouble(dist_i); - SwerveDriveUI::nt_dist_kd.SetDouble(dist_d); + SwerveDriveUI::nt_dist_kp->SetDouble(dist_p); + SwerveDriveUI::nt_dist_ki->SetDouble(dist_i); + SwerveDriveUI::nt_dist_kd->SetDouble(dist_d); - SwerveDriveUI::nt_steer_kp.SetDouble(steer_p); - SwerveDriveUI::nt_steer_ki.SetDouble(steer_i); - SwerveDriveUI::nt_steer_kd.SetDouble(steer_d); + SwerveDriveUI::nt_steer_kp->SetDouble(steer_p); + SwerveDriveUI::nt_steer_ki->SetDouble(steer_i); + SwerveDriveUI::nt_steer_kd->SetDouble(steer_d); return true; } @@ -431,23 +431,23 @@ bool SwerveDrive::UpdateShuffleboard() { // Update encoder UI double encoder_tmp = 0.0; this->GetLeftDriveEncoderAverage(&encoder_tmp); - SwerveDriveUI::nt_left_avg.SetDouble(encoder_tmp); + SwerveDriveUI::nt_left_avg->SetDouble(encoder_tmp); GetRightDriveEncoderAverage(&encoder_tmp); - SwerveDriveUI::nt_right_avg.SetDouble(encoder_tmp); + SwerveDriveUI::nt_right_avg->SetDouble(encoder_tmp); GetDriveEncoderAverage(&encoder_tmp); - SwerveDriveUI::nt_avg_dist.SetDouble(encoder_tmp); + SwerveDriveUI::nt_avg_dist->SetDouble(encoder_tmp); // Heading UI double heading_tmp = 0.0; GetHeading(&heading_tmp); - SwerveDriveUI::nt_heading.SetDouble(heading_tmp); + SwerveDriveUI::nt_heading->SetDouble(heading_tmp); // If competition mode isn't set to true, then allow the PID gains to be // tuned. bool is_competition = RobotParams::GetParam("competition", false); if (!is_competition) { // Update the PID Gains if write mode is true. - if (DrivetrainUI::nt_write_mode.GetBoolean(false)) { + if (SwerveDriveUI::nt_write_mode->GetBoolean(false)) { // Get the current PID parameter values double dist_p = RobotParams::GetParam("swerve.distance_pid.Kp", 0); double dist_i = RobotParams::GetParam("swerve.distance_pid.Ki", 0); @@ -458,13 +458,13 @@ bool SwerveDrive::UpdateShuffleboard() { double steer_d = RobotParams::GetParam("swerve.steer_pid.Kp", 0); // Get the values from shuffleboard. - dist_p = SwerveDriveUI::nt_dist_kp.GetDouble(dist_p); - dist_i = SwerveDriveUI::nt_dist_ki.GetDouble(dist_i); - dist_d = SwerveDriveUI::nt_dist_kd.GetDouble(dist_d); + dist_p = SwerveDriveUI::nt_dist_kp->GetDouble(dist_p); + dist_i = SwerveDriveUI::nt_dist_ki->GetDouble(dist_i); + dist_d = SwerveDriveUI::nt_dist_kd->GetDouble(dist_d); - steer_p = SwerveDriveUI::nt_steer_kp.GetDouble(steer_p); - steer_i = SwerveDriveUI::nt_steer_ki.GetDouble(steer_i); - steer_d = SwerveDriveUI::nt_steer_kd.GetDouble(steer_d); + steer_p = SwerveDriveUI::nt_steer_kp->GetDouble(steer_p); + steer_i = SwerveDriveUI::nt_steer_ki->GetDouble(steer_i); + steer_d = SwerveDriveUI::nt_steer_kd->GetDouble(steer_d); // Update PIDs with values left_front_module.SetDrivePID(dist_p, dist_i, dist_d); @@ -479,17 +479,17 @@ bool SwerveDrive::UpdateShuffleboard() { } // Allow saving parameters in non-competition modes - if (SwerveDriveUI::nt_save.GetBoolean(true)) { + if (SwerveDriveUI::nt_save->GetBoolean(true)) { // Save the parameters. OKC_CALL(RobotParams::SaveParameters(RobotParams::param_file)); - SwerveDriveUI::nt_save.SetBoolean(false); + SwerveDriveUI::nt_save->SetBoolean(false); } } // Resetting the Gyro needs to always be available. - if (SwerveDriveUI::nt_reset_gyro.GetBoolean(false)) { + if (SwerveDriveUI::nt_reset_gyro->GetBoolean(false)) { interface_->reset_gyro = true; - SwerveDriveUI::nt_reset_gyro.SetBoolean(false); + SwerveDriveUI::nt_reset_gyro->SetBoolean(false); } return true; diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index 7218995..a4c75a8 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/src/main/cpp/ui/UserInterface.cpp @@ -99,29 +99,29 @@ namespace SwerveDriveUI { // Add all the defaults // Write mode - nt::GenericEntry &nt_write_mode = + nt::GenericEntry *const nt_write_mode = nt_tab.Add("Write Mode", false).GetEntry(); // Encoder - nt::GenericEntry &nt_left_avg = nt_tab.Add("left avg dist", 0).GetEntry(); - nt::GenericEntry &nt_right_avg = nt_tab.Add("right avg dist", 0).GetEntry(); - nt::GenericEntry &nt_avg_dist = nt_tab.Add("avg dist", 0).GetEntry(); + nt::GenericEntry *const nt_left_avg = nt_tab.Add("left avg dist", 0).GetEntry(); + nt::GenericEntry *const nt_right_avg = nt_tab.Add("right avg dist", 0).GetEntry(); + nt::GenericEntry *const nt_avg_dist = nt_tab.Add("avg dist", 0).GetEntry(); // Distance PID - nt::GenericEntry &nt_dist_kp = nt_tab.Add("Distance kP", 0).GetEntry(); - nt::GenericEntry &nt_dist_ki = nt_tab.Add("Distance kI", 0).GetEntry(); - nt::GenericEntry &nt_dist_kd = nt_tab.Add("Distance kD", 0).GetEntry(); + nt::GenericEntry *const nt_dist_kp = nt_tab.Add("Distance kP", 0).GetEntry(); + nt::GenericEntry *const nt_dist_ki = nt_tab.Add("Distance kI", 0).GetEntry(); + nt::GenericEntry *const nt_dist_kd = nt_tab.Add("Distance kD", 0).GetEntry(); // Steer PID - nt::GenericEntry &nt_steer_kp = nt_tab.Add("Steer kP", 0).GetEntry(); - nt::GenericEntry &nt_steer_ki = nt_tab.Add("Steer kI", 0).GetEntry(); - nt::GenericEntry &nt_steer_kd = nt_tab.Add("Steer kD", 0).GetEntry(); + nt::GenericEntry *const nt_steer_kp = nt_tab.Add("Steer kP", 0).GetEntry(); + nt::GenericEntry *const nt_steer_ki = nt_tab.Add("Steer kI", 0).GetEntry(); + nt::GenericEntry *const nt_steer_kd = nt_tab.Add("Steer kD", 0).GetEntry(); // Gyro - nt::GenericEntry &nt_heading = nt_tab.Add("Heading", 0).GetEntry(); - nt::GenericEntry &nt_reset_gyro = + nt::GenericEntry *const nt_heading = nt_tab.Add("Heading", 0).GetEntry(); + nt::GenericEntry *const nt_reset_gyro = nt_tab.Add("Reset Gyro", false).GetEntry(); // Save parameters button - nt::GenericEntry &nt_save = nt_tab.Add("Save", false).GetEntry(); + nt::GenericEntry *const nt_save = nt_tab.Add("Save", false).GetEntry(); } // namespace SwerveDriveUI \ No newline at end of file diff --git a/src/main/include/ui/UserInterface.h b/src/main/include/ui/UserInterface.h index adb7156..3b47c63 100644 --- a/src/main/include/ui/UserInterface.h +++ b/src/main/include/ui/UserInterface.h @@ -79,27 +79,27 @@ namespace SwerveDriveUI { // Add all the defaults // Write mode - extern nt::GenericEntry &nt_write_mode; + extern nt::GenericEntry *const nt_write_mode; // Encoder - extern nt::GenericEntry &nt_left_avg; - extern nt::GenericEntry &nt_right_avg; - extern nt::GenericEntry &nt_avg_dist; + extern nt::GenericEntry *const nt_left_avg; + extern nt::GenericEntry *const nt_right_avg; + extern nt::GenericEntry *const nt_avg_dist; // Distance PID - extern nt::GenericEntry &nt_dist_kp; - extern nt::GenericEntry &nt_dist_ki; - extern nt::GenericEntry &nt_dist_kd; + extern nt::GenericEntry *const nt_dist_kp; + extern nt::GenericEntry *const nt_dist_ki; + extern nt::GenericEntry *const nt_dist_kd; // Steer PID - extern nt::GenericEntry &nt_steer_kp; - extern nt::GenericEntry &nt_steer_ki; - extern nt::GenericEntry &nt_steer_kd; + extern nt::GenericEntry *const nt_steer_kp; + extern nt::GenericEntry *const nt_steer_ki; + extern nt::GenericEntry *const nt_steer_kd; // Gyro - extern nt::GenericEntry &nt_heading; - extern nt::GenericEntry &nt_reset_gyro; + extern nt::GenericEntry *const nt_heading; + extern nt::GenericEntry *const nt_reset_gyro; // Save drivetrain parameters - extern nt::GenericEntry &nt_save; + extern nt::GenericEntry *const nt_save; } // namespace SwerveDriveUI \ No newline at end of file