diff --git a/README.md b/README.md index 2e696bf..1202357 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,9 @@ SwerveDrive swerveDrive = SwerveParser.fromJSONDirectory(new File(Filesystem.getDeployDirectory(), "swerve")); ``` +# Library Information +* The library is located in [swervelib/](./swervelib) with documentation in [docs/](./docs) and example JSON in [swerve-deploy](./swerve-deploy). + # Images ![Field Shuffleboard](./imgaes/field.png) ![Simulation](./imgaes/simulation.png) diff --git a/SwerveLib/allclasses-index.html b/docs/allclasses-index.html similarity index 100% rename from SwerveLib/allclasses-index.html rename to docs/allclasses-index.html diff --git a/SwerveLib/allclasses.html b/docs/allclasses.html similarity index 100% rename from SwerveLib/allclasses.html rename to docs/allclasses.html diff --git a/SwerveLib/allpackages-index.html b/docs/allpackages-index.html similarity index 100% rename from SwerveLib/allpackages-index.html rename to docs/allpackages-index.html diff --git a/SwerveLib/constant-values.html b/docs/constant-values.html similarity index 100% rename from SwerveLib/constant-values.html rename to docs/constant-values.html diff --git a/SwerveLib/deprecated-list.html b/docs/deprecated-list.html similarity index 100% rename from SwerveLib/deprecated-list.html rename to docs/deprecated-list.html diff --git a/SwerveLib/element-list b/docs/element-list similarity index 100% rename from SwerveLib/element-list rename to docs/element-list diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.SwerveModuleConfig.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.SwerveModuleConfig.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.SwerveModuleConfig.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.SwerveModuleConfig.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveDrive.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveEncoder.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveEncoder.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveEncoder.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveEncoder.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.SwerveModuleLocation.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.SwerveModuleLocation.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.SwerveModuleLocation.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.SwerveModuleLocation.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.Verbosity.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.Verbosity.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.Verbosity.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.Verbosity.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveModule.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveModule.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.ModuleMotorType.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.ModuleMotorType.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.ModuleMotorType.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.ModuleMotorType.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveMotor.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveParser.html b/docs/frc/robot/subsystems/swervedrive/swerve/SwerveParser.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/SwerveParser.html rename to docs/frc/robot/subsystems/swervedrive/swerve/SwerveParser.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/CTRECANCoder.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/CTRECANCoder.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/CTRECANCoder.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/CTRECANCoder.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/PWMAnalogEncoder.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/PWMAnalogEncoder.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/PWMAnalogEncoder.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/PWMAnalogEncoder.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/PWMDutyCycleEncoder.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/PWMDutyCycleEncoder.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/PWMDutyCycleEncoder.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/PWMDutyCycleEncoder.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/REVAbsoluteEncoder.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/REVAbsoluteEncoder.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/REVAbsoluteEncoder.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/REVAbsoluteEncoder.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/package-summary.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/package-summary.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/package-summary.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/package-summary.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/package-tree.html b/docs/frc/robot/subsystems/swervedrive/swerve/encoders/package-tree.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/encoders/package-tree.html rename to docs/frc/robot/subsystems/swervedrive/swerve/encoders/package-tree.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveDriveKinematics2.html b/docs/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveDriveKinematics2.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveDriveKinematics2.html rename to docs/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveDriveKinematics2.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveModuleState2.html b/docs/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveModuleState2.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveModuleState2.html rename to docs/frc/robot/subsystems/swervedrive/swerve/kinematics/SwerveModuleState2.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/package-summary.html b/docs/frc/robot/subsystems/swervedrive/swerve/kinematics/package-summary.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/package-summary.html rename to docs/frc/robot/subsystems/swervedrive/swerve/kinematics/package-summary.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/package-tree.html b/docs/frc/robot/subsystems/swervedrive/swerve/kinematics/package-tree.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/kinematics/package-tree.html rename to docs/frc/robot/subsystems/swervedrive/swerve/kinematics/package-tree.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_pidIdx.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_pidIdx.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_pidIdx.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_pidIdx.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_remoteSensor.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_remoteSensor.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_remoteSensor.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_remoteSensor.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_slotIdx.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_slotIdx.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_slotIdx.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.CTRE_slotIdx.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/CTRESwerveMotor.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.REV_slotIdx.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.REV_slotIdx.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.REV_slotIdx.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.REV_slotIdx.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/package-summary.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/package-summary.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/package-summary.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/package-summary.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/package-tree.html b/docs/frc/robot/subsystems/swervedrive/swerve/motors/package-tree.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/motors/package-tree.html rename to docs/frc/robot/subsystems/swervedrive/swerve/motors/package-tree.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/package-summary.html b/docs/frc/robot/subsystems/swervedrive/swerve/package-summary.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/package-summary.html rename to docs/frc/robot/subsystems/swervedrive/swerve/package-summary.html diff --git a/SwerveLib/frc/robot/subsystems/swervedrive/swerve/package-tree.html b/docs/frc/robot/subsystems/swervedrive/swerve/package-tree.html similarity index 100% rename from SwerveLib/frc/robot/subsystems/swervedrive/swerve/package-tree.html rename to docs/frc/robot/subsystems/swervedrive/swerve/package-tree.html diff --git a/SwerveLib/help-doc.html b/docs/help-doc.html similarity index 100% rename from SwerveLib/help-doc.html rename to docs/help-doc.html diff --git a/SwerveLib/index-files/index-1.html b/docs/index-files/index-1.html similarity index 100% rename from SwerveLib/index-files/index-1.html rename to docs/index-files/index-1.html diff --git a/SwerveLib/index-files/index-10.html b/docs/index-files/index-10.html similarity index 100% rename from SwerveLib/index-files/index-10.html rename to docs/index-files/index-10.html diff --git a/SwerveLib/index-files/index-11.html b/docs/index-files/index-11.html similarity index 100% rename from SwerveLib/index-files/index-11.html rename to docs/index-files/index-11.html diff --git a/SwerveLib/index-files/index-12.html b/docs/index-files/index-12.html similarity index 100% rename from SwerveLib/index-files/index-12.html rename to docs/index-files/index-12.html diff --git a/SwerveLib/index-files/index-13.html b/docs/index-files/index-13.html similarity index 100% rename from SwerveLib/index-files/index-13.html rename to docs/index-files/index-13.html diff --git a/SwerveLib/index-files/index-14.html b/docs/index-files/index-14.html similarity index 100% rename from SwerveLib/index-files/index-14.html rename to docs/index-files/index-14.html diff --git a/SwerveLib/index-files/index-15.html b/docs/index-files/index-15.html similarity index 100% rename from SwerveLib/index-files/index-15.html rename to docs/index-files/index-15.html diff --git a/SwerveLib/index-files/index-16.html b/docs/index-files/index-16.html similarity index 100% rename from SwerveLib/index-files/index-16.html rename to docs/index-files/index-16.html diff --git a/SwerveLib/index-files/index-17.html b/docs/index-files/index-17.html similarity index 100% rename from SwerveLib/index-files/index-17.html rename to docs/index-files/index-17.html diff --git a/SwerveLib/index-files/index-18.html b/docs/index-files/index-18.html similarity index 100% rename from SwerveLib/index-files/index-18.html rename to docs/index-files/index-18.html diff --git a/SwerveLib/index-files/index-19.html b/docs/index-files/index-19.html similarity index 100% rename from SwerveLib/index-files/index-19.html rename to docs/index-files/index-19.html diff --git a/SwerveLib/index-files/index-2.html b/docs/index-files/index-2.html similarity index 100% rename from SwerveLib/index-files/index-2.html rename to docs/index-files/index-2.html diff --git a/SwerveLib/index-files/index-20.html b/docs/index-files/index-20.html similarity index 100% rename from SwerveLib/index-files/index-20.html rename to docs/index-files/index-20.html diff --git a/SwerveLib/index-files/index-21.html b/docs/index-files/index-21.html similarity index 100% rename from SwerveLib/index-files/index-21.html rename to docs/index-files/index-21.html diff --git a/SwerveLib/index-files/index-22.html b/docs/index-files/index-22.html similarity index 100% rename from SwerveLib/index-files/index-22.html rename to docs/index-files/index-22.html diff --git a/SwerveLib/index-files/index-3.html b/docs/index-files/index-3.html similarity index 100% rename from SwerveLib/index-files/index-3.html rename to docs/index-files/index-3.html diff --git a/SwerveLib/index-files/index-4.html b/docs/index-files/index-4.html similarity index 100% rename from SwerveLib/index-files/index-4.html rename to docs/index-files/index-4.html diff --git a/SwerveLib/index-files/index-5.html b/docs/index-files/index-5.html similarity index 100% rename from SwerveLib/index-files/index-5.html rename to docs/index-files/index-5.html diff --git a/SwerveLib/index-files/index-6.html b/docs/index-files/index-6.html similarity index 100% rename from SwerveLib/index-files/index-6.html rename to docs/index-files/index-6.html diff --git a/SwerveLib/index-files/index-7.html b/docs/index-files/index-7.html similarity index 100% rename from SwerveLib/index-files/index-7.html rename to docs/index-files/index-7.html diff --git a/SwerveLib/index-files/index-8.html b/docs/index-files/index-8.html similarity index 100% rename from SwerveLib/index-files/index-8.html rename to docs/index-files/index-8.html diff --git a/SwerveLib/index-files/index-9.html b/docs/index-files/index-9.html similarity index 100% rename from SwerveLib/index-files/index-9.html rename to docs/index-files/index-9.html diff --git a/SwerveLib/index.html b/docs/index.html similarity index 100% rename from SwerveLib/index.html rename to docs/index.html diff --git a/SwerveLib/jquery/external/jquery/jquery.js b/docs/jquery/external/jquery/jquery.js similarity index 100% rename from SwerveLib/jquery/external/jquery/jquery.js rename to docs/jquery/external/jquery/jquery.js diff --git a/SwerveLib/jquery/images/ui-bg_glass_55_fbf9ee_1x400.png b/docs/jquery/images/ui-bg_glass_55_fbf9ee_1x400.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_glass_55_fbf9ee_1x400.png rename to docs/jquery/images/ui-bg_glass_55_fbf9ee_1x400.png diff --git a/SwerveLib/jquery/images/ui-bg_glass_65_dadada_1x400.png b/docs/jquery/images/ui-bg_glass_65_dadada_1x400.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_glass_65_dadada_1x400.png rename to docs/jquery/images/ui-bg_glass_65_dadada_1x400.png diff --git a/SwerveLib/jquery/images/ui-bg_glass_75_dadada_1x400.png b/docs/jquery/images/ui-bg_glass_75_dadada_1x400.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_glass_75_dadada_1x400.png rename to docs/jquery/images/ui-bg_glass_75_dadada_1x400.png diff --git a/SwerveLib/jquery/images/ui-bg_glass_75_e6e6e6_1x400.png b/docs/jquery/images/ui-bg_glass_75_e6e6e6_1x400.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_glass_75_e6e6e6_1x400.png rename to docs/jquery/images/ui-bg_glass_75_e6e6e6_1x400.png diff --git a/SwerveLib/jquery/images/ui-bg_glass_95_fef1ec_1x400.png b/docs/jquery/images/ui-bg_glass_95_fef1ec_1x400.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_glass_95_fef1ec_1x400.png rename to docs/jquery/images/ui-bg_glass_95_fef1ec_1x400.png diff --git a/SwerveLib/jquery/images/ui-bg_highlight-soft_75_cccccc_1x100.png b/docs/jquery/images/ui-bg_highlight-soft_75_cccccc_1x100.png similarity index 100% rename from SwerveLib/jquery/images/ui-bg_highlight-soft_75_cccccc_1x100.png rename to docs/jquery/images/ui-bg_highlight-soft_75_cccccc_1x100.png diff --git a/SwerveLib/jquery/images/ui-icons_222222_256x240.png b/docs/jquery/images/ui-icons_222222_256x240.png similarity index 100% rename from SwerveLib/jquery/images/ui-icons_222222_256x240.png rename to docs/jquery/images/ui-icons_222222_256x240.png diff --git a/SwerveLib/jquery/images/ui-icons_2e83ff_256x240.png b/docs/jquery/images/ui-icons_2e83ff_256x240.png similarity index 100% rename from SwerveLib/jquery/images/ui-icons_2e83ff_256x240.png rename to docs/jquery/images/ui-icons_2e83ff_256x240.png diff --git a/SwerveLib/jquery/images/ui-icons_454545_256x240.png b/docs/jquery/images/ui-icons_454545_256x240.png similarity index 100% rename from SwerveLib/jquery/images/ui-icons_454545_256x240.png rename to docs/jquery/images/ui-icons_454545_256x240.png diff --git a/SwerveLib/jquery/images/ui-icons_888888_256x240.png b/docs/jquery/images/ui-icons_888888_256x240.png similarity index 100% rename from SwerveLib/jquery/images/ui-icons_888888_256x240.png rename to docs/jquery/images/ui-icons_888888_256x240.png diff --git a/SwerveLib/jquery/images/ui-icons_cd0a0a_256x240.png b/docs/jquery/images/ui-icons_cd0a0a_256x240.png similarity index 100% rename from SwerveLib/jquery/images/ui-icons_cd0a0a_256x240.png rename to docs/jquery/images/ui-icons_cd0a0a_256x240.png diff --git a/SwerveLib/jquery/jquery-3.5.1.js b/docs/jquery/jquery-3.5.1.js similarity index 100% rename from SwerveLib/jquery/jquery-3.5.1.js rename to docs/jquery/jquery-3.5.1.js diff --git a/SwerveLib/jquery/jquery-ui.css b/docs/jquery/jquery-ui.css similarity index 100% rename from SwerveLib/jquery/jquery-ui.css rename to docs/jquery/jquery-ui.css diff --git a/SwerveLib/jquery/jquery-ui.js b/docs/jquery/jquery-ui.js similarity index 100% rename from SwerveLib/jquery/jquery-ui.js rename to docs/jquery/jquery-ui.js diff --git a/SwerveLib/jquery/jquery-ui.min.css b/docs/jquery/jquery-ui.min.css similarity index 100% rename from SwerveLib/jquery/jquery-ui.min.css rename to docs/jquery/jquery-ui.min.css diff --git a/SwerveLib/jquery/jquery-ui.min.js b/docs/jquery/jquery-ui.min.js similarity index 100% rename from SwerveLib/jquery/jquery-ui.min.js rename to docs/jquery/jquery-ui.min.js diff --git a/SwerveLib/jquery/jquery-ui.structure.css b/docs/jquery/jquery-ui.structure.css similarity index 100% rename from SwerveLib/jquery/jquery-ui.structure.css rename to docs/jquery/jquery-ui.structure.css diff --git a/SwerveLib/jquery/jquery-ui.structure.min.css b/docs/jquery/jquery-ui.structure.min.css similarity index 100% rename from SwerveLib/jquery/jquery-ui.structure.min.css rename to docs/jquery/jquery-ui.structure.min.css diff --git a/SwerveLib/jquery/jszip-utils/dist/jszip-utils-ie.js b/docs/jquery/jszip-utils/dist/jszip-utils-ie.js similarity index 100% rename from SwerveLib/jquery/jszip-utils/dist/jszip-utils-ie.js rename to docs/jquery/jszip-utils/dist/jszip-utils-ie.js diff --git a/SwerveLib/jquery/jszip-utils/dist/jszip-utils-ie.min.js b/docs/jquery/jszip-utils/dist/jszip-utils-ie.min.js similarity index 100% rename from SwerveLib/jquery/jszip-utils/dist/jszip-utils-ie.min.js rename to docs/jquery/jszip-utils/dist/jszip-utils-ie.min.js diff --git a/SwerveLib/jquery/jszip-utils/dist/jszip-utils.js b/docs/jquery/jszip-utils/dist/jszip-utils.js similarity index 100% rename from SwerveLib/jquery/jszip-utils/dist/jszip-utils.js rename to docs/jquery/jszip-utils/dist/jszip-utils.js diff --git a/SwerveLib/jquery/jszip-utils/dist/jszip-utils.min.js b/docs/jquery/jszip-utils/dist/jszip-utils.min.js similarity index 100% rename from SwerveLib/jquery/jszip-utils/dist/jszip-utils.min.js rename to docs/jquery/jszip-utils/dist/jszip-utils.min.js diff --git a/SwerveLib/jquery/jszip/dist/jszip.js b/docs/jquery/jszip/dist/jszip.js similarity index 100% rename from SwerveLib/jquery/jszip/dist/jszip.js rename to docs/jquery/jszip/dist/jszip.js diff --git a/SwerveLib/jquery/jszip/dist/jszip.min.js b/docs/jquery/jszip/dist/jszip.min.js similarity index 100% rename from SwerveLib/jquery/jszip/dist/jszip.min.js rename to docs/jquery/jszip/dist/jszip.min.js diff --git a/SwerveLib/member-search-index.js b/docs/member-search-index.js similarity index 100% rename from SwerveLib/member-search-index.js rename to docs/member-search-index.js diff --git a/SwerveLib/overview-summary.html b/docs/overview-summary.html similarity index 100% rename from SwerveLib/overview-summary.html rename to docs/overview-summary.html diff --git a/SwerveLib/overview-tree.html b/docs/overview-tree.html similarity index 100% rename from SwerveLib/overview-tree.html rename to docs/overview-tree.html diff --git a/SwerveLib/package-search-index.js b/docs/package-search-index.js similarity index 100% rename from SwerveLib/package-search-index.js rename to docs/package-search-index.js diff --git a/SwerveLib/resources/glass.png b/docs/resources/glass.png similarity index 100% rename from SwerveLib/resources/glass.png rename to docs/resources/glass.png diff --git a/SwerveLib/resources/x.png b/docs/resources/x.png similarity index 100% rename from SwerveLib/resources/x.png rename to docs/resources/x.png diff --git a/SwerveLib/script.js b/docs/script.js similarity index 100% rename from SwerveLib/script.js rename to docs/script.js diff --git a/SwerveLib/search.js b/docs/search.js similarity index 100% rename from SwerveLib/search.js rename to docs/search.js diff --git a/SwerveLib/stylesheet.css b/docs/stylesheet.css similarity index 100% rename from SwerveLib/stylesheet.css rename to docs/stylesheet.css diff --git a/SwerveLib/type-search-index.js b/docs/type-search-index.js similarity index 100% rename from SwerveLib/type-search-index.js rename to docs/type-search-index.js diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java new file mode 100644 index 0000000..90435de --- /dev/null +++ b/swervelib/SwerveDrive.java @@ -0,0 +1,709 @@ +package frc.robot.subsystems.swervedrive.swerve; + +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.RobotDriveBase; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; +import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation; +import frc.robot.subsystems.swervedrive.swerve.SwerveModule.Verbosity; +import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType; +import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveDriveKinematics2; +import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2; +import java.io.Closeable; + +/** + * SwerveDrive base which is meant to be platform agnostic. This implementation expect second order kinematics because + * second order kinematics prevents the drift that builds up when using first order kinematics. As per this ChiefDelphi + * post + */ +public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseable +{ + + + /** + * Count of SwerveModule instances created. + */ + private static int instances; + + /** + * Front left swerve drive + */ + public final SwerveModule m_frontLeft; + /** + * Back left swerve drive + */ + public final SwerveModule m_backLeft; + /** + * Front right swerve drive + */ + public final SwerveModule m_frontRight; + /** + * Back right swerve drive + */ + public final SwerveModule m_backRight; + /** + * Maximum speed in meters per second. + */ + public final double m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS; + /** + * Swerve drive kinematics. + */ + private final SwerveDriveKinematics2 m_swerveKinematics; + /** + * Swerve drive pose estimator for attempting to figure out our current position. + */ + private final SwerveDrivePoseEstimator m_swervePoseEstimator; + /** + * Pigeon 2.0 centered on the robot. + */ + private final WPI_Pigeon2 m_pigeonIMU; + /** + * Field2d displayed on shuffleboard with current position. + */ + private final Field2d m_field = new Field2d(); + /** + * The slew rate limiters to make control smooth. + */ + private final SlewRateLimiter m_xLimiter, m_yLimiter, m_turningLimiter; + private final Timer m_driveTimer; + /** + * Invert the gyro reading. + */ + private boolean m_gyroInverted; + private double m_angle; + private ChassisSpeeds m_prevChassisSpeed = new ChassisSpeeds(0, 0, 0); + private double m_timerPrev; + + /** + * Constructor for Swerve Drive assuming modules have been created and configured with PIDF and conversions. + * + * @param frontLeft Front left swerve module configured. + * @param backLeft Back left swerve module. + * @param frontRight Front right swerve module. + * @param backRight Back right swerve module + * @param pigeon Pigeon IMU. + * @param driverMaxSpeedMetersPerSecond Maximum speed for all modules to follow when in teleop. + * @param driverMaxAngularVelocityRadiansPerSecond Maximum angular velocity for turning when using the drive + * function. + * @param driverMaxDriveAccelerationMetersPerSecond Maximum acceleration in meters per second for the drive motors + * when in teleop for the slew rate limiter. + * @param driverMaxAngularAccelerationRadiansPerSecond Maximum angular acceleration in meters per second for the + * steering motors when in teleop for the slew rate limiters. + * @param physicalMaxSpeedMPS Maximum speed a module can go physically, used to desaturate + * wheel speeds. + * @param gyroInverted Invert the gyroscope for the robot. + * @param start The starting pose on the field. + */ + public SwerveDrive(SwerveModule frontLeft, + SwerveModule frontRight, + SwerveModule backLeft, + SwerveModule backRight, WPI_Pigeon2 pigeon, + double driverMaxSpeedMetersPerSecond, double driverMaxAngularVelocityRadiansPerSecond, + double driverMaxDriveAccelerationMetersPerSecond, + double driverMaxAngularAccelerationRadiansPerSecond, + double physicalMaxSpeedMPS, + boolean gyroInverted, Pose2d start) + { + instances++; + if (instances > 1) + { + throw new RuntimeException("Cannot use more than one swerve drive instance at a time."); + } + m_driveTimer = new Timer(); + m_driveTimer.reset(); + m_driveTimer.start(); + m_timerPrev = m_driveTimer.get(); + + m_frontLeft = frontLeft; + m_backRight = backRight; + m_backLeft = backLeft; + m_frontRight = frontRight; + m_gyroInverted = gyroInverted; + + m_driverMaxSpeedMPS = driverMaxSpeedMetersPerSecond; + m_driverMaxAngularVelocity = driverMaxAngularVelocityRadiansPerSecond; + m_xLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond); + m_yLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond); + m_turningLimiter = new SlewRateLimiter(driverMaxAngularAccelerationRadiansPerSecond); + m_physicalMaxSpeedMPS = physicalMaxSpeedMPS; + + m_pigeonIMU = pigeon; + configurePigeonIMU(); // Reset pigeon to 0 and default settings. + + m_swerveKinematics = new SwerveDriveKinematics2(frontLeft.swerveModuleLocation, + frontRight.swerveModuleLocation, + backLeft.swerveModuleLocation, + backRight.swerveModuleLocation); + m_swervePoseEstimator = new SwerveDrivePoseEstimator( + m_swerveKinematics, + getRotation(), + getPositions(), + start, + VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight + VecBuilder.fill(0.9, 1.0, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight + + // Inspired by https://github.com/Team364/BaseFalconSwerve/blob/main/src/main/java/frc/robot/subsystems/Swerve.java + SmartDashboard.putData(m_field); + SmartDashboard.putData(m_pigeonIMU); + + setVoltageCompensation(); + zeroModules(); // Set all modules to 0. + } + + /** + * Enable voltage compensation to the current battery voltage on all modules. + */ + public void setVoltageCompensation() + { + double currentVoltage = RobotController.getBatteryVoltage(); + m_frontLeft.setVoltageCompensation(currentVoltage); + m_frontRight.setVoltageCompensation(currentVoltage); + m_backLeft.setVoltageCompensation(currentVoltage); + m_backRight.setVoltageCompensation(currentVoltage); + } + + + /** + * Create swerve drive modules + * + * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks + * per rotation. + * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or + * rotations/1), only applied if using Neo's. + * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters. + * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters. + * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters. + * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor. + * @param maxSpeedMPS The maximum drive speed in meters per second. + * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second. + * @param steeringMotorInverted The steering motor is inverted. + * @param drivingMotorInverted The driving motor is inverted. + * @param configs The swerve module configuration classes for the swerve drive given. + * @return Array of swerve modules in the order of front left, front right, back left, back right. + */ + public static SwerveModule[] createModules( + double driveGearRatio, double steerGearRatio, double wheelDiameterMeters, double wheelBaseMeters, + double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS, + double maxDriveAcceleration, boolean steeringMotorInverted, boolean drivingMotorInverted, + SwerveModuleConfig[] configs) + { + SwerveModule[] modules = new SwerveModule[configs.length]; + for (int i = 0; i < configs.length; i++) + { + int loc; + switch (configs[i].loc) + { + case FrontLeft: + loc = 0; + break; + case BackLeft: + loc = 2; + break; + case FrontRight: + loc = 1; + break; + case BackRight: + loc = 3; + break; + default: + loc = i; + } + + modules[loc] = configs[i].createModule(driveGearRatio, steerGearRatio, wheelDiameterMeters, wheelBaseMeters, + driveTrainWidthMeters, steeringMotorFreeSpeedRPM, maxSpeedMPS, + maxDriveAcceleration, steeringMotorInverted, drivingMotorInverted); + } + return modules; + } + + /** + * Sets the speed to 0 and angle to 0 for all the swerve drive modules. + */ + public void zeroModules() + { + // feedWatchdog(); + // m_frontLeft.setAngle(0, 0); + // m_backLeft.setAngle(0, 0); + // m_frontRight.setAngle(0, 0); + // m_backRight.setAngle(0, 0); + m_frontRight.set(0); + m_backLeft.set(0); + m_frontLeft.set(0); + m_backRight.set(0); + set(0, 0, 0, false); + } + + /** + * Configure the PigeonIMU with factory default settings and a zeroed gyroscope. + */ + public void configurePigeonIMU() + { + m_pigeonIMU.configFactoryDefault(); + zeroGyro(); + } + + /** + * Update the swerve drive odometry. + * + * @return Swerve drive odometry. + */ + public SwerveDrivePoseEstimator update() + { + Pose2d pos = m_swervePoseEstimator.update(getRotation(), getPositions()); + if (!Robot.isReal()) + { + m_pigeonIMU.setYaw(pos.getRotation().getDegrees()); + } + return m_swervePoseEstimator; + } + + /** + * Drive swerve based off controller input. + * + * @param forward The speed (-1 to 1) in which the Y axis should go. + * @param strafe The speed (-1 to 1) in which the X axis should go. + * @param turn The speed (-1 to 1) in which the robot should turn. + * @param fieldRelative Whether or not to use field relative controls. + */ + public void drive(double forward, double strafe, double turn, boolean fieldRelative) + { + // 2. Apply deadband/Dead-Zone + forward = Math.abs(forward) > m_deadband ? forward : 0.0; + strafe = Math.abs(strafe) > m_deadband ? strafe : 0.0; + turn = Math.abs(turn) > m_deadband ? turn : 0.0; + + // If nothing is asked of us we do nothing. + if ((Math.abs(forward) + Math.abs(strafe) + Math.abs(turn)) <= m_deadband) + { + zeroModules(); + return; + } + + // 3. Make the driving smoother + forward = m_xLimiter.calculate(forward) * m_driverMaxSpeedMPS; + strafe = m_yLimiter.calculate(strafe) * m_driverMaxSpeedMPS; + turn = m_turningLimiter.calculate(turn) * m_driverMaxAngularVelocity; + + set(forward, strafe, turn, fieldRelative); + } + + + /** + * Swerve drive function + * + * @param forward forward meters per second, strafe facing left from alliance wall + * @param strafe strafe meters per second, forward away from alliance wall + * @param radianPerSecond radians per second + * @param fieldRelative field relative + */ + public void set(double forward, double strafe, double radianPerSecond, boolean fieldRelative) + { + ChassisSpeeds node = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, radianPerSecond, + getRotation()) + : new ChassisSpeeds(forward, strafe, radianPerSecond); + set(node); + } + + /** + * Set the swerve drive module states. + * + * @param node Chassis speeds to set the swerve module too. + */ + public void set(ChassisSpeeds node) + { + + // Taken from https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5?u=nstrike + double timerDt = (m_driveTimer.get() - m_timerPrev); + Pose2d robot_pose_vel = new Pose2d(node.vxMetersPerSecond * timerDt, + node.vyMetersPerSecond * timerDt, + Rotation2d.fromRadians(node.omegaRadiansPerSecond * timerDt)); + Twist2d twist_vel = new Pose2d(0, 0, new Rotation2d(0)).log(robot_pose_vel); + ChassisSpeeds updated_chassis_speeds = new ChassisSpeeds( + twist_vel.dx / timerDt, twist_vel.dy / timerDt, twist_vel.dtheta / timerDt); + + SwerveModuleState2[] moduleStates = m_swerveKinematics.toSwerveModuleStates(updated_chassis_speeds); +// new Translation2d((m_frontLeft.swerveModuleLocation.getX() + m_frontRight.swerveModuleLocation.getX()) / 2, +// (m_frontLeft.swerveModuleLocation.getY() + m_backLeft.swerveModuleLocation.getY()) / 2)); + + setModuleStates(moduleStates); + m_prevChassisSpeed = updated_chassis_speeds; + m_timerPrev = m_driveTimer.get(); + + try + { + if (!Robot.isReal()) + { + m_angle += updated_chassis_speeds.omegaRadiansPerSecond * 0.02; + } + this.update(); + m_field.setRobotPose(m_swervePoseEstimator.getEstimatedPosition()); + } catch (Exception e) + { + System.err.println("Cannot update SwerveDrive Odometry!"); + } + } + + /** + * Set the swerve module states given an array of states. Normalize the wheel speeds to abide by maximum supplied + * + * @param states Module states in a specified order. [front left, front right, back left, back right] + * @throws RuntimeException If the CANCoder is inaccessible or not configured. + */ + public void setModuleStates(SwerveModuleState2[] states) + { + feedWatchdog(); // Required + SwerveDriveKinematics2.desaturateWheelSpeeds(states, m_physicalMaxSpeedMPS); + m_frontLeft.setState(states[0]); + m_frontRight.setState(states[1]); + m_backLeft.setState(states[2]); + m_backRight.setState(states[3]); + } + + /** + * Sets the wheels into an X formation to prevent movement. + */ + public void setX() + { + setModuleStates(new SwerveModuleState2[]{new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0), + new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0), + new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0), + new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0)}); + } + + /** + * Invert the gyroscope reading. + * + * @param isInverted Inversion of the gryoscope, true is inverted. + */ + public void setGyroInverted(boolean isInverted) + { + m_gyroInverted = isInverted; + } + + /** + * Get the current robot rotation. + * + * @return {@link Rotation2d} of the robot. + */ + public Rotation2d getRotation() + { + if (Robot.isReal()) + { + return m_gyroInverted ? Rotation2d.fromDegrees(360 - m_pigeonIMU.getYaw()) : Rotation2d.fromDegrees( + m_pigeonIMU.getYaw()); + } else + { + return new Rotation2d(m_angle); + } + } + + /** + * Get the current Pose, used in autonomous. + * + * @return Current pose based off odometry. + */ + public Pose2d getPose() + { + return m_swervePoseEstimator.getEstimatedPosition(); + } + + /** + * Get current swerve module positions in order. + * + * @return Swerve module positions array. + */ + public SwerveModulePosition[] getPositions() + { + return new SwerveModulePosition[]{m_frontLeft.getPosition(), m_frontRight.getPosition(), + m_backLeft.getPosition(), m_backRight.getPosition()}; + } + + /** + * Get current swerve module states in order. + * + * @return Swerve module states array. + */ + public SwerveModuleState2[] getStates() + { + return new SwerveModuleState2[]{m_frontLeft.getState(), m_frontRight.getState(), + m_backLeft.getState(), m_backRight.getState()}; + } + + /** + * Reset the odometry given the position and using current rotation from the PigeonIMU 2. + * + * @param pose Current position on the field. + */ + public void resetOdometry(Pose2d pose) + { + m_swervePoseEstimator.resetPosition(getRotation(), getPositions(), pose); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() + { + m_frontLeft.resetEncoders(); + m_backLeft.resetEncoders(); + m_frontRight.resetEncoders(); + m_backRight.resetEncoders(); + } + + /** + * Set the current rotation of the gyroscope (pigeonIMU 2) to 0. + */ + public void zeroGyro() + { + m_pigeonIMU.setYaw(0); + } + + /** + * Stop all running and turning motors. + */ + @Override + public void stopMotor() + { + feedWatchdog(); + m_frontLeft.stopMotor(); + m_frontRight.stopMotor(); + m_backLeft.stopMotor(); + m_backRight.stopMotor(); + } + + /** + * Update swerve module parameters based on data in the dashboard. + */ + public void subscribe() + { + m_frontRight.subscribe(); + m_frontLeft.subscribe(); + m_backLeft.subscribe(); + m_backRight.subscribe(); + } + + /** + * Publish data from the Swerve Modules to the dashboard. + * + * @param level Verbosity level in terms of CAN utilization. + */ + public void publish(Verbosity level) + { + m_frontRight.publish(level); + m_frontLeft.publish(level); + m_backRight.publish(level); + m_backLeft.publish(level); + } + + /** + * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID + *

+ * P = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations + * stop and angle is perfect or near perfect. + *

+ *

+ * I = 0 tune this if your PID never quite reaches the target, after tuning D. Increase this by + * P*.01 each time and adjust accordingly. + *

+ *

+ * D = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by P*10 + * and adjust accordingly. + *

+ *

+ * F = 0 tune this if the PID is being used for velocity, the F is multiplied by the target and added + * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner. + *

+ * Documentation for this is best described by CTRE here. + * + * @param p Proportional gain for closed loop. This is multiplied by closed loop error in sensor units. + * @param i Integral gain for closed loop. This is multiplied by closed loop error in sensor units every + * PID Loop. + * @param d Derivative gain for closed loop. This is multiplied by derivative error (sensor units per + * PID loop). + * @param f Feed Fwd gain for Closed loop. + * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too + * far from the target. This prevents unstable oscillation if the kI is too large. Value is in + * sensor units. + * @param moduleMotorType Swerve drive motor type. + */ + public void setPIDF(double p, double i, double d, double f, double integralZone, + ModuleMotorType moduleMotorType) + { + m_frontRight.setPIDF(p, i, d, f, integralZone, moduleMotorType); + m_frontLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType); + m_backRight.setPIDF(p, i, d, f, integralZone, moduleMotorType); + m_backLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType); + } + + /** + * Synchronize internal steering encoders with the absolute encoder. + */ + public void synchronizeEncoders() + { + m_backRight.synchronizeSteeringEncoder(); + m_backLeft.synchronizeSteeringEncoder(); + m_frontRight.synchronizeSteeringEncoder(); + m_frontLeft.synchronizeSteeringEncoder(); + } + + /** + * Get the description of the robot drive base. + * + * @return string of the RobotDriveBase + */ + @Override + public String getDescription() + { + return "Swerve Drive Base"; + } + + /** + * Initializes this {@link Sendable} object. + * + * @param builder sendable builder + */ + @Override + public void initSendable(SendableBuilder builder) + { +// builder.setSmartDashboardType("SwerveDrive"); +// SendableRegistry.addChild(this, m_frontLeft); +// SendableRegistry.addChild(this, m_frontRight); +// SendableRegistry.addChild(this, m_backLeft); +// SendableRegistry.addChild(this, m_backRight); +// SendableRegistry.addChild(this, m_field); +// SendableRegistry.addChild(this, m_pigeonIMU); + } + + /** + * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects + * managed by the {@code try}-with-resources statement. + * + *

While this interface method is declared to throw {@code + * Exception}, implementers are strongly encouraged to declare concrete implementations of the {@code close} + * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail. + * + *

Cases where the close operation may fail require careful + * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally + * mark the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be + * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it + * reduces problems that could arise when the resource wraps, or is wrapped, by another resource. + * + *

Implementers of this interface are also strongly advised + * to not have the {@code close} method throw {@link InterruptedException}. + *

+ * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an + * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}. + *

+ * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close} + * method should not throw it. + * + *

Note that unlike the {@link Closeable#close close} + * method of {@link Closeable}, this {@code close} method is not required to be idempotent. In other words, + * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close} + * which is required to have no effect if called more than once. + *

+ * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent. + * + * @throws Exception if this resource cannot be closed + */ + @Override + public void close() throws Exception + { + SendableRegistry.remove(this); + } + + /** + * Helper class for easier swerve module creation + * + * @param The motor type for the drive motor on the swerve moduule. + * @param The motor type for the steering motor on the module. + * @param The absolute encoder type. + */ + public static class SwerveModuleConfig + { + + public DriveMotorType drive; + public SteeringMotorType steering; + public AbsoluteEncoder encoder; + public double angleOffset; + public SwerveModuleLocation loc; + + /** + * Swerve Module configuration class to define the motor CAN IDs and absolute encoder offset of a swerve module. + * + * @param driveMotor Drive motor for the swerve module. + * @param steerMotor Steer motor for the swerve module. + * @param encoderSteering CANCoder for the steering motor on the swerve module. + * @param offset Absolute encoder offset. + * @param location Swerve Moduule location on the chassis. + */ + public SwerveModuleConfig(DriveMotorType driveMotor, SteeringMotorType steerMotor, AbsoluteEncoder encoderSteering, + double offset, + SwerveModuleLocation location) + { + drive = driveMotor; + steering = steerMotor; + angleOffset = offset; + encoder = encoderSteering; + loc = location; + } + + /** + * Create the swerve module from the configuration. + * + * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder + * ticks per rotation. + * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or + * rotations/1), only applied if using Neo's. + * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters. + * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters. + * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters. + * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor. + * @param maxSpeedMPS The maximum drive speed in meters per second. + * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second. + * @param steeringInverted The steering motor is inverted. + * @param drivingInverted The driving motor is inverted. + * @return The Swerve Module. + */ + public SwerveModule createModule(double driveGearRatio, + double steerGearRatio, + double wheelDiameterMeters, + double wheelBaseMeters, + double driveTrainWidthMeters, + double steeringMotorFreeSpeedRPM, + double maxSpeedMPS, + double maxDriveAcceleration, + boolean steeringInverted, + boolean drivingInverted) + { + return new SwerveModule<>(drive, steering, encoder, loc, + driveGearRatio, steerGearRatio, + angleOffset, wheelDiameterMeters, + wheelBaseMeters, + driveTrainWidthMeters, + steeringMotorFreeSpeedRPM, + maxSpeedMPS, maxDriveAcceleration, .6, .4, steeringInverted, drivingInverted); + } + } + +} \ No newline at end of file diff --git a/swervelib/SwerveEncoder.java b/swervelib/SwerveEncoder.java new file mode 100644 index 0000000..408e67a --- /dev/null +++ b/swervelib/SwerveEncoder.java @@ -0,0 +1,102 @@ +package frc.robot.subsystems.swervedrive.swerve; + +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import com.revrobotics.AbsoluteEncoder; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.subsystems.swervedrive.swerve.encoders.CTRECANCoder; +import frc.robot.subsystems.swervedrive.swerve.encoders.PWMAnalogEncoder; +import frc.robot.subsystems.swervedrive.swerve.encoders.PWMDutyCycleEncoder; +import frc.robot.subsystems.swervedrive.swerve.encoders.REVAbsoluteEncoder; + +/** + * Swerve Encoder class definition for common interfaces. + */ +public abstract class SwerveEncoder +{ + + /** + * The encoder can be directly referenced for configuration purposes of onboard PIDs. + */ + public AbsoluteEncoderType m_encoder; + + /** + * Get the SwerveEncoder class from the given encoder types + * + * @param encoder Encoder + * @param One of DutyCycleEncoder, AnalogEncoder, AbsoluteEncoder, and CANCoder + * @return SwerveEncoder subclass + */ + public static SwerveEncoder fromEncoder(AbsoluteEncoderType encoder) + { + if (encoder instanceof CANCoder) + { + return new CTRECANCoder((CANCoder) encoder); + } else if (encoder instanceof DutyCycleEncoder) + { + return new PWMDutyCycleEncoder((DutyCycleEncoder) encoder); + } else if (encoder instanceof AnalogEncoder) + { + return new PWMAnalogEncoder((AnalogEncoder) encoder); + } else if (encoder instanceof AbsoluteEncoder) + { + return new REVAbsoluteEncoder((AbsoluteEncoder) encoder); + } + + return null; + } + + /** + * Configure the absolute encoder if possible. + */ + public abstract void configure(); + + /** + * Reset encoder to factory default settings, if possible. + */ + public abstract void factoryDefault(); + + /** + * Get the magnetic field strength, if available. + * + * @return CTRE MagneticFieldStrength Enum. + */ + public abstract MagnetFieldStrength getMagnetFieldStrength(); + + /** + * Get the absolute position in degrees. + * + * @return Absolute position (0, 360] + */ + public abstract double getAbsolutePosition(); + + /** + * Get the velocity of the absolute encoder in degrees per second. + * + * @return Velocity in degrees per second. + */ + public abstract double getVelocity(); + + /** + * Configure the magnetic offset for the AbsoluteEncoder. + * + * @param offset Offset in degrees. + */ + public abstract void setOffset(double offset); + + /** + * Is the encoder reachable? + * + * @return True if reachable, false otherwise. + */ + public abstract boolean reachable(); + + /** + * Configure the sensor direction + * + * @param isInverted Inverted or not. + */ + public abstract void setInverted(boolean isInverted); + +} diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java new file mode 100644 index 0000000..994d19f --- /dev/null +++ b/swervelib/SwerveModule.java @@ -0,0 +1,938 @@ +package frc.robot.subsystems.swervedrive.swerve; + +import static java.util.Objects.requireNonNull; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; +import frc.robot.Robot; +import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType; +import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2; +import java.io.Closeable; +import java.util.HashMap; +import java.util.Map; + +/** + * Swerve module for representing a single swerve module of the robot. + * + * @param Main motor type that drives the wheel. + * @param Motor that controls the angle of the wheel. + * @param Absolute encoder for the swerve drive module. + */ +public class SwerveModule + implements MotorController, Sendable, AutoCloseable +{ + + /** + * Swerve Module location object relative to the center of the robot. + */ + public final Translation2d swerveModuleLocation; + /** + * Motor Controllers for drive motor of the swerve module. + */ + public final SwerveMotor driveMotor; + /*** + * Motor Controller for the turning motor of the swerve drive module. + */ + public final SwerveMotor turningMotor; + /** + * Enum representing the swerve module's location on the robot, assuming square. + */ + private final SwerveModuleLocation swerveLocation; + /** + * Absolute encoder for the swerve module. + */ + private final SwerveEncoder absoluteEncoder; + /** + * The Distance between centers of right and left wheels in meters. + */ + private final double driveTrainWidth; + /** + * The Distance between front and back wheels of the robot in meters. + */ + private final double wheelBase; + /** + * Drive feedforward for PID when driving by velocity. + */ + private final SimpleMotorFeedforward driveFeedforward; + /** + * kV for steering feedforward. + */ + private final double steeringKV; + private final ShuffleboardTab moduleTab; + private final HashMap NT4Entries = new HashMap<>(); + /** + * Angle offset of the CANCoder at initialization. + */ + public double angleOffset = 0; + /** + * Maximum speed in meters per second, used to eliminate unnecessary movement of the module. + */ + public double maxDriveSpeedMPS; + /** + * Inverted drive motor. + */ + private boolean invertedDrive = false; + /** + * Inverted turning motor. + */ + private boolean invertedTurn = false; + /** + * Power to drive motor from -1 to 1. + */ + private double drivePower = 0; + /** + * Store the last angle for optimization. + */ + private double targetAngle; + /** + * Angular velocity in radians per second. + */ + private double targetAngularVelocityRPS = 0; + /** + * Target velocity for the swerve module. + */ + private double targetVelocity = 0; + + ///////////////////////////// CONFIGURATION FUNCTIONS SECTION /////////////////////////////////////////////////// + + /** + * Swerve module constructor. Both motors MUST be a {@link MotorController} class. It is recommended to create + * a command to reset the encoders when triggered and + * + * @param mainMotor Main drive motor. Must be a {@link MotorController} type. + * @param angleMotor Angle motor for controlling the angle of the swerve module. + * @param encoder Absolute encoder for the swerve module. + * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks + * per rotation. + * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or + * rotations/1), only applied if using Neo's. + * @param swervePosition Swerve Module position on the robot. + * @param steeringOffsetDegrees The current offset of the absolute encoder from 0 in degrees. + * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters. + * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters. + * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters. + * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor. + * @param maxSpeedMPS The maximum drive speed in meters per second. + * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second. + * @param drivingPowerLimit The power limit for the closed loop PID of the driver motor. + * @param steeringPowerLimit The power limit for the closed loop PID of the steering motor. + * @param steeringInverted The steering motor is inverted. + * @param drivingInverted The driving motor is inverted. + * @throws RuntimeException if an assertion fails or invalid swerve module location is given. + */ + public SwerveModule(DriveMotorType mainMotor, AngleMotorType angleMotor, AbsoluteEncoderType encoder, + SwerveModuleLocation swervePosition, double driveGearRatio, double steerGearRatio, + double steeringOffsetDegrees, double wheelDiameterMeters, double wheelBaseMeters, + double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS, + double maxDriveAcceleration, double drivingPowerLimit, double steeringPowerLimit, + boolean steeringInverted, boolean drivingInverted) + { + // Steps to configure swerve drive are as follows + // 1. Set Current limit of turning motor to 20 amps + // 2. Enable voltage compensation with optimal battery voltage + // 3. Configure CANCoder + // 4. Set inverted motors. + // 5. Configure status frames + // 6. Set all motors to brake mode. + // 7. Set velocity and position conversion factors on drive motor encoder. + // 8. Set PIDF with integral zone on drive motor controller PID. + // 9. Set velocity and position conversion factors on turning motor controller. (Usually P=0.01,I=0,D=0,F=0,IZ=1) + // 10. Set PIDF with integral zone on turning motor controller. + // 11. Check all CAN devices are active. + // 12. Reset the angle on the internal encoder to the absolute encoder. + requireNonNull(mainMotor); + requireNonNull(angleMotor); + requireNonNull(encoder); + + this.wheelBase = wheelBaseMeters; + this.driveTrainWidth = driveTrainWidthMeters; + + assert mainMotor instanceof CANSparkMax || mainMotor instanceof TalonFX; + assert angleMotor instanceof CANSparkMax || angleMotor instanceof TalonFX; + assert encoder instanceof DutyCycleEncoder || encoder instanceof AnalogEncoder || encoder instanceof CANCoder || + encoder instanceof AbsoluteEncoder; + + absoluteEncoder = SwerveEncoder.fromEncoder(encoder); + assert absoluteEncoder != null; + + absoluteEncoder.factoryDefault(); + + driveMotor = SwerveMotor.fromMotor(mainMotor, + absoluteEncoder, + ModuleMotorType.DRIVE, + driveGearRatio, + wheelDiameterMeters, + 0, + drivingPowerLimit); + + turningMotor = SwerveMotor.fromMotor(angleMotor, + absoluteEncoder, + ModuleMotorType.TURNING, + steerGearRatio, + wheelDiameterMeters, + steeringMotorFreeSpeedRPM, + steeringPowerLimit); + + swerveLocation = swervePosition; + swerveModuleLocation = getSwerveModulePosition(swervePosition); + + assert driveMotor != null; + assert turningMotor != null; + + // Set the maximum speed for each swerve module for use when trying to optimize movements. + // Drive feedforward gains + // public static final double KS = 0; + // public static final double KV = 12 / MAX_SPEED; // Volt-seconds per meter (max voltage divided by max + // speed) + // public static final double KA = 12 / MAX_ACCELERATION; // Volt-seconds^2 per meter (max voltage divided + // by max accel) + maxDriveSpeedMPS = maxSpeedMPS; + driveFeedforward = new SimpleMotorFeedforward(0, 12 / maxDriveSpeedMPS, 12 / maxDriveAcceleration); + steeringKV = (12 * 60) / (steeringMotorFreeSpeedRPM * Math.toRadians(360 / steerGearRatio)); + + driveMotor.setInverted(drivingInverted); + invertedDrive = drivingInverted; + turningMotor.setInverted(steeringInverted); + invertedTurn = steeringInverted; + + absoluteEncoder.configure(); + + setAngleOffset(steeringOffsetDegrees); + resetEncoders(); + + // Convert CANCoder to read data as unsigned 0 to 360 for synchronization purposes. + absoluteEncoder.configure(); + + assert activeCAN(); + + resetEncoders(); + synchronizeSteeringEncoder(); + + driveMotor.saveConfig(); + turningMotor.saveConfig(); + + // targetAngle = getState().angle.getDegrees(); + targetAngle = 0; + + if (!remoteIntegratedEncoder() && Robot.isReal()) + { + Robot.getInstance().addPeriodic(this::synchronizeSteeringEncoder, 0.5); + } + + // Shuffleboard Data + moduleTab = Shuffleboard.getTab(SwerveModule.SwerveModuleLocationToString(swerveLocation)); + publish(Verbosity.SETUP); + + } + + /** + * Convert {@link SwerveModuleLocation} to {@link String} representation. + * + * @param swerveLocation Swerve position to convert. + * @return {@link String} name of the {@link SwerveModuleLocation} enum. + */ + public static String SwerveModuleLocationToString(SwerveModuleLocation swerveLocation) + { + switch (swerveLocation) + { + case FrontLeft: + return "Front Left Module"; + case BackLeft: + return "Back Left Module"; + case FrontRight: + return "Front Right Module"; + case BackRight: + return "Back Right Module"; + default: + return "Unknown"; + } + } + + /** + * Reset the REV encoders onboard the SparkMax's to 0, and set's the drive motor to position to 0 and synchronizes the + * internal steering encoders with the absolute encoder. + */ + public void resetEncoders() + { + driveMotor.setEncoder(0); + + if (!remoteIntegratedEncoder()) + { + turningMotor.setEncoder(0); + synchronizeSteeringEncoder(); + } + } + + /** + * Synchronizes the internal encoder for the steering motor with the value from the absolute encoder. + */ + public void synchronizeSteeringEncoder() + { + if (!remoteIntegratedEncoder()) + { + if (absoluteEncoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) + { + System.err.println("CANCoder magnetic field strength is unacceptable, will not synchronize encoders."); + return; + } + turningMotor.setEncoder(absoluteEncoder.getAbsolutePosition() - angleOffset); + } + } + + /////////////////////// END OF CONFIGURATION FUNCTIONS SECTION ////////////////////////// + + ////////////////////////////// STATUS FUNCTIONS SECTION ////////////////////////////////////////////////////// + + /** + * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID + *

+ * P = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations + * stop and angle is perfect or near perfect. + *

+ *

+ * I = 0 tune this if your PID never quite reaches the target, after tuning D. Increase this by + * P*.01 each time and adjust accordingly. + *

+ *

+ * D = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by P*10 + * and adjust accordingly. + *

+ *

+ * F = 0 tune this if the PID is being used for velocity, the F is multiplied by the target and added + * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner. + *

+ * Documentation for this is best described by CTRE here. + * + * @param p Proportional gain for closed loop. This is multiplied by closed loop error in sensor units. + * @param i Integral gain for closed loop. This is multiplied by closed loop error in sensor units every + * PID Loop. + * @param d Derivative gain for closed loop. This is multiplied by derivative error (sensor units per + * PID loop). + * @param f Feed Fwd gain for Closed loop. + * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too + * far from the target. This prevents unstable oscillation if the kI is too large. Value is in + * sensor units. + * @param moduleMotorType Swerve drive motor type. + */ + public void setPIDF(double p, double i, double d, double f, double integralZone, + ModuleMotorType moduleMotorType) + { + if ((moduleMotorType == ModuleMotorType.DRIVE)) + { + driveMotor.setPIDF(p, i, d, f, integralZone); + } else + { + turningMotor.setPIDF(p, i, d, f, integralZone); + } + } + + //////////////////////////// END OF STATUS FUNCTIONS SECTION //////////////////////////////////////////////// + + //////////////////////////// ODOMETRY AND STATE FUNCTIONS SECTION /////////////////////////////////////////// + + /** + * Configure the magnetic offset in the CANCoder. + * + * @param offset Magnetic offset in degrees. + */ + public void setAngleOffset(double offset) + { + // System.out.println(offset); + angleOffset = offset; + absoluteEncoder.setOffset(0); + } + + /** + * Check that the link is good on the swerve module and CAN bus is able to retrieve data. + * + * @return true on all devices are accessible over CAN. + */ + public boolean activeCAN() + { + boolean drive = driveMotor.reachable(), turn = turningMotor.reachable(), encoder = absoluteEncoder.reachable(); + + return drive && turn && encoder; + } + + /** + * Checks if the absolute encoder is able to be read directly by the motor controller instead of using the + * synchronizeEncoder() function periodically. + * + * @return If the absolute sensor in the steering motor is directly accessed by the motor controller. + */ + public boolean remoteIntegratedEncoder() + { + return turningMotor.remoteIntegratedEncoder(); + } + + /** + * Set the angle of the swerve module. + * + * @param angle Angle in degrees. + * @param feedforward The feedforward for the PID. + */ + public void setAngle(double angle, double feedforward) + { + turningMotor.setTarget(Math.round(angle) % 180, feedforward); + } + + /** + * Set the angle of the swerve module without feedforward. + * + * @param angle Angle in degrees. + */ + private void setAngle(double angle) + { + turningMotor.setTarget(angle, 0); + } + + /** + * Set the drive motor velocity in MPS. + * + * @param velocity Velocity in MPS. + */ + public void setVelocity(double velocity) + { + targetVelocity = velocity; + driveMotor.setTarget(velocity, driveFeedforward.calculate(velocity)); + } + + /** + * Get the module state. + * + * @return SwerveModuleState with the encoder inputs. + * @throws RuntimeException Exception if CANCoder doesnt exist + */ + public SwerveModuleState2 getState() + { + double mps = driveMotor.get(); + double angularVelocityRPS; + Rotation2d angle; + angle = Rotation2d.fromDegrees(Math.round( + Robot.isReal() ? absoluteEncoder.getAbsolutePosition() - angleOffset : targetAngle)); + angularVelocityRPS = Robot.isReal() ? Math.toRadians(absoluteEncoder.getVelocity()) : targetAngularVelocityRPS; + //^ Convert degrees per second to radians per second. + return new SwerveModuleState2(mps, angle, angularVelocityRPS); + } + + /////////////////// END OF ODOMETRY AND STATE FUNCTIONS SECTION //////////////////////////////////////// + + /////////////////// DIAGNOSTIC AND TUNING FUNCTIONS SECTION //////////////////////////////////////////// + + /** + * Set the module speed and angle based off the module state. + * + * @param state Module state. + */ + public void setState(SwerveModuleState2 state) + { + // state.angle = state.angle.minus(Rotation2d.fromDegrees(angleOffset)); + // inspired by https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L22 + Rotation2d currentAngle = getState().angle; +// SwerveModuleState optimizedState = SwerveModuleState.optimize(new SwerveModuleState(state.speedMetersPerSecond, +// state.angle), currentAngle); + SwerveModuleState2 optimizedState = SwerveModuleState2.optimize(state, currentAngle); + double angle = optimizedState.angle.getDegrees(); // getDegrees returns in the range of -180 to 180 we want 0 to + // 360. + double velocity = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0 + : + optimizedState.speedMetersPerSecond; + // turn motor code + // Prevent rotating module if speed is less then 1%. Prevents Jittering. + angle = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0 : angle; + setAngle(angle, optimizedState.angularVelocityRadPerSecond * steeringKV); + setVelocity(velocity); + targetAngle = angle; + targetAngularVelocityRPS = optimizedState.angularVelocityRadPerSecond; + } + + /** + * Get the swerve module position based off the sensors. + * + * @return Swerve Module position, with the angle as what the angle is supposed to be, not what it actually is. + */ + public SwerveModulePosition getPosition() + { + return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(targetAngle)); + } + + //////////////////////////// END OF DIAGNOSTIC AND TUNING FUNCTIONS SECTION ///////////////////////// + + /** + * Subscribe from data within smart dashboard to make changes to the swerve module. + */ + public void subscribe() + { + + } + + //////////////////////////// ENUMS SECTION ////////////////////////////////////////////////////////// + + /** + * Create a widget and add the entry to the hashmap of entries for network tables. + * + * @param name Key to display on shuffleboard. + * @param defaultValue Default value. + * @return Widget that can be modified. + */ + private SimpleWidget addEntry(String name, Object defaultValue) + { + if (NT4Entries.containsKey(name)) + { + GenericEntry entry = NT4Entries.get(name).getEntry(); + if (defaultValue instanceof Integer) + { + entry.setInteger((int) defaultValue); + } else if (defaultValue instanceof Double) + { + entry.setDouble((double) defaultValue); + } else if (defaultValue instanceof Boolean) + { + entry.setBoolean((boolean) defaultValue); + } else if (defaultValue instanceof Double[]) + { + entry.setDoubleArray((Double[]) defaultValue); + } else + { + throw new RuntimeException("Cannot publish value for " + name); + } + return NT4Entries.get(name); + } + try + { + SimpleWidget widget = moduleTab.add(name, defaultValue); + NT4Entries.put(name, widget); + return widget; + } catch (Exception e) + { + System.err.println(e); + } + throw new RuntimeException("Error creating entry"); + } + + /** + * Publish data to the smart dashboard relating to this swerve module. + * + * @param level Verbosity level, affects the CAN utilization, on HIGH it will enable the update button. + */ + public void publish(Verbosity level) + { + boolean goodLaptop = false; + switch (level) + { + case SETUP: + // PID + addEntry("drive/pid/kP", driveMotor.kP); + addEntry("drive/pid/kI", driveMotor.kI); + addEntry("drive/pid/kD", driveMotor.kD); + addEntry("drive/pid/kF", driveMotor.kF); + addEntry("drive/pid/kIZ", driveMotor.kIZ); + + addEntry("steer/pid/kP", turningMotor.kP); + addEntry("steer/pid/kI", turningMotor.kI); + addEntry("steer/pid/kD", turningMotor.kD); + addEntry("steer/pid/kF", turningMotor.kF); + addEntry("steer/pid/kIZ", turningMotor.kIZ); + + // Pretty Widget Setup + if (goodLaptop) + { + addEntry("Angle (Degrees)", + new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()}) + .withPosition(0, 1) + .withSize(4, 4) + .withProperties(Map.of("visible time", 5)) + .withWidget(BuiltInWidgets.kGraph); + addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition()) + .withPosition(0, 0) + .withSize(2, 1); + addEntry("Integrated Angle", turningMotor.get()) + .withPosition(2, 0) + .withSize(2, 1); + addEntry("Target Angle", targetAngle) + .withPosition(8, 4) + .withSize(2, 2); + + addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()}) + .withPosition(4, 1) + .withSize(4, 4) + .withProperties(Map.of("visible time", 5)) + .withWidget(BuiltInWidgets.kGraph); + addEntry("Motor Velocity", driveMotor.get()) + .withPosition(4, 0) + .withSize(4, 1); + addEntry("Target Velocity", targetVelocity) + .withPosition(8, 2) + .withSize(2, 2); + + // Inverted Motors. + addEntry("Steer Motor Inverted", invertedTurn) + .withPosition(0, 5) + .withSize(4, 1); + addEntry("Drive Motor inverted", invertedDrive) + .withPosition(4, 5) + .withSize(4, 1); + + addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value) + .withPosition(10, 2) + .withSize(2, 1); + addEntry("Drive Motor Current", driveMotor.getAmps()) + .withPosition(10, 5) + .withSize(2, 1); + + // Angle Constants + addEntry("Absolute Encoder Offset", angleOffset) + .withProperties(Map.of("min", -180, "max", 180)) + .withPosition(10, 3) + .withSize(2, 2) + .withWidget(BuiltInWidgets.kDial); + } else + { + addEntry("Target Angle", targetAngle) + .withProperties(Map.of("min", -180, "max", 180)) + .withWidget(BuiltInWidgets.kDial) + .withPosition(0, 0) + .withSize(4, 2); + addEntry("Integrated Angle", turningMotor.get()) + .withProperties(Map.of("min", 0, "max", 360)) + .withWidget(BuiltInWidgets.kDial) + .withPosition(0, 2) + .withSize(2, 2); + addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition()) + .withProperties(Map.of("min", 0, "max", 360)) + .withWidget(BuiltInWidgets.kDial) + .withPosition(2, 2) + .withSize(2, 2); + + addEntry("Target Velocity", targetVelocity) + .withPosition(4, 0) + .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS)) + .withSize(4, 2) + .withWidget(BuiltInWidgets.kNumberBar); + addEntry("Motor Velocity", driveMotor.get()) + .withPosition(4, 2) + .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS)) + .withSize(4, 2) + .withWidget(BuiltInWidgets.kNumberBar); + + // Inverted Motors. + addEntry("Steer Motor Inverted", invertedTurn) + .withPosition(0, 4) + .withSize(4, 1); + addEntry("Drive Motor inverted", invertedDrive) + .withPosition(4, 4) + .withSize(4, 1); + + addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value) + .withPosition(8, 2) + .withSize(2, 1); + + addEntry("Drive Motor Current", driveMotor.getAmps()) + .withPosition(10, 2) + .withSize(2, 1); + + // Angle Constants + addEntry("Absolute Encoder Offset", angleOffset) + .withProperties(Map.of("min", -180, "max", 180)) + .withPosition(8, 3) + .withSize(2, 2) + .withWidget(BuiltInWidgets.kDial); + } + + addEntry("CAN Connection", activeCAN()) + .withPosition(8, 0) + .withSize(4, 2); + + case HIGH: + // Update if button is set. + // The higher the better, 2 and 3 are what we want. + addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value); + case NORMAL: + // Steering Encoder Values + if (goodLaptop) + { + addEntry("Angle (Degrees)", + new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()}); + } + addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition()); + addEntry("Integrated Angle", turningMotor.get()); + + // Driving Encoder Values + if (goodLaptop) + { + addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()}); + } + addEntry("Motor Velocity", driveMotor.get()); + addEntry("Drive Motor Current", driveMotor.getAmps()); + + // CAN Bus is accessible + addEntry("CAN Connection", activeCAN()); + + case LOW: + // PID + addEntry("Target Velocity", targetVelocity); + addEntry("Target Angle", targetAngle); + + } + } + + /** + * Initializes this {@link Sendable} object. + * + * @param builder sendable builder + */ + @Override + public void initSendable(SendableBuilder builder) + { + builder.setSmartDashboardType(SwerveModuleLocationToString(swerveLocation) + " SwerveDriveModule"); + builder.setActuator(true); + builder.setSafeState(this::stopMotor); + } + + //////////////////////////////////// END OF ENUMS SECTION ////////////////////////////////////////////// + + ////////////////////////////////// OVERRIDES SECTION /////////////////////////////////////////////////// + + /** + * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects + * managed by the {@code try}-with-resources statement. + * + *

While this interface method is declared to throw {@code + * Exception}, implementers are strongly encouraged to declare concrete implementations of the {@code close} + * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail. + * + *

Cases where the close operation may fail require careful + * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally + * mark the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be + * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it + * reduces problems that could arise when the resource wraps, or is wrapped, by another resource. + * + *

Implementers of this interface are also strongly advised + * to not have the {@code close} method throw {@link InterruptedException}. + *

+ * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an + * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}. + *

+ * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close} + * method should not throw it. + * + *

Note that unlike the {@link Closeable#close close} + * method of {@link Closeable}, this {@code close} method is not required to be idempotent. In other words, + * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close} + * which is required to have no effect if called more than once. + *

+ * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent. + */ + @Override + public void close() + { + SendableRegistry.remove(this); + } + + /** + * Disable the motor controller. + */ + @Override + public void disable() + { + stopMotor(); + } + + /** + * Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor. + */ + @Override + public void stopMotor() + { +// turningMotor.stop(); + driveMotor.stop(); + } + + /** + * Common interface for setting the speed of a motor controller. + * + * @param speed The speed to set. Value should be between -1.0 and 1.0. + */ + @Override + public void set(double speed) + { + drivePower = speed; + driveMotor.set(speed); + } + + /** + * Common interface for getting the current set speed of a motor controller. + * + * @return The current set speed. Value is between -1.0 and 1.0. + */ + @Override + public double get() + { + return drivePower; + } + + /** + * Get the swerve module position in {@link Translation2d} from the enum passed. + * + * @param swerveLocation Swerve module location enum. + * @return Location as {@link Translation2d}. + * @throws RuntimeException If Enum value is not defined. + */ + private Translation2d getSwerveModulePosition(SwerveModuleLocation swerveLocation) + { + // Modeling off of https://github.com/Stampede3630/2022-Code/blob/master/src/main/java/frc/robot/SwerveDrive.java + switch (swerveLocation) + { + case FrontLeft: + return new Translation2d(wheelBase / 2, driveTrainWidth / 2); + case BackLeft: + return new Translation2d(-wheelBase / 2, driveTrainWidth / 2); + case FrontRight: + return new Translation2d(wheelBase / 2, -driveTrainWidth / 2); + case BackRight: + return new Translation2d(-wheelBase / 2, -driveTrainWidth / 2); + default: + throw new RuntimeException("Invalid location given"); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + public void setVoltageCompensation(double nominalVoltage) + { + driveMotor.setVoltageCompensation(nominalVoltage); + turningMotor.setVoltageCompensation(nominalVoltage); + } + + //////////////////////////////////// END OF OVERRIDES SECTION //////////////////////////////////////////// + + //////////////////////////////////// STATIC FUNCTIONS SECTION //////////////////////////////////////////// + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + * @param type Swerve Drive Motor type to configure. + */ + public void setCurrentLimit(int currentLimit, ModuleMotorType type) + { + if (type == ModuleMotorType.DRIVE) + { + driveMotor.setCurrentLimit(currentLimit); + } else + { + turningMotor.setCurrentLimit(currentLimit); + } + } + + /////////////////////////////////// END OF STATIC FUNCTIONS SECTION ////////////////////////////////////////// + + /////////////////////////////////// EXTRA FUNCTIONS THAT PROBABLY WONT MATTER //////////////////////////////// + + /** + * Set the steering motor to be inverted. + * + * @param isInverted The state of inversion, true is inverted. + */ + public void setInvertedTurn(boolean isInverted) + { + invertedTurn = isInverted; + turningMotor.setInverted(isInverted); + } + + /** + * Invert the absolute encoder. + * + * @param isInverted The state of inversion, true is inverted. + */ + public void setInvertedAbsoluteEncoder(boolean isInverted) + { + absoluteEncoder.setInverted(isInverted); + } + + ////////////// CUSTOM INVERSION FUNCTIONS SECTION ////////////////////////// + + /** + * Common interface for returning if a motor controller is in the inverted state or not. + * + * @return isInverted The state of the inversion true is inverted. + */ + @Override + public boolean getInverted() + { + return invertedDrive; + } + + /** + * Common interface for inverting direction of a motor controller. + * + * @param isInverted The state of inversion, true is inverted. + */ + @Override + public void setInverted(boolean isInverted) + { + invertedDrive = isInverted; + driveMotor.setInverted(isInverted); + } + + /** + * Swerve Module location on the robot. + */ + public enum SwerveModuleLocation + { + FrontLeft, + BackLeft, + FrontRight, + BackRight + } + + /** + * Verbosity levels for data publishing, + */ + public enum Verbosity + { + /** + * The bare minimum and not utilize the CAN bus when reporting data. Only posts data from attributes. + */ + LOW, + /** + * Utilize the CAN bus minimally. + */ + NORMAL, + /** + * Extensively use the CAN bus to fetch data and report back. + */ + HIGH, + /** + * Creates every field for the module. + */ + SETUP + } + + ////////////////////////////// END OF CUSTOM INVERSION FUNCTIONS SECTION ///////////////////////////////////// + +} \ No newline at end of file diff --git a/swervelib/SwerveMotor.java b/swervelib/SwerveMotor.java new file mode 100644 index 0000000..4d68eab --- /dev/null +++ b/swervelib/SwerveMotor.java @@ -0,0 +1,204 @@ +package frc.robot.subsystems.swervedrive.swerve; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import frc.robot.subsystems.swervedrive.swerve.motors.CTRESwerveMotor; +import frc.robot.subsystems.swervedrive.swerve.motors.REVSwerveMotor; + +public abstract class SwerveMotor +{ + + /** + * Module motor type. + */ + public ModuleMotorType m_motorType; + /** + * PIDF Values. + */ + public double kP, kI, kD, kF, kIZ; + /** + * Target value for PID. + */ + public double target; + + /** + * Constructor for Swerve Motor, expecting CANSparkMax or TalonFX. Clears sticky faults and restores factory + * defaults. + * + * @param motor Motor controller. + * @param absoluteEncoder The absolute encoder used for the module, if the motor is a turning motor and the encoder is + * compatible it will set the encoder as the remote integrated encoder and does not need + * periodic synchronization. + * @param type Swerve module motor type. + * @param gearRatio Gear ratio. + * @param wheelDiameter Wheel diameter in meters. + * @param freeSpeedRPM Free speed RPM of the motor. + * @param powerLimit Power limit for the motor. + * @param Motor type to use. + */ + public static SwerveMotor fromMotor(MotorType motor, + SwerveEncoder absoluteEncoder, + ModuleMotorType type, + double gearRatio, + double wheelDiameter, + double freeSpeedRPM, double powerLimit) + { + if (motor instanceof CANSparkMax) + { + return new REVSwerveMotor(((CANSparkMax) motor), + absoluteEncoder, + type, + gearRatio, + wheelDiameter, + freeSpeedRPM, + powerLimit); + } + if (motor instanceof TalonFX) + { + return new CTRESwerveMotor((TalonFX) motor, + absoluteEncoder, + type, + gearRatio, + wheelDiameter, + freeSpeedRPM, + powerLimit); + } + return null; + } + + + /** + * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base. + * + * @param min Minimum output. + * @param max Maximum output. + */ + public abstract void setPIDOutputRange(double min, double max); + + /** + * Set the PIDF coefficients for the closed loop PID onboard the SparkMax. + * + * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units. + * Default is 1.0 + * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every + * PID Loop. + * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID + * loop). Default is 0.1 + * @param F Feed Fwd gain for Closed loop. + * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far + * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor + * units. + */ + public abstract void setPIDF(double P, double I, double D, double F, double integralZone); + + /** + * Configures the conversion factor based upon which motor. + * + * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning + * motor. + */ + public abstract void setConversionFactor(double conversionFactor); + + /** + * Set the target for the PID loop. + * + * @param target The PID target to aim for. + * @param feedforward The feedforward for this target. + */ + public abstract void setTarget(double target, double feedforward); + + /** + * Stop the motor by sending 0 volts to it. + */ + public abstract void stop(); + + /** + * Set the speed of the drive motor from -1 to 1. + * + * @param speed Speed from -1 to 1. + */ + public abstract void set(double speed); + + /** + * Get the current value of the encoder corresponding to the PID. + * + * @return Current value of the encoder either in velocity or position.. + */ + public abstract double get(); + + /** + * Get the current output. + * + * @return Output amps. + */ + public abstract double getAmps(); + + /** + * Get the current value of the encoder. + * + * @return Current value of the encoder. + */ + public abstract double getPosition(); + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + public abstract void setVoltageCompensation(double nominalVoltage); + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + public abstract void setCurrentLimit(int currentLimit); + + /** + * Set the encoder value. + * + * @param value Value to set the encoder to. + */ + public abstract void setEncoder(double value); + + /** + * Check that the link is good on the swerve module and CAN bus is able to retrieve data. + * + * @return true on all devices are accessible over CAN. + */ + public abstract boolean reachable(); + + /** + * Check if the absolute encoder is used inplace of the integrated encoder. + * + * @return true, if the absolute encoder is being used as the integrated controller. + */ + public abstract boolean remoteIntegratedEncoder(); + + /** + * Optimize the CAN status frames to reduce utilization. + */ + public abstract void optimizeCANFrames(); + + /** + * Save configuration data to the motor controller so it is persistent on reboot. + */ + public abstract void saveConfig(); + + /** + * Invert the motor. + * + * @param inverted Set the motor as inverted. + */ + public abstract void setInverted(boolean inverted); + + /** + * Motor type for the swerve drive module + */ + public enum ModuleMotorType + { + DRIVE, TURNING + } +} diff --git a/swervelib/SwerveParser.java b/swervelib/SwerveParser.java new file mode 100644 index 0000000..baf1594 --- /dev/null +++ b/swervelib/SwerveParser.java @@ -0,0 +1,303 @@ +package frc.robot.subsystems.swervedrive.swerve; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation; +import java.io.File; +import java.io.IOException; + +/** + * Swerve Drive JSON parser. + */ +public class SwerveParser +{ + + /** + * Open JSON file. + * + * @param file JSON File to open. + * @return JsonNode of file. + */ + private static JsonNode openJson(File file) + { + try + { + return new ObjectMapper().readTree(file); + } catch (IOException e) + { + throw new RuntimeException(e); + } + } + + /** + * Check directory structure. + * + * @param directory JSON Configuration Directory + */ + private static void checkDirectory(File directory) + { + assert new File(directory, "swerve.json").exists(); + assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); + assert new File(directory, "modules/back").exists() && new File(directory, "modules/back").isDirectory(); + assert new File(directory, "modules/front").exists() && new File(directory, "modules/front").isDirectory(); + assert new File(directory, "modules/back/left.json").exists(); + assert new File(directory, "modules/back/right.json").exists(); + assert new File(directory, "modules/front/left.json").exists(); + assert new File(directory, "modules/front/right.json").exists(); + } + + /** + * Create the motor controller based off the config. + * + * @param motorConfig JSON Motor config object. + * @return Motor Controller + */ + private static MotorController createMotor(JsonNode motorConfig) + { + int motorID = motorConfig.get("ID").asInt(); + switch (motorConfig.get("Type").asText()) + { + case "Neo": + return new CANSparkMax(motorID, MotorType.kBrushless); + case "Falcon": + return motorConfig.has("CANBus") ? new WPI_TalonFX(motorID, motorConfig.get("CANBus").asText()) + : new WPI_TalonFX(motorID); + } + return null; + } + + /** + * Create SwerveEncoder from JSON config. + * + * @param encoderConfig Swerve Encoder JSON config. + * @return SwerveEncoder + */ + private static Object createEncoder(JsonNode encoderConfig) + { + int encoderID = encoderConfig.get("ID").asInt(); + switch (encoderConfig.get("Type").asText()) + { + case "CANCoder": + return encoderConfig.has("CANBus") ? new CANCoder(encoderID, encoderConfig.get("CANBus").asText()) + : new CANCoder(encoderID); + case "ThriftyBot": + case "AnalogEncoder": + return new AnalogEncoder(encoderID); + case "SRXMagEncoder": + case "REVThroughBore": + return new DutyCycleEncoder(encoderID); // This probably doesn't work + } + return null; + } + + /** + * Create the gyro object based off the configuration. + * + * @param gyroConfig Gyro configuration. + * @return Gyro object. + */ + private static WPI_Pigeon2 createGyro(JsonNode gyroConfig) + { + int gyroID = gyroConfig.get("ID").asInt(); + + if (gyroConfig.get("Type").asText().equals("Pigeon2")) + { + return gyroConfig.has("CANBus") ? new WPI_Pigeon2(gyroID, gyroConfig.get("CANBus").asText()) + : new WPI_Pigeon2(gyroID); +// case "Pigeon": +// return new WPI_PigeonIMU(gyroID); + } + return null; + } + + /** + * Check module JSON config for existing values. + * + * @param moduleJson Module JSON object. + */ + private static void checkModule(JsonNode moduleJson) + { + assert moduleJson.has("Motor"); + assert moduleJson.get("Motor").has("Drive"); + assert moduleJson.get("Motor").get("Drive").has("Type") && moduleJson.get("Motor").get("Drive").has("ID"); + assert moduleJson.get("Motor").has("Steer"); + assert moduleJson.get("Motor").get("Steer").has("Type") && moduleJson.get("Motor").get("Steer").has("ID") && + moduleJson.get("Motor").get("Steer").has("FreeSpeedRPM"); + assert moduleJson.has("AbsoluteEncoder"); + assert moduleJson.get("AbsoluteEncoder").has("Type") && + moduleJson.get("AbsoluteEncoder").has("ID") && + moduleJson.get("AbsoluteEncoder").has("Offset"); + } + + /** + * Check the main json for correct attributes. + * + * @param mainJson Main JSON. + */ + private static void checkMain(JsonNode mainJson) + { + assert mainJson.has("WheelBase"); + assert mainJson.has("TrackWidth"); + assert mainJson.has("WheelDiameter"); + assert mainJson.has("Speed"); + assert mainJson.get("Speed").has("MetersPerSecond") && mainJson.get("Speed").has("RadianPerSecond") && + mainJson.get("Speed").has("PhysicalMetersPerSecond"); + assert mainJson.has("Acceleration"); + assert mainJson.get("Acceleration").has("MetersPerSecond") && mainJson.get("Acceleration").has("RadianPerSecond"); + assert mainJson.has("Drive"); + assert mainJson.get("Drive").has("Inverted") && mainJson.get("Drive").has("GearRatio") && + mainJson.get("Drive").has("MaxPower"); + assert mainJson.has("Steer"); + assert mainJson.get("Steer").has("Inverted") && mainJson.get("Steer").has("GearRatio") && + mainJson.get("Steer").has("MaxPower"); + assert mainJson.has("Gyro"); + assert mainJson.get("Gyro").has("Inverted") && mainJson.get("Gyro").has("Type") && mainJson.get("Gyro").has("ID"); + assert mainJson.has("Initial Pose"); + assert mainJson.get("Initial Pose").has("X") && mainJson.get("Initial Pose").has("Y") && + mainJson.get("Initial Pose").has("Rotation"); + } + + /** + * Create SwerveModule from JSON configuration file. + * + * @param mainJson Main JSON to pull from. + * @param moduleFile Module specific JSON data to pull from. + * @param moduleLocation Swerve module location that is being created. + * @return SwerveModule + */ + private static SwerveModule createModule(JsonNode mainJson, File moduleFile, + SwerveModuleLocation moduleLocation) + { + JsonNode moduleJson = openJson(moduleFile); + checkModule(moduleJson); + MotorController driveMotor = createMotor(moduleJson.get("Motor").get("Drive")), + steerMotor = createMotor(moduleJson.get("Motor").get("Steer")); + Object encoder = createEncoder(moduleJson.get("AbsoluteEncoder")); + + SwerveModule module = new SwerveModule<>(driveMotor, + steerMotor, + encoder, + moduleLocation, + mainJson.get("Drive").get("GearRatio").asDouble(), + mainJson.get("Steer").get("GearRatio").asDouble(), + moduleJson.get("AbsoluteEncoder").get("Offset").asDouble(), + Units.inchesToMeters(mainJson.get("WheelDiameter").asDouble()), + Units.inchesToMeters(mainJson.get("WheelBase").asDouble()), + Units.inchesToMeters(mainJson.get("TrackWidth").asDouble()), + moduleJson.get("Motor").get("Steer").get("FreeSpeedRPM") + .asDouble(), + mainJson.get("Speed").get("MetersPerSecond").asDouble(), + mainJson.get("Acceleration").get("MetersPerSecond").asDouble(), + mainJson.get("Drive").get("MaxPower").asDouble(), + mainJson.get("Steer").get("MaxPower").asDouble(), + mainJson.get("Steer").get("Inverted").asBoolean(), + mainJson.get("Drive").get("Inverted").asBoolean()); + + if (moduleJson.get("Motor").get("Steer").has("PID")) + { + JsonNode jsonPIDF = moduleJson.get("Motor").get("Steer").get("PID"); + setModulePIDF(module.turningMotor, jsonPIDF); + } else if (mainJson.get("Steer").has("PID")) + { + JsonNode jsonPIDF = mainJson.get("Steer").get("PID"); + setModulePIDF(module.turningMotor, jsonPIDF); + } + + if (moduleJson.get("Motor").get("Drive").has("PID")) + { + JsonNode jsonPIDF = moduleJson.get("Motor").get("Drive").get("PID"); + setModulePIDF(module.driveMotor, jsonPIDF); + } else if (mainJson.get("Steer").has("PID")) + { + JsonNode jsonPIDF = mainJson.get("Drive").get("PID"); + setModulePIDF(module.turningMotor, jsonPIDF); + } + + if (moduleJson.get("Motor").get("Steer").has("CurrentLimit")) + { + module.turningMotor.setCurrentLimit(moduleJson.get("Motor").get("Steer").get("CurrentLimit") + .asInt()); + } else if (mainJson.get("Steer").has("CurrentLimit")) + { + module.turningMotor.setCurrentLimit(mainJson.get("Steer").get("CurrentLimit").asInt()); + } + + if (moduleJson.get("Motor").get("Drive").has("CurrentLimit")) + { + module.driveMotor.setCurrentLimit(moduleJson.get("Motor").get("Drive").get("CurrentLimit") + .asInt()); + } else if (mainJson.get("Drive").has("CurrentLimit")) + { + module.turningMotor.setCurrentLimit(mainJson.get("Drive").get("CurrentLimit").asInt()); + } + + return module; + } + + /** + * Set the PIDF for the module if the given JSON has the paramters. + * + * @param motor Motor to configure. + * @param jsonPIDF JSON ndoe with values. + */ + private static void setModulePIDF(SwerveMotor motor, JsonNode jsonPIDF) + { + if (jsonPIDF.has("P") && jsonPIDF.has("I") && jsonPIDF.has("D") && + jsonPIDF.has("F") && jsonPIDF.has("IntegralZone")) + { + motor.setPIDF(jsonPIDF.get("P").asDouble(), + jsonPIDF.get("I").asDouble(), + jsonPIDF.get("D").asDouble(), + jsonPIDF.get("F").asDouble(), + jsonPIDF.get("IntegralZone").asDouble()); + } + } + + /** + * Create SwerveDrive from JSON configuration directory. + * + * @param directory JSON Configuration directory. + * @return SwerveDrive + */ + public static SwerveDrive fromJSONDirectory(File directory) + { + checkDirectory(directory); + JsonNode swerveJson = openJson(new File(directory, "swerve.json")); + checkMain(swerveJson); + return new SwerveDrive(createModule(swerveJson, + new File(directory, "modules/front/left.json"), + SwerveModuleLocation.FrontLeft), + createModule(swerveJson, + new File(directory, "modules/front/right.json"), + SwerveModuleLocation.FrontRight), + createModule(swerveJson, + new File(directory, "modules/back/left.json"), + SwerveModuleLocation.BackLeft), + createModule(swerveJson, + new File(directory, "modules/back/right.json"), + SwerveModuleLocation.BackRight), + createGyro(swerveJson.get("Gyro")), + swerveJson.get("Speed").get("MetersPerSecond").asDouble(), + swerveJson.get("Speed").get("RadianPerSecond").asDouble() * Math.PI, + swerveJson.get("Acceleration").get("MetersPerSecond").asDouble(), + swerveJson.get("Acceleration").get("RadianPerSecond").asDouble() * Math.PI, + swerveJson.get("Speed").get("PhysicalMetersPerSecond").asDouble(), + swerveJson.get("Gyro").get("Inverted").asBoolean(), + new Pose2d(Units.inchesToMeters(swerveJson.get("Initial Pose").get("X").asDouble()), + Units.inchesToMeters(swerveJson.get("Initial Pose").get("Y").asDouble()), + Rotation2d.fromDegrees(swerveJson.get("Initial Pose").get("Rotation") + .asDouble()))); + + } +} diff --git a/swervelib/encoders/CTRECANCoder.java b/swervelib/encoders/CTRECANCoder.java new file mode 100644 index 0000000..7f34fd6 --- /dev/null +++ b/swervelib/encoders/CTRECANCoder.java @@ -0,0 +1,111 @@ +package frc.robot.subsystems.swervedrive.swerve.encoders; + +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix.sensors.SensorTimeBase; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; + +public class CTRECANCoder extends SwerveEncoder +{ + + /** + * Create SwerveEncoder based off CANCoder. + * + * @param encoder CANCoder to use. + */ + public CTRECANCoder(CANCoder encoder) + { + m_encoder = encoder; + } + + /** + * Configure the absolute encoder if possible. + */ + public void configure() + { + CANCoderConfiguration sensorConfig = new CANCoderConfiguration(); + + sensorConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; + sensorConfig.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition; + sensorConfig.sensorTimeBase = SensorTimeBase.PerSecond; + m_encoder.configAllSettings(sensorConfig); + } + + /** + * Get the magnetic field strength, if available. + * + * @return CTRE MagneticFieldStrength Enum. + */ + @Override + public MagnetFieldStrength getMagnetFieldStrength() + { + return m_encoder.getMagnetFieldStrength(); + } + + /** + * Get the absolute position in degrees. + * + * @return Absolute position (0, 360] + */ + @Override + public double getAbsolutePosition() + { + return m_encoder.getAbsolutePosition(); + } + + /** + * Get the velocity of the absolute encoder in degrees per second. + * + * @return Velocity in degrees per second. + */ + @Override + public double getVelocity() + { + return m_encoder.getVelocity(); + } + + /** + * Configure the magnetic offset for the AbsoluteEncoder. + * + * @param offset Offset in degrees. + */ + @Override + public void setOffset(double offset) + { + m_encoder.configMagnetOffset(offset); + } + + /** + * Is the encoder reachable? + * + * @return True if reachable, false otherwise. + */ + @Override + public boolean reachable() + { + return m_encoder.getFirmwareVersion() > 0; + } + + /** + * Configure the sensor direction + * + * @param isInverted Inverted or not. + */ + @Override + public void setInverted(boolean isInverted) + { + m_encoder.configSensorDirection(isInverted); + } + + /** + * Reset encoder to factory default settings, if possible. + */ + @Override + public void factoryDefault() + { + m_encoder.configFactoryDefault(); + } +} diff --git a/swervelib/encoders/PWMAnalogEncoder.java b/swervelib/encoders/PWMAnalogEncoder.java new file mode 100644 index 0000000..4d4dc89 --- /dev/null +++ b/swervelib/encoders/PWMAnalogEncoder.java @@ -0,0 +1,128 @@ +package frc.robot.subsystems.swervedrive.swerve.encoders; + +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Robot; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; + +public class PWMAnalogEncoder extends SwerveEncoder +{ + + private final Timer m_timer; + private boolean m_inverted = false; + private double m_lastAngle, m_velocity, m_lastTime; + + /** + * Constructor for analog encoder Swerve Encoder. + * + * @param encoder AnalogEncoder. + */ + public PWMAnalogEncoder(AnalogEncoder encoder) + { + m_encoder = encoder; + m_timer = new Timer(); + m_timer.start(); + configure(); + + m_lastTime = m_timer.get(); + m_lastAngle = m_encoder.getDistance(); + Robot.getInstance().addPeriodic(this::updateVelocity, 1); + } + + /** + * Updates the velocity every second. + */ + public void updateVelocity() + { + double currentTime = m_timer.get(), currentAngle = m_encoder.getDistance(); + m_velocity = (currentAngle - m_lastAngle) / (currentTime - m_lastTime); + m_lastTime = currentTime; + m_lastAngle = currentAngle; + } + + /** + * Configure the absolute encoder if possible. + */ + @Override + public void configure() + { + m_encoder.setDistancePerRotation(360); + } + + /** + * Reset encoder to factory default settings, if possible. + */ + @Override + public void factoryDefault() + { + m_encoder.reset(); + } + + /** + * Get the magnetic field strength, if available. + * + * @return CTRE MagneticFieldStrength Enum. + */ + @Override + public MagnetFieldStrength getMagnetFieldStrength() + { + return MagnetFieldStrength.Good_GreenLED; + } + + /** + * Get the absolute position in degrees. + * + * @return Absolute position (0, 360] + */ + @Override + public double getAbsolutePosition() + { + return m_encoder.getDistance() * (m_inverted ? -1 : 1); + } + + /** + * Get the velocity of the absolute encoder in degrees per second. + * + * @return Velocity in degrees per second. + */ + @Override + public double getVelocity() + { + return m_velocity; + } + + /** + * Configure the magnetic offset for the AbsoluteEncoder. + * + * @param offset Offset in degrees. + */ + @Override + public void setOffset(double offset) + { + m_encoder.setPositionOffset((Rotation2d.fromDegrees(offset).getDegrees() + 180) / 360); + } + + /** + * Is the encoder reachable? + * + * @return True if reachable, false otherwise. + */ + @Override + public boolean reachable() + { + return true; + } + + /** + * Configure the sensor direction + * + * @param isInverted Inverted or not. + */ + @Override + public void setInverted(boolean isInverted) + { + m_inverted = isInverted; + } +} diff --git a/swervelib/encoders/PWMDutyCycleEncoder.java b/swervelib/encoders/PWMDutyCycleEncoder.java new file mode 100644 index 0000000..2a3291b --- /dev/null +++ b/swervelib/encoders/PWMDutyCycleEncoder.java @@ -0,0 +1,129 @@ +package frc.robot.subsystems.swervedrive.swerve.encoders; + +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Robot; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; + +public class PWMDutyCycleEncoder extends SwerveEncoder +{ + + private final Timer m_timer; + private boolean m_inverted = false; + private double m_lastAngle, m_velocity, m_lastTime; + + /** + * Construct PWM DutyCycleEncoder class. + * + * @param encoder Encoder class. + */ + public PWMDutyCycleEncoder(DutyCycleEncoder encoder) + { + m_timer = new Timer(); + m_timer.start(); + + m_encoder = encoder; + configure(); + + m_lastTime = m_timer.get(); + m_lastAngle = m_encoder.getDistance(); + Robot.getInstance().addPeriodic(this::updateVelocity, 1); + } + + /** + * Updates the velocity every second. + */ + public void updateVelocity() + { + double currentTime = m_timer.get(), currentAngle = m_encoder.getDistance(); + m_velocity = (currentAngle - m_lastAngle) / (currentTime - m_lastTime); + m_lastTime = currentTime; + m_lastAngle = currentAngle; + } + + /** + * Configure the absolute encoder if possible. + */ + @Override + public void configure() + { + m_encoder.setDistancePerRotation(360); + } + + /** + * Reset encoder to factory default settings, if possible. + */ + @Override + public void factoryDefault() + { + m_encoder.reset(); + } + + /** + * Get the magnetic field strength, if available. + * + * @return CTRE MagneticFieldStrength Enum. + */ + @Override + public MagnetFieldStrength getMagnetFieldStrength() + { + return MagnetFieldStrength.Good_GreenLED; + } + + /** + * Get the absolute position in degrees. + * + * @return Absolute position (0, 360] + */ + @Override + public double getAbsolutePosition() + { + return m_encoder.getDistance() * (m_inverted ? -1 : 1); + } + + /** + * Get the velocity of the absolute encoder in degrees per second. + * + * @return Velocity in degrees per second. + */ + @Override + public double getVelocity() + { + return m_velocity; + } + + /** + * Configure the magnetic offset for the AbsoluteEncoder. + * + * @param offset Offset in degrees. + */ + @Override + public void setOffset(double offset) + { + m_encoder.setPositionOffset((Rotation2d.fromDegrees(offset).getDegrees() + 180) / 360); + } + + /** + * Is the encoder reachable? + * + * @return True if reachable, false otherwise. + */ + @Override + public boolean reachable() + { + return true; + } + + /** + * Configure the sensor direction + * + * @param isInverted Inverted or not. + */ + @Override + public void setInverted(boolean isInverted) + { + m_inverted = isInverted; + } +} diff --git a/swervelib/encoders/REVAbsoluteEncoder.java b/swervelib/encoders/REVAbsoluteEncoder.java new file mode 100644 index 0000000..39536d9 --- /dev/null +++ b/swervelib/encoders/REVAbsoluteEncoder.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems.swervedrive.swerve.encoders; + +import com.ctre.phoenix.sensors.MagnetFieldStrength; +import com.revrobotics.AbsoluteEncoder; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; + +public class REVAbsoluteEncoder extends SwerveEncoder +{ + + /** + * Constructor for AbsoluteEncoder class. + * + * @param encoder Encoder to wrap around. + */ + public REVAbsoluteEncoder(AbsoluteEncoder encoder) + { + m_encoder = encoder; + configure(); + } + + /** + * Configure the absolute encoder if possible. + */ + @Override + public void configure() + { + // Redundant, but ensures everything is always correct. + m_encoder.setPositionConversionFactor(360); + m_encoder.setVelocityConversionFactor(360.0 / 60.0); + } + + /** + * Reset encoder to factory default settings, if possible. + */ + @Override + public void factoryDefault() + { + // Nothing to do here. + } + + /** + * Get the magnetic field strength, if available. + * + * @return CTRE MagneticFieldStrength Enum. + */ + @Override + public MagnetFieldStrength getMagnetFieldStrength() + { + // Unable to check, assume green. + return MagnetFieldStrength.Good_GreenLED; + } + + /** + * Get the absolute position in degrees. + * + * @return Absolute position (0, 360] + */ + @Override + public double getAbsolutePosition() + { + return m_encoder.getPosition(); + } + + /** + * Get the velocity of the absolute encoder in degrees per second. + * + * @return Velocity in degrees per second. + */ + @Override + public double getVelocity() + { + return m_encoder.getVelocity(); + } + + /** + * Configure the magnetic offset for the AbsoluteEncoder. + * + * @param offset Offset in degrees. + */ + @Override + public void setOffset(double offset) + { + m_encoder.setZeroOffset(offset); + } + + /** + * Is the encoder reachable? + * + * @return True if reachable, false otherwise. + */ + @Override + public boolean reachable() + { + return true; + } + + /** + * Configure the sensor direction. + * + * @param isInverted Inverted or not. + */ + @Override + public void setInverted(boolean isInverted) + { + m_encoder.setInverted(isInverted); + } +} diff --git a/swervelib/kinematics/SwerveDriveKinematics2.java b/swervelib/kinematics/SwerveDriveKinematics2.java new file mode 100644 index 0000000..96dd1a3 --- /dev/null +++ b/swervelib/kinematics/SwerveDriveKinematics2.java @@ -0,0 +1,353 @@ +package frc.robot.subsystems.swervedrive.swerve.kinematics; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.util.Arrays; +import java.util.Collections; +import org.ejml.simple.SimpleMatrix; + +// Taken from https://github.com/FRC2539/javabot-2023/tree/main/src/main/java/frc/lib/swerve +// and https://github.com/first95/FRC2022/tree/feature/secondOrderKinematics/SwervyBot/src/main/java/frc/lib + +/** + * Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis + * speed. + *

Taken from https://github.com/FRC2539/javabot-2023/tree/main/src/main/java/frc/lib/swerve

+ *

https://github.com/first95/FRC2022/tree/feature/secondOrderKinematics/SwervyBot/src/main/java/frc/lib

+ * Makes use of {@link SwerveDriveKinematics2} to add the angular velocity that is required of the module as an output. + */ +public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.SwerveDriveKinematics +{ + + private final SimpleMatrix m_inverseKinematics; + private final SimpleMatrix m_forwardKinematics; + private final SimpleMatrix bigInverseKinematics; + + private final int m_numModules; + private final Translation2d[] m_modules; + private final SwerveModuleState2[] m_moduleStates; + private Translation2d m_prevCoR = new Translation2d(); + + /** + * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as + * {@link Translation2d}s. The order in which you pass in the wheel locations is the same order that you will receive + * the module states when performing inverse kinematics. It is also expected that you pass in the module states in the + * same order when calling the forward kinematics methods. + * + * @param wheelsMeters The locations of the wheels relative to the physical center of the robot. + */ + public SwerveDriveKinematics2(Translation2d... wheelsMeters) + { + super(wheelsMeters); + if (wheelsMeters.length < 2) + { + throw new IllegalArgumentException("A swerve drive requires at least two modules"); + } + m_numModules = wheelsMeters.length; + m_modules = Arrays.copyOf(wheelsMeters, m_numModules); + m_moduleStates = new SwerveModuleState2[m_numModules]; + Arrays.fill(m_moduleStates, new SwerveModuleState2()); + m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + bigInverseKinematics = new SimpleMatrix(m_numModules * 2, 4); + + for (int i = 0; i < m_numModules; i++) + { + m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); + m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); + bigInverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY()); + bigInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX()); + } + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + + MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1); + } + + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the + * wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while + * maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach. + */ + public static void desaturateWheelSpeeds( + SwerveModuleState2[] moduleStates, double attainableMaxSpeedMetersPerSecond) + { + double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond; + if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) + { + for (SwerveModuleState2 moduleState : moduleStates) + { + moduleState.speedMetersPerSecond = + moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + } + } + } + + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of + * joystick saturation at edges of joystick. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the + * wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while + * maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be + * mutated with the normalized speeds! + * @param currentChassisSpeed The current speed of the robot + * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach + * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while + * translating + * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating + */ + public static void desaturateWheelSpeeds( + SwerveModuleState2[] moduleStates, + ChassisSpeeds currentChassisSpeed, + double attainableMaxModuleSpeedMetersPerSecond, + double attainableMaxTranslationalSpeedMetersPerSecond, + double attainableMaxRotationalVelocityRadiansPerSecond) + { + double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond; + + if (attainableMaxTranslationalSpeedMetersPerSecond == 0 + || attainableMaxRotationalVelocityRadiansPerSecond == 0 + || realMaxSpeed == 0) + { + return; + } + double translationalK = + Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond) + / attainableMaxTranslationalSpeedMetersPerSecond; + double rotationalK = + Math.abs(currentChassisSpeed.omegaRadiansPerSecond) + / attainableMaxRotationalVelocityRadiansPerSecond; + double k = Math.max(translationalK, rotationalK); + double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1); + for (SwerveModuleState2 moduleState : moduleStates) + { + moduleState.speedMetersPerSecond *= scale; + } + } + + /** + * Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used + * to convert joystick values into module speeds and angles. + * + *

This function also supports variable centers of rotation. During normal operations, the + * center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to + * that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or + * for any other use case, you can do so. + * + *

In the case that the desired chassis speeds are zero (i.e. the robot will be stationary), + * the previously calculated module angle will be maintained. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one corner + * of the robot and provide a chassis speed that only has a dtheta component, the robot + * will rotate around that corner. + * @return An array containing the module states. Use caution because these module states are not normalized. + * Sometimes, a user input may cause one of the module speeds to go above the attainable max velocity. Use the + * {@link #desaturateWheelSpeeds(SwerveModuleState2[], double) DesaturateWheelSpeeds} function to rectify this issue. + */ + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public SwerveModuleState2[] toSwerveModuleStates( + ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) + { + if (chassisSpeeds.vxMetersPerSecond == 0.0 + && chassisSpeeds.vyMetersPerSecond == 0.0 + && chassisSpeeds.omegaRadiansPerSecond == 0.0) + { + for (int i = 0; i < m_numModules; i++) + { + m_moduleStates[i].speedMetersPerSecond = 0.0; + } + + return m_moduleStates; + } + + if (!centerOfRotationMeters.equals(m_prevCoR)) + { + for (int i = 0; i < m_numModules; i++) + { + m_inverseKinematics.setRow( + i * 2, + 0, /* Start Data */ + 1, + 0, + -m_modules[i].getY() + centerOfRotationMeters.getY()); + m_inverseKinematics.setRow( + i * 2 + 1, + 0, /* Start Data */ + 0, + 1, + +m_modules[i].getX() - centerOfRotationMeters.getX()); + bigInverseKinematics.setRow( + i * 2, + 0, /* Start Data */ + 1, + 0, + -m_modules[i].getX() + centerOfRotationMeters.getX(), + -m_modules[i].getY() + centerOfRotationMeters.getY()); + bigInverseKinematics.setRow( + i * 2 + 1, + 0, /* Start Data */ + 0, + 1, + -m_modules[i].getY() + centerOfRotationMeters.getY(), + +m_modules[i].getX() - centerOfRotationMeters.getX()); + } + m_prevCoR = centerOfRotationMeters; + } + + var chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.setColumn( + 0, + 0, + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + + var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + + var accelerationVector = new SimpleMatrix(4, 1); + accelerationVector.setColumn( + 0, + 0, + 0, + 0, + Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), + 0 + ); + + var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector); + + for (int i = 0; i < m_numModules; i++) + { + double x = moduleVelocityStatesMatrix.get(i * 2, 0); + double y = moduleVelocityStatesMatrix.get(i * 2 + 1, 0); + + double ax = moduleAccelerationStatesMatrix.get(i * 2, 0); + double ay = moduleAccelerationStatesMatrix.get(i * 2 + 1, 0); + + double speed = Math.hypot(x, y); + Rotation2d angle = new Rotation2d(x, y); + + var trigThetaAngle = new SimpleMatrix(2, 2); + trigThetaAngle.setColumn( + 0, + 0, + angle.getCos(), + -angle.getSin() + ); + trigThetaAngle.setColumn( + 1, + 0, + angle.getSin(), + angle.getCos() + ); + + var accelVector = new SimpleMatrix(2, 1); + accelVector.setColumn( + 0, + 0, + ax, + ay + ); + + var omegaVector = trigThetaAngle.mult(accelVector); + + double omega = omegaVector.get(1, 0) / speed; + m_moduleStates[i] = new SwerveModuleState2(speed, angle, omega); + } + + return m_moduleStates; + } + + /** + * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)} toSwerveModuleStates + * for more information. + * + * @param chassisSpeeds The desired chassis speed. + * @return An array containing the module states. + */ + public SwerveModuleState2[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) + { + return toSwerveModuleStates(chassisSpeeds, new Translation2d()); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the given module states. This method is + * often used for odometry -- determining the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from respective encoders and + * gyros. The order of the swerve module states should be same as passed into the constructor of + * this class. + * @return The resulting chassis speed. + */ + public ChassisSpeeds toChassisSpeeds(SwerveModuleState2... wheelStates) + { + if (wheelStates.length != m_numModules) + { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) + { + var module = wheelStates[i]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + } + + var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); + return new ChassisSpeeds( + chassisSpeedsVector.get(0, 0), + chassisSpeedsVector.get(1, 0), + chassisSpeedsVector.get(2, 0)); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the given module states. This method is + * often used for odometry -- determining the robot's position on the field using data from the real-world speed and + * angle of each module on the robot. + * + * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition type) as measured from + * respective encoders and gyros. The order of the swerve module states should be same as passed + * into the constructor of this class. + * @return The resulting Twist2d. + */ + public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) + { + if (wheelDeltas.length != m_numModules) + { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor"); + } + var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) + { + var module = wheelDeltas[i]; + moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos()); + moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin()); + } + + var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix); + return new Twist2d( + chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0)); + } +} \ No newline at end of file diff --git a/swervelib/kinematics/SwerveModuleState2.java b/swervelib/kinematics/SwerveModuleState2.java new file mode 100644 index 0000000..f891acf --- /dev/null +++ b/swervelib/kinematics/SwerveModuleState2.java @@ -0,0 +1,119 @@ +package frc.robot.subsystems.swervedrive.swerve.kinematics; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +// Copied from team3181's REVSwerve2023 repo +public class SwerveModuleState2 extends edu.wpi.first.math.kinematics.SwerveModuleState +{ + + /** + * Angular velocity in radians per second. Angular Velocity = omega. + */ + public double angularVelocityRadPerSecond = 0; + + /** + * Constructs a SwerveModuleState with zeros for speed and angle. + */ + public SwerveModuleState2() + { + } + + /** + * Constructs a SwerveModuleState with zeros for speed and angle. + */ + public SwerveModuleState2(SwerveModuleState self) + { + super(self.speedMetersPerSecond, self.angle); + this.angularVelocityRadPerSecond = 0; + } + + /** + * Constructs a SwerveModuleState. + * + * @param speedMetersPerSecond The speed of the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle) + { + super(speedMetersPerSecond, angle); + this.angularVelocityRadPerSecond = 0; + } + + /** + * Constructs a SwerveModuleState. + * + * @param speedMetersPerSecond The speed of the wheel of the module. + * @param angle The angle of the module. + * @param angularVelocityRadPerSecond The angular velocity of the module. + */ + public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double angularVelocityRadPerSecond) + { + super(speedMetersPerSecond, angle); + this.angularVelocityRadPerSecond = angularVelocityRadPerSecond; + } + + /** + * Minimize the change in heading the desired swerve module state would require by potentially reversing the direction + * the wheel spins. Customized from WPILib's version to include placing in appropriate scope for CTRE and REV onboard + * control as both controllers as of writing don't have support for continuous input. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + public static SwerveModuleState2 optimize( + SwerveModuleState2 desiredState, Rotation2d currentAngle) + { + SwerveModuleState2 state; + Rotation2d delta = desiredState.angle.minus(currentAngle); + if (Math.abs(delta.getDegrees()) > 90.0) + { + state = new SwerveModuleState2( + -desiredState.speedMetersPerSecond, + desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0))); + } else + { + state = new SwerveModuleState2(desiredState.speedMetersPerSecond, desiredState.angle); + } + // state.angle = Rotation2d.fromDegrees( + // placeInAppropriate0To360Scope(currentAngle.getDegrees(), state.angle.getDegrees())); + return state; + } + + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within scope + */ + public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) + { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) + { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else + { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) + { + newAngle += 360; + } + while (newAngle > upperBound) + { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) + { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) + { + newAngle += 360; + } + return newAngle; + } +} \ No newline at end of file diff --git a/swervelib/motors/CTRESwerveMotor.java b/swervelib/motors/CTRESwerveMotor.java new file mode 100644 index 0000000..f847243 --- /dev/null +++ b/swervelib/motors/CTRESwerveMotor.java @@ -0,0 +1,492 @@ +package frc.robot.subsystems.swervedrive.swerve.motors; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; +import frc.robot.subsystems.swervedrive.swerve.SwerveMotor; +import java.util.function.Supplier; + +public class CTRESwerveMotor extends SwerveMotor +{ + + private final SwerveEncoder m_angleEncoder; + private final boolean m_integratedAbsEncoder; + private final double m_allowableClosedLoopError; + + private final ControlMode m_controlMode; + private final int m_mainPIDSlotId; + private final int m_mainPidId; + private final Supplier m_encoderRet; + private final TalonFX m_motor; + + /** + * kV feed forward for PID + */ + private final double m_moduleRadkV; + + // TODO: Finish this based off of BaseFalconSwerve + public CTRESwerveMotor(TalonFX motor, SwerveEncoder encoder, ModuleMotorType type, double gearRatio, + double wheelDiameter, + double freeSpeedRPM, double powerLimit) + { + TalonFXConfiguration config = new TalonFXConfiguration(); + + m_integratedAbsEncoder = encoder.m_encoder instanceof CANCoder; + // Inspired by the following sources + // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/rev/NeoDriveControllerFactoryBuilder.java#L38 + // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L68 + // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/Constants.java#L89 + // https://github.com/AusTINCANsProgrammingTeam/2022Swerve/blob/main/2022Swerve/src/main/java/frc/robot/Constants.java + motor.clearStickyFaults(); + motor.configFactoryDefault(); + + m_angleEncoder = encoder; + m_motorType = type; + m_motor = motor; + + m_motor.setNeutralMode(NeutralMode.Brake); + m_motor.setSensorPhase(true); + + m_motor.enableVoltageCompensation(true); + + m_mainPidId = CTRE_pidIdx.PRIMARY_PID.ordinal(); + if (type == ModuleMotorType.DRIVE) + { + m_moduleRadkV = 1; + m_allowableClosedLoopError = Units.inchesToMeters(1) * 60; + + m_mainPIDSlotId = CTRE_slotIdx.Velocity.ordinal(); + m_controlMode = ControlMode.Velocity; + m_encoderRet = m_motor::getSelectedSensorVelocity; + + setCurrentLimit(40); + + setConversionFactor(((Math.PI * wheelDiameter) / gearRatio) * 10); // Convert units to MPS. + } else + { + m_moduleRadkV = (12 * 60) / (freeSpeedRPM * Math.toRadians(360 / (m_integratedAbsEncoder ? 1 : gearRatio))); + m_allowableClosedLoopError = 5; + + // Configure the CANCoder as the remote sensor. + if (m_integratedAbsEncoder) + { + m_motor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + m_motor.configRemoteFeedbackFilter((CANCoder) m_angleEncoder.m_encoder, + CTRE_remoteSensor.REMOTE_SENSOR_0.ordinal()); + ((CANCoder) m_angleEncoder.m_encoder).configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180); + } else + { + m_motor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + m_motor.configIntegratedSensorInitializationStrategy(SensorInitializationStrategy.BootToZero); + m_motor.configIntegratedSensorAbsoluteRange(AbsoluteSensorRange.Signed_PlusMinus180); + setEncoder(0); + } + + m_mainPIDSlotId = CTRE_slotIdx.Distance.ordinal(); + m_controlMode = ControlMode.Position; + m_encoderRet = m_motor::getSelectedSensorPosition; + + setCurrentLimit(20); + + m_motor.configFeedbackNotContinuous(false, 100); + } + + setPIDOutputRange(-powerLimit, powerLimit); + setVoltageCompensation(12); + + optimizeCANFrames(); + + } + + /** + * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base. + * + * @param min Minimum output. + * @param max Maximum output. + */ + @Override + public void setPIDOutputRange(double min, double max) + { + m_motor.configPeakOutputReverse(min); + m_motor.configPeakOutputForward(max); + } + + /** + * Set the PIDF coefficients for the closed loop PID onboard the SparkMax. + * + * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units. + * Default is 1.0 + * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every + * PID Loop. + * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID + * loop). Default is 0.1 + * @param F Feed Fwd gain for Closed loop. + * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far + * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor + * units. + */ + @Override + public void setPIDF(double P, double I, double D, double F, double integralZone) + { + m_motor.selectProfileSlot(m_mainPIDSlotId, m_mainPidId); + // More Closed-Loop Configs at + // https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#closed-loop-configs-per-slot-four-slots-available + // Example at + // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20General/VelocityClosedLoop_ArbFeedForward/src/main/java/frc/robot/Robot.java + m_motor.config_kP(m_mainPIDSlotId, P); + m_motor.config_kI(m_mainPIDSlotId, I); + m_motor.config_kD(m_mainPIDSlotId, D); + m_motor.config_kF(m_mainPIDSlotId, F); + m_motor.config_IntegralZone(m_mainPIDSlotId, integralZone); + m_motor.configAllowableClosedloopError(m_mainPIDSlotId, m_allowableClosedLoopError); + + // If the closed loop error is within this threshold, the motor output will be neutral. Set to 0 to disable. + // Value is in sensor units. + } + + /** + * Configures the conversion factor based upon which motor. + * + * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning + * motor. + */ + @Override + public void setConversionFactor(double conversionFactor) + { + m_motor.configSelectedFeedbackCoefficient(conversionFactor); + } + + /** + * Set the target for the PID loop. + * + * @param target The PID target to aim for. + * @param feedforward The feedforward for this target. + */ + @Override + public void setTarget(double target, double feedforward) + { + m_motor.set(m_controlMode, target, DemandType.ArbitraryFeedForward, feedforward); + } + + /** + * Stop the motor by sending 0 volts to it. + */ + @Override + public void stop() + { + m_motor.set(ControlMode.PercentOutput, 0); + } + + /** + * Set the speed of the drive motor from -1 to 1. + * + * @param speed Speed from -1 to 1. + */ + @Override + public void set(double speed) + { + m_motor.set(ControlMode.PercentOutput, speed); + } + + /** + * Get the current value of the encoder corresponding to the PID. + * + * @return Current value of the encoder. + */ + @Override + public double get() + { + return m_encoderRet.get(); + } + + /** + * Get the current value of the encoder. + * + * @return Current value of the encoder. + */ + @Override + public double getPosition() + { + return m_motor.getSelectedSensorPosition(); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + m_motor.configVoltageCompSaturation(nominalVoltage); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + m_motor.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, currentLimit, currentLimit, 1)); + } + + /** + * Set the encoder value. + * + * @param value Value to set the encoder to. + */ + @Override + public void setEncoder(double value) + { + m_motor.setSelectedSensorPosition(value, m_mainPidId, 100); + } + + /** + * Check that the link is good on the swerve module and CAN bus is able to retrieve data. + * + * @return true on all devices are accessible over CAN. + */ + @Override + public boolean reachable() + { + return m_motor.getFirmwareVersion() != 0; + } + + /** + * Check if the absolute encoder is used inplace of the integrated encoder. + * + * @return true, if the absolute encoder is being used as the integrated controller. + */ + @Override + public boolean remoteIntegratedEncoder() + { + return m_integratedAbsEncoder; + } + + /** + * Optimize the CAN status frames to reduce utilization. + */ + @Override + public void optimizeCANFrames() + { + + } + + /** + * Save configuration data to the motor controller so it is persistent on reboot. + */ + @Override + public void saveConfig() + { + // Config is on the fly for falcons. + } + + /** + * Invert the motor. + * + * @param inverted Set the motor as inverted. + */ + @Override + public void setInverted(boolean inverted) + { + m_motor.setInverted(inverted); + } + + @Override + public double getAmps() + { + return m_motor.getStatorCurrent(); + } + + /** + * The Talon SRX Slot profile used to configure the motor to use for the PID. + */ + enum CTRE_slotIdx + { + Distance, Turning, Velocity, MotionProfile + } + + /** + * The TalonSRX PID to use onboard. + */ + enum CTRE_pidIdx + { + PRIMARY_PID, AUXILIARY_PID, THIRD_PID, FOURTH_PID + } + + enum CTRE_remoteSensor + { + REMOTE_SENSOR_0, REMOTE_SENSOR_1 + } + +// /** +// * Set the PIDF coefficients for the closed loop PID onboard the TalonSRX. +// * +// * @param profile The {@link CTRE_slotIdx} to use. +// * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor +// units. +// * Note the closed loop output interprets a final value of 1023 as full output. So use a gain +// * of '0.25' to get full output if err is 4096u (Mag Encoder 1 rotation) +// * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units +// every +// * PID Loop. Note the closed loop output interprets a final value of 1023 as full output. So +// * use a gain of '0.00025' to get full output if err is 4096u (Mag Encoder 1 rotation) after +// * 1000 loops +// * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per +// * PID loop). Note the closed loop output interprets a final value of 1023 as full output. So +// * use a gain of '250' to get full output if derr is 4096u per (Mag Encoder 1 rotation) per +// * 1000 loops (typ 1 sec) +// * @param F Feed Fwd gain for Closed loop. See documentation for calculation details. If using +// velocity, +// * motion magic, or motion profile, use (1023 * duty-cycle / +// * sensor-velocity-sensor-units-per-100ms) +// * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too +// * far from the target. This prevents unstable oscillation if the kI is too large. Value +// is in +// * sensor units. (ticks per 100ms) +// * @param moduleMotorType Motor Type for swerve module. +// */ +// private void setCTREPIDF(CTRE_slotIdx profile, double P, double I, double D, double F, double integralZone, +// ModuleMotorType moduleMotorType) +// { +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor +// : m_turningMotor)).selectProfileSlot( +// profile.ordinal(), CTRE_pidIdx.PRIMARY_PID.ordinal()); +// // More Closed-Loop Configs at +// // https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#closed-loop-configs-per-slot-four-slots-available +// // Example at +// // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20General/VelocityClosedLoop_ArbFeedForward/src/main/java/frc/robot/Robot.java +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kP( +// profile.ordinal(), P); +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kI( +// profile.ordinal(), I); +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kD( +// profile.ordinal(), D); +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kF( +// profile.ordinal(), F); +// +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor +// : m_turningMotor)).config_IntegralZone( +// profile.ordinal(), integralZone); +// +// // If the closed loop error is within this threshold, the motor output will be neutral. Set to 0 to disable. +// // Value is in sensor units. +// +// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor +// : m_turningMotor)).configAllowableClosedloopError( +// profile.ordinal(), 0); +// +// } +// +// /** +// * Set the angle using the onboard controller when working with CTRE Talons +// * +// * @param angle Angle in degrees +// */ +// private void setCTREAngle(double angle) +// { +// ((BaseTalon) m_turningMotor).set(ControlMode.Position, angle); +// // TODO: Pass feedforward down. +// } +// +// /** +// * Set the velocity of the drive motor. +// * +// * @param velocity Velocity in meters per second. +// */ +// private void setCTREDrive(double velocity) +// { +// ((BaseTalon) m_driveMotor).set(ControlMode.Velocity, driveFeedforward.calculate(velocity)); +// } +// +// /** +// * Set up the CTRE motors and configure class attributes correspondingly +// * +// * @param motor Motor controller to configure. +// * @param moduleMotorType Motor type to configure +// * @param gearRatio Gear ratio of the motor for one revolution. +// */ +// private void setupCTREMotor(BaseTalon motor, ModuleMotorType moduleMotorType, double gearRatio) +// { +// +// // Purposely did not configure status frames since CTRE motors should be on a CANivore +// +// motor.setSensorPhase(true); +// motor.setNeutralMode(NeutralMode.Brake); +// // Unable to use TalonFX configs since this should support both TalonSRX's and TalonFX's +// setVoltageCompensation(12, moduleMotorType); +// // Code is based off of. +// // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java#L91 +// // and here +// // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L52 +// +// if (moduleMotorType == ModuleMotorType.DRIVE) +// { +// setCurrentLimit(80, moduleMotorType); +// // Math set's the coefficient to the OUTPUT of the ENCODER (ticks/100ms) which is the INPUT to the PID. +// // We want to set the PID to use MPS == meters/second :) +// // Dimensional analysis, solve for K +// // ticks/100ms * K = meters/second +// // ticks/100ms * 100ms/(1s=1000ms) * (pi*diameter)meters/(ticks[4096]*gearRatio)ticks = meters/second +// // ticks/100ms * 1/10 * (pi*diameter)/(ticks[4096]*gearRatio)ticks = meters/second +// // ticks/100ms * (pi*diameter)/((ticks[4096]*gearRatio)*10) = meters/second +// // K = (pi*diameter)/((ticks[4096]*gearRatio)*10) +// // Set the feedback sensor up earlier in setCANRemoteFeedbackSensor() +// motor.configSelectedFeedbackCoefficient(((Math.PI * wheelDiameter) / ((4096 / gearRatio)) * 10)); +// } else +// { +// setCurrentLimit(20, moduleMotorType); +// setPIDF(0.2, 0, 0.1, 0, 100, moduleMotorType); +// } +// } +// +// /** +// * Set the CANCoder to be the primary PID on the motor controller and configure the PID to accept inputs in degrees. +// * The talon will communicate independently of the roboRIO to fetch the current CANCoder position (which will result +// * in PID adjustments when using a CANivore). +// * +// * @param motor Talon Motor controller to configure. +// * @param encoder CANCoder to use as the remote sensor. +// */ +// private void setupCTRECANCoderRemoteSensor(BaseTalon motor, CANCoder encoder) +// { +// motor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); +// motor.configRemoteFeedbackFilter(encoder, CTRE_remoteSensor.REMOTE_SENSOR_0.ordinal()); +// motor.configSelectedFeedbackCoefficient((double) 360 / 4096); // Degrees/Ticks +// // The CANCoder has 4096 ticks per 1 revolution. +// } +// +// /** +// * Returns whether the turning motor is a CTRE motor. +// * +// * @return is the turning motor a CTRE motor? +// */ +// private boolean isCTRETurningMotor() +// { +// return m_turningMotor instanceof BaseMotorController; +// } +// +// /** +// * Returns whether the drive motor is a CTRE motor. All CTRE motors implement the {@link BaseMotorController} class. +// * We will only support the TalonSRX and TalonFX. +// * +// * @return is the drive motor a CTRE motor? +// */ +// private boolean isCTREDriveMotor() +// { +// return m_driveMotor instanceof TalonFX || m_driveMotor instanceof TalonSRX; +// } + +} diff --git a/swervelib/motors/REVSwerveMotor.java b/swervelib/motors/REVSwerveMotor.java new file mode 100644 index 0000000..9eeddae --- /dev/null +++ b/swervelib/motors/REVSwerveMotor.java @@ -0,0 +1,395 @@ +package frc.robot.subsystems.swervedrive.swerve.motors; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.MotorFeedbackSensor; +import com.revrobotics.REVPhysicsSim; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxPIDController.ArbFFUnits; +import frc.robot.Robot; +import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder; +import frc.robot.subsystems.swervedrive.swerve.SwerveMotor; +import java.util.function.Supplier; + +public class REVSwerveMotor extends SwerveMotor +{ + + private final int m_mainPidSlot; + private final int m_secondaryPidSlot; + private final ControlType m_pidControlType; + private final CANSparkMax m_motor; + private final MotorFeedbackSensor m_encoder; + private final boolean m_integratedAbsEncoder; + private final SparkMaxPIDController m_pid; + private final ArbFFUnits m_feedforwardUnits; + private final Supplier m_encoderRet, m_encoderPosRet; + /** + * kV feed forward for PID + */ + private final double m_moduleRadkV; + + /** + * Constructor for REV Swerve Motor, expecting CANSparkMax. Clears sticky faults and restores factory defaults. + * + * @param motor SparkMAX motor controller. + * @param absoluteEncoder The absolute encoder used for the module, if the motor is a turning motor and the encoder is + * compatible it will set the encoder as the remote integrated encoder and does not need + * periodic synchronization. + * @param type Swerve module motor type. + * @param gearRatio Gear ratio. + * @param wheelDiameter Wheel diameter in meters. + * @param freeSpeedRPM Free speed RPM of the motor. + * @param powerLimit Power limit for the motor. + */ + public REVSwerveMotor(CANSparkMax motor, SwerveEncoder absoluteEncoder, ModuleMotorType type, double gearRatio, + double wheelDiameter, double freeSpeedRPM, double powerLimit) + { + m_integratedAbsEncoder = absoluteEncoder.m_encoder instanceof AbsoluteEncoder && type == ModuleMotorType.TURNING; + + // Inspired by the following sources + // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/rev/NeoDriveControllerFactoryBuilder.java#L38 + // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L68 + // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/Constants.java#L89 + // https://github.com/AusTINCANsProgrammingTeam/2022Swerve/blob/main/2022Swerve/src/main/java/frc/robot/Constants.java + motor.restoreFactoryDefaults(); + + m_motor = motor; + m_motorType = type; + m_encoder = m_integratedAbsEncoder ? (AbsoluteEncoder) absoluteEncoder.m_encoder : motor.getEncoder(); + m_pid = motor.getPIDController(); + + m_pid.setFeedbackDevice(m_encoder); + + motor.clearFaults(); + motor.setIdleMode(IdleMode.kBrake); + +// double powerDriving = .6, powerTurning = .4; // TODO: Change from magic numbers. + + if (type == ModuleMotorType.DRIVE) + { + m_moduleRadkV = 1; + + m_encoderRet = ((RelativeEncoder) m_encoder)::getVelocity; + m_encoderPosRet = ((RelativeEncoder) m_encoder)::getPosition; + m_feedforwardUnits = ArbFFUnits.kPercentOut; + m_pidControlType = ControlType.kVelocity; + m_mainPidSlot = REV_slotIdx.Velocity.ordinal(); + m_secondaryPidSlot = REV_slotIdx.Position.ordinal(); + +// setPIDF(0.1, 0, 0, 0, 1); + setPIDF(0.01, 0, 0.005, 0, 0); + + setConversionFactor(((Math.PI * wheelDiameter) / gearRatio) / 60); + + setCurrentLimit(50); + } else + { + m_moduleRadkV = (12 * 60) / (freeSpeedRPM * Math.toRadians(360 / (m_integratedAbsEncoder ? 1 : gearRatio))); + + if (m_integratedAbsEncoder) + { + m_encoderPosRet = m_encoderRet = ((AbsoluteEncoder) m_encoder)::getPosition; + } else + { + m_encoderPosRet = m_encoderRet = ((RelativeEncoder) m_encoder)::getPosition; + } + + m_feedforwardUnits = ArbFFUnits.kVoltage; + m_pidControlType = ControlType.kPosition; + m_mainPidSlot = REV_slotIdx.Position.ordinal(); + m_secondaryPidSlot = REV_slotIdx.Velocity.ordinal(); + + setPIDF(0.01, 0, 0.005, 0, 0); + + setConversionFactor(m_integratedAbsEncoder ? 360 : 360 / gearRatio); + + m_pid.setPositionPIDWrappingEnabled(true); + m_pid.setPositionPIDWrappingMinInput(-180); + m_pid.setPositionPIDWrappingMaxInput(180); + + setCurrentLimit(20); + } + + setPIDOutputRange(-powerLimit, powerLimit); + setVoltageCompensation(12); + + setEncoder(0); + + optimizeCANFrames(); + + m_motor.setCANTimeout(0); // Spin off configurations in a different thread. + if (!Robot.isReal()) + { + REVPhysicsSim.getInstance().addSparkMax(m_motor, 2.6f, 5676); + } + } + + /** + * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base. + * + * @param min Minimum output. + * @param max Maximum output. + */ + @Override + public void setPIDOutputRange(double min, double max) + { + m_pid.setOutputRange(min, max, m_mainPidSlot); + m_pid.setOutputRange(min, max, m_secondaryPidSlot); + } + + /** + * Set the PIDF coefficients for the closed loop PID onboard the SparkMax. + * + * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units. + * Default is 1.0 + * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every + * PID Loop. + * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID + * loop). Default is 0.1 + * @param F Feed Fwd gain for Closed loop. + * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far + * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor + * units. + */ + @Override + public void setPIDF(double P, double I, double D, double F, double integralZone) + { + // Example at + // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Velocity%20Closed%20Loop%20Control/src/main/java/frc/robot/Robot.java#L65-L71 + kP = P; + kI = I; + kD = D; + kF = F; + kIZ = integralZone; + m_pid.setP(P, m_mainPidSlot); + m_pid.setI(I, m_mainPidSlot); + m_pid.setD(D, m_mainPidSlot); + m_pid.setFF(F, m_mainPidSlot); + m_pid.setIZone(integralZone, m_mainPidSlot); + // m_pid.setOutputRange(-1, 1, m_mainPidSlot); + } + + /** + * Configures the conversion factor based upon which motor. + * + * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning + * motor. + */ + @Override + public void setConversionFactor(double conversionFactor) + { + if (m_motorType == ModuleMotorType.TURNING) + { + if (m_encoder instanceof AbsoluteEncoder) + { + ((AbsoluteEncoder) m_encoder).setPositionConversionFactor(conversionFactor); + ((AbsoluteEncoder) m_encoder).setVelocityConversionFactor(conversionFactor / 60); + } else + { + ((RelativeEncoder) m_encoder).setPositionConversionFactor(conversionFactor); + ((RelativeEncoder) m_encoder).setVelocityConversionFactor(conversionFactor / 60); + } + } else + { + ((RelativeEncoder) m_encoder).setVelocityConversionFactor(conversionFactor); + ((RelativeEncoder) m_encoder).setPositionConversionFactor( + conversionFactor * 60 * (Robot.isReal() ? 1 : 42 * 60)); // RPS -> RPM sim + // SparkMaxSimProfile assumes the velocity is in RPM and multiplies it by 60, in our use case velocity is in RPS + // The Sim profile also neglects to take into account that there are 42 ticks per rotation. + } + } + + /** + * Set the target for the PID loop. + * + * @param target The PID target to aim for. + * @param feedforward feedForward value. + */ + @Override + public void setTarget(double target, double feedforward) + { + this.target = target; + m_pid.setReference(target, m_pidControlType, m_mainPidSlot, feedforward * m_moduleRadkV, m_feedforwardUnits); + } + + /** + * Stop the motor by sending 0 volts to it. + */ + @Override + public void stop() + { + m_motor.set(0); + } + + /** + * Set the speed of the drive motor from -1 to 1. + * + * @param speed Speed from -1 to 1. + */ + @Override + public void set(double speed) + { + m_motor.set(speed); + } + + /** + * Get the current value of the encoder corresponding to the PID. + * + * @return Current value of the encoder. + */ + @Override + public double get() + { + return m_encoderRet.get(); + } + + /** + * Get the current output. + * + * @return Output amps. + */ + @Override + public double getAmps() + { + return m_motor.getOutputCurrent(); + } + + /** + * Get the current value of the encoder. + * + * @return Current value of the encoder. + */ + @Override + public double getPosition() + { + return m_encoderPosRet.get(); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + m_motor.enableVoltageCompensation(nominalVoltage); + } + + /** + * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with + * voltage compensation. This is useful to protect the motor from current spikes. + * + * @param currentLimit Current limit in AMPS at free speed. + */ + @Override + public void setCurrentLimit(int currentLimit) + { + m_motor.setSmartCurrentLimit(currentLimit); + } + + /** + * Set the encoder value. + * + * @param value Value to set the encoder to. + */ + @Override + public void setEncoder(double value) + { + if (m_encoder instanceof RelativeEncoder) + { + ((RelativeEncoder) m_encoder).setPosition(value); + } + } + + /** + * Set the CAN status frames. + * + * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower + * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current + * @param CANStatus2 Motor Position + * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position + * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position + */ + public void setCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) + { + m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0); + m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1); + m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2); + m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3); + m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4); + // TODO: Configure Status Frame 5 and 6 if necessary + // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + } + + /** + * Check that the link is good on the swerve module and CAN bus is able to retrieve data. + * + * @return true on all devices are accessible over CAN. + */ + @Override + public boolean reachable() + { + // Based off of https://github.com/DigitalDislocators/SDS-MK4i-NEO-Swerve-Template/blob/main/src/main/java/frc/robot/subsystems/SwerveModuleSparkMax.java#L490 + return m_motor.getFirmwareVersion() != 0; + } + + /** + * Check if the absolute encoder is used inplace of the integrated encoder. + * + * @return true, if the absolute encoder is being used as the integrated controller. + */ + @Override + public boolean remoteIntegratedEncoder() + { + return m_integratedAbsEncoder; + } + + /** + * Optimize the CAN status frames to reduce utilization. + */ + @Override + public void optimizeCANFrames() + { + // Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + if (m_motorType == ModuleMotorType.DRIVE) + { + setCANStatusFrames(10, 20, 500, 500, 500); + } else + { + setCANStatusFrames(10, 500, 20, 500, 500); + } + } + + /** + * Save configuration data to the motor controller so it is persistent on reboot. + */ + @Override + public void saveConfig() + { + m_motor.burnFlash(); + } + + /** + * Invert the motor. + * + * @param inverted Set the motor as inverted. + */ + @Override + public void setInverted(boolean inverted) + { + m_motor.setInverted(inverted); + } + + /** + * REV Slots for PID configuration. + */ + enum REV_slotIdx + { + Position, Velocity, Simulation + } +}