├── jitpack.yml ├── gradlew ├── .SysId ├── sysid.json └── sysid-window.json ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── src └── main │ └── java │ └── frc │ ├── swervelib │ ├── SwerveModuleFactoryBuilder.java │ ├── AbsoluteEncoderFactory.java │ ├── SimConstants.java │ ├── AbsoluteEncoder.java │ ├── SwerveInput.java │ ├── DriveController.java │ ├── usdigital │ │ ├── MA3AbsoluteConfiguration.java │ │ └── MA3FactoryBuilder.java │ ├── SteerController.java │ ├── SwerveModule.java │ ├── Gyroscope.java │ ├── ctre │ │ ├── CanCoderAbsoluteConfiguration.java │ │ ├── Falcon500SteerConfiguration.java │ │ ├── PigeonFactoryBuilder.java │ │ ├── CanCoderFactoryBuilder.java │ │ ├── Falcon500DriveControllerFactoryBuilder.java │ │ └── Falcon500SteerControllerFactoryBuilder.java │ ├── DriveControllerFactory.java │ ├── SteerControllerFactory.java │ ├── SwerveConstants.java │ ├── GyroscopeHelper.java │ ├── rev │ │ ├── NeoSteerConfiguration.java │ │ ├── NeoDriveControllerFactoryBuilder.java │ │ └── NeoSteerControllerFactoryBuilder.java │ ├── kauailabs │ │ └── navXFactoryBuilder.java │ ├── TrajectoryLogging.java │ ├── SwerveSubsystem.java │ ├── Mk3ModuleConfiguration.java │ ├── PoseTelemetry.java │ ├── SdsModuleConfigurations.java │ ├── Mk4ModuleConfiguration.java │ ├── ModuleConfiguration.java │ ├── SwerveModuleFactory.java │ ├── SwerveDrivetrainModel.java │ ├── Mk4iSwerveModuleHelper.java │ └── Mk4SwerveModuleHelper.java │ └── wpiClasses │ ├── SimpleMotorWithMassModel.java │ ├── Vector2d.java │ ├── ForceAtPose2d.java │ ├── MotorGearboxWheelSim.java │ ├── Force2d.java │ ├── QuadSwerveSim.java │ └── SwerveModuleSim.java ├── .wpilib └── wpilib_preferences.json ├── .vscode ├── launch.json └── settings.json ├── BearSwerve.json ├── settings.gradle ├── vendordeps ├── PathplannerLib.json ├── navx_frc.json ├── WPILibNewCommands.json ├── photonlib.json ├── REVLib.json └── Phoenix.json ├── README.md ├── WPILib-License.md ├── gradlew.bat ├── .gitignore └── config.gradle /jitpack.yml: -------------------------------------------------------------------------------- 1 | jdk: 2 | - openjdk11 3 | -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6391-Ursuline-Bearbotics/BearSwerve/HEAD/gradlew -------------------------------------------------------------------------------- /.SysId/sysid.json: -------------------------------------------------------------------------------- 1 | { 2 | "SysId": { 3 | "Analysis Type": "General Mechanism" 4 | } 5 | } 6 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6391-Ursuline-Bearbotics/BearSwerve/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveModuleFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public class SwerveModuleFactoryBuilder { 4 | } 5 | -------------------------------------------------------------------------------- /.wpilib/wpilib_preferences.json: -------------------------------------------------------------------------------- 1 | { 2 | "enableCppIntellisense": false, 3 | "currentLanguage": "java", 4 | "projectYear": "2022", 5 | "teamNumber": 6391 6 | } 7 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/AbsoluteEncoderFactory.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | @FunctionalInterface 4 | public interface AbsoluteEncoderFactory { 5 | AbsoluteEncoder create(Configuration configuration); 6 | } 7 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SimConstants.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public class SimConstants { 4 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////// 5 | // MISC CONSTANTS 6 | // Nominal Periodic code execution rates 7 | static public final double SIM_SAMPLE_RATE_SEC = 0.02; 8 | static public final double CTRLS_SAMPLE_RATE_SEC = 0.02; 9 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/AbsoluteEncoder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public interface AbsoluteEncoder { 4 | /** 5 | * Gets the current angle reading of the encoder in radians. 6 | * 7 | * @return The current angle in radians. Range: [0, 2pi) 8 | */ 9 | double getAbsoluteAngle(); 10 | 11 | double getAbsoluteAngleRetry(); 12 | 13 | void setAbsoluteEncoder(double position, double velocity); 14 | } 15 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveInput.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public class SwerveInput { 4 | public double m_translationX; 5 | public double m_translationY; 6 | public double m_rotation; 7 | 8 | public SwerveInput(double translationX, double translationY, double rotation) { 9 | this.m_translationX = translationX; 10 | this.m_translationY = translationY; 11 | this.m_rotation = rotation; 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/DriveController.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.math.system.plant.DCMotor; 4 | 5 | public interface DriveController { 6 | void setReferenceVoltage(double voltage); 7 | 8 | void setVelocity(double velocity); 9 | 10 | void resetEncoder(); 11 | 12 | DCMotor getDriveMotor(); 13 | 14 | double getStateVelocity(); 15 | 16 | double getOutputVoltage(); 17 | 18 | void setDriveEncoder(double position, double velocity); 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/usdigital/MA3AbsoluteConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.usdigital; 2 | 3 | public class MA3AbsoluteConfiguration { 4 | private final int id; 5 | private final double offset; 6 | 7 | public MA3AbsoluteConfiguration(int id, double offset) { 8 | this.id = id; 9 | this.offset = offset; 10 | } 11 | 12 | public int getId() { 13 | return id; 14 | } 15 | 16 | public double getOffset() { 17 | return offset; 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SteerController.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.math.system.plant.DCMotor; 4 | 5 | public interface SteerController { 6 | double getReferenceAngle(); 7 | 8 | void setReferenceAngle(double referenceAngleRadians); 9 | 10 | DCMotor getSteerMotor(); 11 | 12 | AbsoluteEncoder getAbsoluteEncoder(); 13 | 14 | double getStateAngle(); 15 | 16 | double getOutputVoltage(); 17 | 18 | void setSteerEncoder(double position, double velocity); 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveModule.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public interface SwerveModule { 4 | double getDriveVelocity(); 5 | 6 | double getSteerAngle(); 7 | 8 | ModuleConfiguration getModuleConfiguration(); 9 | 10 | DriveController getDriveController(); 11 | 12 | SteerController getSteerController(); 13 | 14 | AbsoluteEncoder getAbsoluteEncoder(); 15 | 16 | void resetWheelEncoder(); 17 | 18 | void set(double driveVoltage, double steerAngle); 19 | 20 | void setVelocity(double driveVelocity, double steerAngle); 21 | } 22 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | 8 | { 9 | "type": "wpilib", 10 | "name": "WPILib Desktop Debug", 11 | "request": "launch", 12 | "desktop": true, 13 | }, 14 | { 15 | "type": "wpilib", 16 | "name": "WPILib roboRIO Debug", 17 | "request": "launch", 18 | "desktop": false, 19 | } 20 | ] 21 | } 22 | -------------------------------------------------------------------------------- /BearSwerve.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "BearSwerve.json", 3 | "name": "bearswerve", 4 | "version": "2022.0.14", 5 | "uuid": "e00fa50a-6b3e-11ec-90d6-0242ac120003", 6 | "mavenUrls": [ 7 | "https://jitpack.io" 8 | ], 9 | "jsonUrl": "https://raw.githubusercontent.com/6391-Ursuline-Bearbotics/BearSwerve/master/BearSwerve.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.github.6391-Ursuline-Bearbotics", 13 | "artifactId": "BearSwerve", 14 | "version": "2022.0.14" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [] 19 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/Gyroscope.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.math.geometry.Rotation2d; 4 | 5 | public interface Gyroscope { 6 | /** 7 | * Gets the current heading (Yaw) as reported by the gyroscope. 8 | * @return The Rotation2d value of the heading. 9 | */ 10 | Rotation2d getGyroHeading(); 11 | 12 | /** 13 | * Sets the gyroscope angle. This can be used to set the direction the robot is currently facing to the 14 | * 'forwards' direction. 15 | */ 16 | void zeroGyroscope(double angle); 17 | 18 | /** 19 | * Sets the simulated gyroscope to a specified angle 20 | * @param angle Angle to be set in degrees. 21 | */ 22 | void setAngle(double angle); 23 | 24 | /** 25 | * Determines if the Gyro is ready to be used. 26 | * @return True/False if the gyro is ready to be used. 27 | */ 28 | Boolean getGyroReady(); 29 | } 30 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "java.configuration.updateBuildConfiguration": "automatic", 3 | "java.server.launchMode": "Standard", 4 | "files.exclude": { 5 | "**/.git": true, 6 | "**/.svn": true, 7 | "**/.hg": true, 8 | "**/CVS": true, 9 | "**/.DS_Store": true, 10 | "bin/": true, 11 | "**/.classpath": true, 12 | "**/.project": true, 13 | "**/.settings": true, 14 | "**/.factorypath": true, 15 | "**/*~": true 16 | }, 17 | "java.test.config": [ 18 | { 19 | "name": "WPIlibUnitTests", 20 | "workingDirectory": "${workspaceFolder}/build/jni/release", 21 | "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], 22 | "env": { 23 | "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , 24 | "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" 25 | } 26 | }, 27 | ], 28 | "java.test.defaultConfig": "WPIlibUnitTests" 29 | } 30 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2022' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name 'frcHome' 24 | url frcHomeMaven 25 | } 26 | } 27 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/CanCoderAbsoluteConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import com.ctre.phoenix.sensors.SensorInitializationStrategy; 4 | 5 | public class CanCoderAbsoluteConfiguration { 6 | private final int id; 7 | private final double offset; 8 | private final SensorInitializationStrategy initStrategy; 9 | 10 | public CanCoderAbsoluteConfiguration(int id, double offset, SensorInitializationStrategy initStrategy) { 11 | this.id = id; 12 | this.offset = offset; 13 | this.initStrategy = initStrategy; 14 | } 15 | 16 | public CanCoderAbsoluteConfiguration(int id, double offset) { 17 | this(id, offset, SensorInitializationStrategy.BootToAbsolutePosition); 18 | } 19 | 20 | public int getId() { 21 | return id; 22 | } 23 | 24 | public double getOffset() { 25 | return offset; 26 | } 27 | 28 | public SensorInitializationStrategy getInitStrategy() { 29 | return initStrategy; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/DriveControllerFactory.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; 4 | 5 | @FunctionalInterface 6 | public interface DriveControllerFactory { 7 | default void addDashboardEntries( 8 | ShuffleboardContainer container, 9 | Controller controller 10 | ) { 11 | container.addNumber("Current Velocity", controller::getStateVelocity); 12 | } 13 | 14 | default Controller create( 15 | ShuffleboardContainer container, 16 | DriveConfiguration driveConfiguration, 17 | ModuleConfiguration moduleConfiguration 18 | ) { 19 | var controller = create(driveConfiguration, moduleConfiguration); 20 | //addDashboardEntries(container, controller); 21 | 22 | return controller; 23 | } 24 | 25 | Controller create(DriveConfiguration driveConfiguration, ModuleConfiguration moduleConfiguration); 26 | } 27 | -------------------------------------------------------------------------------- /.SysId/sysid-window.json: -------------------------------------------------------------------------------- 1 | { 2 | "MainWindow": { 3 | "GLOBAL": { 4 | "height": "720", 5 | "maximized": "0", 6 | "style": "0", 7 | "userScale": "2", 8 | "width": "1280", 9 | "xpos": "262", 10 | "ypos": "154" 11 | } 12 | }, 13 | "Window": { 14 | "###Analyzer": { 15 | "Collapsed": "0", 16 | "Pos": "360,25", 17 | "Size": "530,530" 18 | }, 19 | "###Generator": { 20 | "Collapsed": "0", 21 | "Pos": "5,25", 22 | "Size": "350,273" 23 | }, 24 | "###Logger": { 25 | "Collapsed": "0", 26 | "Pos": "3,303", 27 | "Size": "350,400" 28 | }, 29 | "###Program Log": { 30 | "Collapsed": "0", 31 | "Pos": "360,560", 32 | "Size": "530,125" 33 | }, 34 | "Debug##Default": { 35 | "Collapsed": "0", 36 | "Pos": "60,60", 37 | "Size": "400,400" 38 | }, 39 | "Diagnostic Plots": { 40 | "Collapsed": "0", 41 | "Pos": "895,25", 42 | "Size": "375,660" 43 | } 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /vendordeps/PathplannerLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "PathplannerLib.json", 3 | "name": "PathplannerLib", 4 | "version": "2022.2.1", 5 | "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", 6 | "mavenUrls": [ 7 | "https://3015rangerrobotics.github.io/pathplannerlib/repo" 8 | ], 9 | "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.pathplanner.lib", 13 | "artifactId": "PathplannerLib-java", 14 | "version": "2022.2.1" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [ 19 | { 20 | "groupId": "com.pathplanner.lib", 21 | "artifactId": "PathplannerLib-cpp", 22 | "version": "2022.2.1", 23 | "libName": "PathplannerLib", 24 | "headerClassifier": "headers", 25 | "sharedLibrary": false, 26 | "skipInvalidPlatforms": true, 27 | "binaryPlatforms": [ 28 | "osxx86-64", 29 | "linuxathena" 30 | ] 31 | } 32 | ] 33 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SteerControllerFactory.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; 4 | 5 | @FunctionalInterface 6 | public interface SteerControllerFactory { 7 | default void addDashboardEntries( 8 | ShuffleboardContainer container, 9 | Controller controller 10 | ) { 11 | container.addNumber("Current Angle", () -> Math.toDegrees(controller.getStateAngle())); 12 | container.addNumber("Target Angle", () -> Math.toDegrees(controller.getReferenceAngle())); 13 | } 14 | 15 | default Controller create( 16 | ShuffleboardContainer dashboardContainer, 17 | SteerConfiguration steerConfiguration, 18 | ModuleConfiguration moduleConfiguration 19 | ) { 20 | var controller = create(steerConfiguration, moduleConfiguration); 21 | //addDashboardEntries(dashboardContainer, controller); 22 | 23 | return controller; 24 | } 25 | 26 | Controller create(SteerConfiguration steerConfiguration, ModuleConfiguration moduleConfiguration); 27 | } 28 | -------------------------------------------------------------------------------- /vendordeps/navx_frc.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "navx_frc.json", 3 | "name": "KauaiLabs_navX_FRC", 4 | "version": "4.0.442", 5 | "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", 6 | "mavenUrls": [ 7 | "https://repo1.maven.org/maven2/" 8 | ], 9 | "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.kauailabs.navx.frc", 13 | "artifactId": "navx-java", 14 | "version": "4.0.442" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [ 19 | { 20 | "groupId": "com.kauailabs.navx.frc", 21 | "artifactId": "navx-cpp", 22 | "version": "4.0.442", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": false, 26 | "libName": "navx_frc", 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "linuxathena", 30 | "linuxraspbian", 31 | "windowsx86-64" 32 | ] 33 | } 34 | ] 35 | } -------------------------------------------------------------------------------- /vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "2020.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "mavenUrls": [], 7 | "jsonUrl": "", 8 | "javaDependencies": [ 9 | { 10 | "groupId": "edu.wpi.first.wpilibNewCommands", 11 | "artifactId": "wpilibNewCommands-java", 12 | "version": "wpilib" 13 | } 14 | ], 15 | "jniDependencies": [], 16 | "cppDependencies": [ 17 | { 18 | "groupId": "edu.wpi.first.wpilibNewCommands", 19 | "artifactId": "wpilibNewCommands-cpp", 20 | "version": "wpilib", 21 | "libName": "wpilibNewCommands", 22 | "headerClassifier": "headers", 23 | "sourcesClassifier": "sources", 24 | "sharedLibrary": true, 25 | "skipInvalidPlatforms": true, 26 | "binaryPlatforms": [ 27 | "linuxathena", 28 | "linuxraspbian", 29 | "linuxaarch64bionic", 30 | "windowsx86-64", 31 | "windowsx86", 32 | "linuxx86-64", 33 | "osxx86-64" 34 | ] 35 | } 36 | ] 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveConstants.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.math.controller.PIDController; 4 | import edu.wpi.first.math.geometry.Pose2d; 5 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 6 | import edu.wpi.first.math.trajectory.TrapezoidProfile; 7 | 8 | public class SwerveConstants { 9 | public static double MAX_FWD_REV_SPEED_MPS; 10 | public static double MAX_STRAFE_SPEED_MPS; 11 | public static double MAX_ROTATE_SPEED_RAD_PER_SEC; 12 | public static double MAX_VOLTAGE; 13 | public static Pose2d DFLT_START_POSE; 14 | 15 | public static double THETACONTROLLERkP; 16 | public static double TRAJECTORYXkP; 17 | public static double TRAJECTORYYkP; 18 | public static TrapezoidProfile.Constraints THETACONTROLLERCONSTRAINTS; 19 | 20 | public static double TRACKWIDTH_METERS; 21 | public static double TRACKLENGTH_METERS; 22 | public static double MASS_kg; 23 | public static double MOI_KGM2; 24 | public static SwerveDriveKinematics KINEMATICS; 25 | 26 | public static PIDController XPIDCONTROLLER = new PIDController(TRAJECTORYXkP, 0, 0); 27 | public static PIDController YPIDCONTROLLER = new PIDController(TRAJECTORYYkP, 0, 0); 28 | } 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![](https://jitpack.io/v/6391-Ursuline-Bearbotics/BearSwerve.svg)](https://jitpack.io/#6391-Ursuline-Bearbotics/BearSwerve) 2 | 3 | ### BearSwerve 4 | 5 | BearSwerve is intended to be an all in one swerve solution including: teleop driving, simulation, trajectory following and more. It combines the work of [SDS swerve-lib](https://github.com/SwerveDriveSpecialties/swerve-lib), gerth2's [Swerve Sim](https://github.com/wpilibsuite/allwpilib/pull/3374), and [PathPlanner](https://github.com/mjansen4857/pathplanner/releases) 6 | 7 | ### Vendor Dependancy 8 | 9 | Bearswerve is intended to be used as a vendor dependancy. It can be added using this online link: `https://raw.githubusercontent.com/6391-Ursuline-Bearbotics/BearSwerve/master/BearSwerve.json`. You need to also have the: NavX, PathPlanner, CTRE Phoenix, and REVLib dependencies even if you aren't using them. 10 | 11 | ### BearSwerve Support 12 | 13 | While BearSwerve has currently only been tested on MK4 modules as mentioned above it borrows code from other established repositories and at least a portion of this code will probably make it into WPILib in the near future. 14 | 15 | BearSwerve is intended and generic enough to support many different swerve modules and motor / hardware configurations with those modules. 16 | 17 | ### Known Issues 18 | 19 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/GyroscopeHelper.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import com.ctre.phoenix.motorcontrol.can.TalonSRX; 4 | import com.ctre.phoenix.sensors.WPI_PigeonIMU; 5 | import com.kauailabs.navx.frc.AHRS; 6 | import com.kauailabs.navx.frc.AHRS.SerialDataType; 7 | 8 | import edu.wpi.first.wpilibj.SPI; 9 | import edu.wpi.first.wpilibj.SerialPort; 10 | import frc.swervelib.ctre.PigeonFactoryBuilder; 11 | import frc.swervelib.kauailabs.navXFactoryBuilder; 12 | 13 | public final class GyroscopeHelper { 14 | private GyroscopeHelper() { 15 | } 16 | 17 | public static Gyroscope createPigeonController(TalonSRX controller) { 18 | WPI_PigeonIMU pigeon = new WPI_PigeonIMU(controller); 19 | return new PigeonFactoryBuilder().build(pigeon); 20 | } 21 | 22 | public static Gyroscope createPigeonCAN(Integer id) { 23 | WPI_PigeonIMU pigeon = new WPI_PigeonIMU(id); 24 | return new PigeonFactoryBuilder().build(pigeon); 25 | } 26 | 27 | public static Gyroscope createnavXMXP() { 28 | AHRS navx = new AHRS(SPI.Port.kMXP, (byte) 200); 29 | return new navXFactoryBuilder().build(navx); 30 | } 31 | 32 | public static Gyroscope createnavXUSB() { 33 | AHRS navx = new AHRS(SerialPort.Port.kUSB, SerialDataType.kProcessedData, (byte) 200); 34 | return new navXFactoryBuilder().build(navx); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /vendordeps/photonlib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "photonlib.json", 3 | "name": "photonlib", 4 | "version": "v2021.1.8", 5 | "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", 6 | "mavenUrls": [ 7 | "https://maven.photonvision.org/repository/internal", 8 | "https://maven.photonvision.org/repository/snapshots" 9 | ], 10 | "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", 11 | "jniDependencies": [], 12 | "cppDependencies": [ 13 | { 14 | "groupId": "org.photonvision", 15 | "artifactId": "PhotonLib-cpp", 16 | "version": "v2021.1.8", 17 | "libName": "Photon", 18 | "headerClassifier": "headers", 19 | "sharedLibrary": true, 20 | "skipInvalidPlatforms": true, 21 | "binaryPlatforms": [ 22 | "windowsx86-64", 23 | "linuxathena", 24 | "linuxx86-64", 25 | "osxx86-64" 26 | ] 27 | } 28 | ], 29 | "javaDependencies": [ 30 | { 31 | "groupId": "org.photonvision", 32 | "artifactId": "PhotonLib-java", 33 | "version": "v2021.1.8" 34 | }, 35 | { 36 | "groupId": "org.photonvision", 37 | "artifactId": "PhotonTargeting-java", 38 | "version": "v2021.1.8" 39 | } 40 | ] 41 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/rev/NeoSteerConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.rev; 2 | 3 | import java.util.Objects; 4 | 5 | public class NeoSteerConfiguration { 6 | private final int motorPort; 7 | private final EncoderConfiguration encoderConfiguration; 8 | 9 | public NeoSteerConfiguration(int motorPort, EncoderConfiguration encoderConfiguration) { 10 | this.motorPort = motorPort; 11 | this.encoderConfiguration = encoderConfiguration; 12 | } 13 | 14 | public int getMotorPort() { 15 | return motorPort; 16 | } 17 | 18 | public EncoderConfiguration getEncoderConfiguration() { 19 | return encoderConfiguration; 20 | } 21 | 22 | @Override 23 | public boolean equals(Object o) { 24 | if (this == o) return true; 25 | if (o == null || getClass() != o.getClass()) return false; 26 | NeoSteerConfiguration that = (NeoSteerConfiguration) o; 27 | return getMotorPort() == that.getMotorPort() && getEncoderConfiguration().equals(that.getEncoderConfiguration()); 28 | } 29 | 30 | @Override 31 | public int hashCode() { 32 | return Objects.hash(getMotorPort(), getEncoderConfiguration()); 33 | } 34 | 35 | @Override 36 | public String toString() { 37 | return "NeoSteerConfiguration{" + 38 | "motorPort=" + motorPort + 39 | ", encoderConfiguration=" + encoderConfiguration + 40 | '}'; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/Falcon500SteerConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import java.util.Objects; 4 | 5 | public class Falcon500SteerConfiguration { 6 | private final int motorPort; 7 | private final EncoderConfiguration encoderConfiguration; 8 | 9 | public Falcon500SteerConfiguration(int motorPort, EncoderConfiguration encoderConfiguration) { 10 | this.motorPort = motorPort; 11 | this.encoderConfiguration = encoderConfiguration; 12 | } 13 | 14 | public int getMotorPort() { 15 | return motorPort; 16 | } 17 | 18 | public EncoderConfiguration getEncoderConfiguration() { 19 | return encoderConfiguration; 20 | } 21 | 22 | @Override 23 | public boolean equals(Object o) { 24 | if (this == o) return true; 25 | if (o == null || getClass() != o.getClass()) return false; 26 | Falcon500SteerConfiguration that = (Falcon500SteerConfiguration) o; 27 | return getMotorPort() == that.getMotorPort() && getEncoderConfiguration().equals(that.getEncoderConfiguration()); 28 | } 29 | 30 | @Override 31 | public int hashCode() { 32 | return Objects.hash(getMotorPort(), getEncoderConfiguration()); 33 | } 34 | 35 | @Override 36 | public String toString() { 37 | return "Falcon500SteerConfiguration{" + 38 | "motorPort=" + motorPort + 39 | ", encoderConfiguration=" + encoderConfiguration + 40 | '}'; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/PigeonFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import com.ctre.phoenix.sensors.BasePigeonSimCollection; 4 | import com.ctre.phoenix.sensors.WPI_PigeonIMU; 5 | import com.ctre.phoenix.sensors.PigeonIMU.PigeonState; 6 | 7 | import edu.wpi.first.math.geometry.Rotation2d; 8 | import frc.swervelib.Gyroscope; 9 | 10 | public class PigeonFactoryBuilder { 11 | private static BasePigeonSimCollection pigeonSim; 12 | 13 | private static double gyroOffset = 0.0; 14 | 15 | public Gyroscope build(WPI_PigeonIMU pigeon) { 16 | return new GyroscopeImplementation(pigeon); 17 | } 18 | 19 | private static class GyroscopeImplementation implements Gyroscope { 20 | private final WPI_PigeonIMU pigeon; 21 | 22 | private GyroscopeImplementation(WPI_PigeonIMU pigeon) { 23 | this.pigeon = pigeon; 24 | pigeonSim = pigeon.getSimCollection(); 25 | } 26 | 27 | @Override 28 | public Rotation2d getGyroHeading() { 29 | return Rotation2d.fromDegrees(pigeon.getFusedHeading() + gyroOffset); 30 | } 31 | 32 | @Override 33 | public Boolean getGyroReady() { 34 | return pigeon.getState().equals(PigeonState.Ready); 35 | } 36 | 37 | @Override 38 | public void zeroGyroscope(double angle) { 39 | gyroOffset = angle - getGyroHeading().getDegrees(); 40 | } 41 | 42 | @Override 43 | public void setAngle(double angle) { 44 | pigeonSim.setRawHeading(angle); 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /WPILib-License.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009-2021 FIRST and other WPILib contributors 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of FIRST, WPILib, nor the names of other WPILib 12 | contributors may be used to endorse or promote products derived from 13 | this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR 18 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR 19 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/SimpleMotorWithMassModel.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.system.plant.DCMotor; 4 | import edu.wpi.first.wpilibj.simulation.FlywheelSim; 5 | 6 | 7 | class SimpleMotorWithMassModel { 8 | 9 | double m_curDisplacementRev; 10 | 11 | FlywheelSim m_fwSim; 12 | 13 | /** 14 | * So far - this is just a wrapper around FlywheelSim to get position as an output. 15 | * @param motor Motor driving this mass 16 | * @param gearing Gearing between motor and controlled mass 17 | * @param moi Moment of inertia of the controlled mass 18 | */ 19 | SimpleMotorWithMassModel(DCMotor motor, double gearing, double moi) { 20 | m_fwSim = new FlywheelSim(motor, gearing, moi); 21 | } 22 | 23 | void update(double motorVoltage, double dtSeconds) { 24 | m_fwSim.setInputVoltage(motorVoltage); 25 | m_fwSim.update(dtSeconds); 26 | //Add additional state of displacement in a hacky-ish calculation 27 | m_curDisplacementRev += m_fwSim.getAngularVelocityRPM() / 60.0 * dtSeconds; 28 | } 29 | 30 | /** 31 | * Gets the present speed of the rotating mass. 32 | */ 33 | double getMechanismSpeed_RPM() { 34 | return m_fwSim.getAngularVelocityRPM(); 35 | } 36 | 37 | /** 38 | * Returns the present current draw of the mechanism. 39 | */ 40 | double getCurrent_A() { 41 | return m_fwSim.getCurrentDrawAmps(); 42 | } 43 | 44 | /** 45 | * Returns the present displacement in Revolutions. 46 | */ 47 | double getMechanismPositionRev() { 48 | return m_curDisplacementRev; 49 | } 50 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/kauailabs/navXFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.kauailabs; 2 | 3 | import com.kauailabs.navx.frc.AHRS; 4 | 5 | import edu.wpi.first.hal.SimDouble; 6 | import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 7 | import edu.wpi.first.math.geometry.Rotation2d; 8 | import frc.swervelib.Gyroscope; 9 | 10 | public class navXFactoryBuilder { 11 | public Gyroscope build(AHRS navX) { 12 | return new GyroscopeImplementation(navX); 13 | } 14 | 15 | private static class GyroscopeImplementation implements Gyroscope { 16 | private final AHRS navX; 17 | private final SimDouble angleSim; 18 | 19 | private static double gyroOffset = 0.0; 20 | 21 | private GyroscopeImplementation(AHRS navX) { 22 | this.navX = navX; 23 | 24 | int dev = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); 25 | angleSim = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "Yaw")); 26 | } 27 | 28 | @Override 29 | public Rotation2d getGyroHeading() { 30 | if (navX.isMagnetometerCalibrated()) { 31 | // We will only get valid fused headings if the magnetometer is calibrated 32 | return Rotation2d.fromDegrees(navX.getFusedHeading() + gyroOffset); 33 | } 34 | // We have to invert the angle of the NavX so that rotating the robot counter-clockwise makes the angle increase. 35 | return Rotation2d.fromDegrees(360.0 - navX.getYaw() + gyroOffset); 36 | } 37 | 38 | @Override 39 | public Boolean getGyroReady() { 40 | return !navX.isCalibrating(); 41 | } 42 | 43 | @Override 44 | public void zeroGyroscope(double angle) { 45 | gyroOffset = angle - getGyroHeading().getDegrees(); 46 | } 47 | 48 | @Override 49 | public void setAngle(double angle) { 50 | angleSim.set(angle); 51 | } 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/TrajectoryLogging.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.swervelib; 6 | 7 | import java.util.function.Supplier; 8 | 9 | import com.pathplanner.lib.PathPlannerTrajectory; 10 | import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; 11 | 12 | import edu.wpi.first.math.geometry.Pose2d; 13 | import edu.wpi.first.wpilibj.Timer; 14 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 15 | import edu.wpi.first.wpilibj2.command.CommandBase; 16 | 17 | public class TrajectoryLogging extends CommandBase { 18 | private final Timer m_timer = new Timer(); 19 | PathPlannerTrajectory trajectory; 20 | Supplier poseSupplier; 21 | 22 | /** Creates a new TrajectoryLogging. */ 23 | public TrajectoryLogging(PathPlannerTrajectory trajectory, Supplier pose) { 24 | this.trajectory = trajectory; 25 | this.poseSupplier = pose; 26 | } 27 | 28 | // Called when the command is initially scheduled. 29 | @Override 30 | public void initialize() { 31 | m_timer.reset(); 32 | m_timer.start(); 33 | } 34 | 35 | // Called every time the scheduler runs while the command is scheduled. 36 | @Override 37 | public void execute() { 38 | PathPlannerState state = (PathPlannerState) trajectory.sample(m_timer.get()); 39 | SmartDashboard.putNumber("Desired X", state.poseMeters.getX()); 40 | SmartDashboard.putNumber("Desired Y", state.poseMeters.getY()); 41 | SmartDashboard.putNumber("Desired Rotation", state.holonomicRotation.getDegrees()); 42 | SmartDashboard.putNumber("Current X", poseSupplier.get().getX()); 43 | SmartDashboard.putNumber("Current Y", poseSupplier.get().getY()); 44 | } 45 | 46 | // Called once the command ends or is interrupted. 47 | @Override 48 | public void end(boolean interrupted) {} 49 | 50 | // Returns true when the command should end. 51 | @Override 52 | public boolean isFinished() { 53 | return false; 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveSubsystem.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.swervelib; 6 | 7 | import frc.wpiClasses.QuadSwerveSim; 8 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 9 | import edu.wpi.first.math.kinematics.SwerveModuleState; 10 | import edu.wpi.first.wpilibj.DriverStation; 11 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 12 | 13 | import java.util.ArrayList; 14 | 15 | public class SwerveSubsystem extends SubsystemBase { 16 | private SwerveModuleState[] states; 17 | 18 | private ArrayList modules = new ArrayList(QuadSwerveSim.NUM_MODULES); 19 | public SwerveDrivetrainModel dt; 20 | 21 | public SwerveSubsystem(SwerveDrivetrainModel dt) { 22 | this.dt = dt; 23 | modules = dt.getRealModules(); 24 | } 25 | 26 | @Override 27 | public void periodic() { 28 | states = dt.getSwerveModuleStates(); 29 | 30 | if (states != null) { 31 | SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_FWD_REV_SPEED_MPS); 32 | 33 | modules.get(0).set(states[0].speedMetersPerSecond / SwerveConstants.MAX_FWD_REV_SPEED_MPS * SwerveConstants.MAX_VOLTAGE, states[0].angle.getRadians()); 34 | modules.get(1).set(states[1].speedMetersPerSecond / SwerveConstants.MAX_FWD_REV_SPEED_MPS * SwerveConstants.MAX_VOLTAGE, states[1].angle.getRadians()); 35 | modules.get(2).set(states[2].speedMetersPerSecond / SwerveConstants.MAX_FWD_REV_SPEED_MPS * SwerveConstants.MAX_VOLTAGE, states[2].angle.getRadians()); 36 | modules.get(3).set(states[3].speedMetersPerSecond / SwerveConstants.MAX_FWD_REV_SPEED_MPS * SwerveConstants.MAX_VOLTAGE, states[3].angle.getRadians()); 37 | 38 | dt.m_poseEstimator.update(dt.getGyroscopeRotation(), states[0], states[1], states[2], states[3]); 39 | } 40 | 41 | dt.updateTelemetry(); 42 | } 43 | 44 | @Override 45 | public void simulationPeriodic() { 46 | dt.update(DriverStation.isDisabled(), 13.2); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/Vector2d.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | /** This is a 2D vector struct that supports basic vector operations. */ 4 | public class Vector2d { 5 | @SuppressWarnings("MemberName") 6 | public double x; 7 | 8 | @SuppressWarnings("MemberName") 9 | public double y; 10 | 11 | public Vector2d() {} 12 | 13 | public Vector2d(double x, double y) { 14 | this.x = x; 15 | this.y = y; 16 | } 17 | 18 | /** 19 | * Rotate a vector in Cartesian space. 20 | * 21 | * @param angle angle in degrees by which to rotate vector counter-clockwise. 22 | */ 23 | public void rotate(double angle) { 24 | double cosA = Math.cos(angle * (Math.PI / 180.0)); 25 | double sinA = Math.sin(angle * (Math.PI / 180.0)); 26 | double[] out = new double[2]; 27 | out[0] = x * cosA - y * sinA; 28 | out[1] = x * sinA + y * cosA; 29 | x = out[0]; 30 | y = out[1]; 31 | } 32 | 33 | /** 34 | * Returns dot product of this vector with argument. 35 | * 36 | * @param vec Vector with which to perform dot product. 37 | * @return Dot product of this vector with argument. 38 | */ 39 | public double dot(Vector2d vec) { 40 | return x * vec.x + y * vec.y; 41 | } 42 | 43 | /** 44 | * Returns magnitude of vector. 45 | * 46 | * @return Magnitude of vector. 47 | */ 48 | public double magnitude() { 49 | return Math.sqrt(x * x + y * y); 50 | } 51 | 52 | /** 53 | * Returns scalar projection of this vector onto argument. 54 | * 55 | * @param vec Vector onto which to project this vector. 56 | * @return scalar projection of this vector onto argument. 57 | */ 58 | public double scalarProject(Vector2d vec) { 59 | return dot(vec) / vec.magnitude(); 60 | } 61 | 62 | /** 63 | * Returns the cross product of this vector with another. 64 | * 65 | * @param other Other vector to cross with 66 | * @return this X other 67 | */ 68 | public double cross(Vector2d other) { 69 | return this.x * other.y - this.y * other.x; 70 | } 71 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/usdigital/MA3FactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.usdigital; 2 | 3 | import edu.wpi.first.wpilibj.AnalogInput; 4 | import edu.wpi.first.wpilibj.RobotController; 5 | import edu.wpi.first.wpilibj.Timer; 6 | import frc.swervelib.AbsoluteEncoder; 7 | import frc.swervelib.AbsoluteEncoderFactory; 8 | 9 | public class MA3FactoryBuilder { 10 | private Direction direction = Direction.COUNTER_CLOCKWISE; 11 | private int periodMilliseconds = 10; 12 | private static MA3AbsoluteConfiguration configuration; 13 | private static double angle = 0; 14 | 15 | public MA3FactoryBuilder withReadingUpdatePeriod(int periodMilliseconds) { 16 | this.periodMilliseconds = periodMilliseconds; 17 | return this; 18 | } 19 | 20 | public AbsoluteEncoderFactory build() { 21 | return configuration -> { 22 | this.configuration = configuration; 23 | 24 | AnalogInput encoder = new AnalogInput(configuration.getId()); 25 | 26 | return new EncoderImplementation(encoder); 27 | }; 28 | } 29 | 30 | private static class EncoderImplementation implements AbsoluteEncoder { 31 | private final AnalogInput encoder; 32 | 33 | private EncoderImplementation(AnalogInput encoder) { 34 | this.encoder = encoder; 35 | } 36 | 37 | @Override 38 | public double getAbsoluteAngle() { 39 | angle = (1.0 - encoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI; 40 | angle += configuration.getOffset(); 41 | angle %= 2.0 * Math.PI; 42 | if (angle < 0.0) { 43 | angle += 2.0 * Math.PI; 44 | } 45 | 46 | return angle; 47 | } 48 | 49 | @Override 50 | public double getAbsoluteAngleRetry() { 51 | // No communication error to 52 | return getAbsoluteAngle(); 53 | } 54 | 55 | @Override 56 | public void setAbsoluteEncoder(double position, double velocity) { 57 | //encoder.setSimDevice(device); 58 | } 59 | } 60 | 61 | public enum Direction { 62 | CLOCKWISE, 63 | COUNTER_CLOCKWISE 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/Mk3ModuleConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import java.util.Objects; 4 | 5 | /** 6 | * Additional Mk3 module configuration parameters. 7 | *

8 | * The configuration parameters here are used to customize the behavior of the Mk3 swerve module. 9 | * Each setting is initialized to a default that should be adequate for most use cases. 10 | */ 11 | public class Mk3ModuleConfiguration { 12 | private double nominalVoltage = 12.0; 13 | private double driveCurrentLimit = 80.0; 14 | private double steerCurrentLimit = 20.0; 15 | 16 | public double getNominalVoltage() { 17 | return nominalVoltage; 18 | } 19 | 20 | public void setNominalVoltage(double nominalVoltage) { 21 | this.nominalVoltage = nominalVoltage; 22 | } 23 | 24 | public double getDriveCurrentLimit() { 25 | return driveCurrentLimit; 26 | } 27 | 28 | public void setDriveCurrentLimit(double driveCurrentLimit) { 29 | this.driveCurrentLimit = driveCurrentLimit; 30 | } 31 | 32 | public double getSteerCurrentLimit() { 33 | return steerCurrentLimit; 34 | } 35 | 36 | public void setSteerCurrentLimit(double steerCurrentLimit) { 37 | this.steerCurrentLimit = steerCurrentLimit; 38 | } 39 | 40 | @Override 41 | public boolean equals(Object o) { 42 | if (this == o) return true; 43 | if (o == null || getClass() != o.getClass()) return false; 44 | Mk3ModuleConfiguration that = (Mk3ModuleConfiguration) o; 45 | return Double.compare(that.getNominalVoltage(), getNominalVoltage()) == 0 && Double.compare(that.getDriveCurrentLimit(), getDriveCurrentLimit()) == 0 && Double.compare(that.getSteerCurrentLimit(), getSteerCurrentLimit()) == 0; 46 | } 47 | 48 | @Override 49 | public int hashCode() { 50 | return Objects.hash(getNominalVoltage(), getDriveCurrentLimit(), getSteerCurrentLimit()); 51 | } 52 | 53 | @Override 54 | public String toString() { 55 | return "Mk3ModuleConfiguration{" + 56 | "nominalVoltage=" + nominalVoltage + 57 | ", driveCurrentLimit=" + driveCurrentLimit + 58 | ", steerCurrentLimit=" + steerCurrentLimit + 59 | '}'; 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /vendordeps/REVLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVLib.json", 3 | "name": "REVLib", 4 | "version": "2022.1.1", 5 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 6 | "mavenUrls": [ 7 | "https://maven.revrobotics.com/" 8 | ], 9 | "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.revrobotics.frc", 13 | "artifactId": "REVLib-java", 14 | "version": "2022.1.1" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.revrobotics.frc", 20 | "artifactId": "REVLib-driver", 21 | "version": "2022.1.1", 22 | "skipInvalidPlatforms": true, 23 | "isJar": false, 24 | "validPlatforms": [ 25 | "windowsx86-64", 26 | "windowsx86", 27 | "linuxaarch64bionic", 28 | "linuxx86-64", 29 | "linuxathena", 30 | "linuxraspbian", 31 | "osxx86-64" 32 | ] 33 | } 34 | ], 35 | "cppDependencies": [ 36 | { 37 | "groupId": "com.revrobotics.frc", 38 | "artifactId": "REVLib-cpp", 39 | "version": "2022.1.1", 40 | "libName": "REVLib", 41 | "headerClassifier": "headers", 42 | "sharedLibrary": false, 43 | "skipInvalidPlatforms": true, 44 | "binaryPlatforms": [ 45 | "windowsx86-64", 46 | "windowsx86", 47 | "linuxaarch64bionic", 48 | "linuxx86-64", 49 | "linuxathena", 50 | "linuxraspbian", 51 | "osxx86-64" 52 | ] 53 | }, 54 | { 55 | "groupId": "com.revrobotics.frc", 56 | "artifactId": "REVLib-driver", 57 | "version": "2022.1.1", 58 | "libName": "REVLibDriver", 59 | "headerClassifier": "headers", 60 | "sharedLibrary": false, 61 | "skipInvalidPlatforms": true, 62 | "binaryPlatforms": [ 63 | "windowsx86-64", 64 | "windowsx86", 65 | "linuxaarch64bionic", 66 | "linuxx86-64", 67 | "linuxathena", 68 | "linuxraspbian", 69 | "osxx86-64" 70 | ] 71 | } 72 | ] 73 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/PoseTelemetry.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 4 | import edu.wpi.first.math.geometry.Pose2d; 5 | import edu.wpi.first.math.geometry.Transform2d; 6 | import edu.wpi.first.wpilibj.RobotBase; 7 | import edu.wpi.first.wpilibj.smartdashboard.Field2d; 8 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 9 | import frc.wpiClasses.QuadSwerveSim; 10 | 11 | public class PoseTelemetry { 12 | QuadSwerveSim swerveDt; 13 | SwerveDrivePoseEstimator m_poseEstimator; 14 | private static Field2d field = new Field2d(); 15 | 16 | // Pose at the end of the last update 17 | Pose2d endPose = SwerveConstants.DFLT_START_POSE; 18 | 19 | // Desired Position says where path planning logic wants the 20 | // robot to be at any given time. 21 | Pose2d desiredPose = new Pose2d(); 22 | 23 | // Estimated position says where you think your robot is at 24 | // Based on encoders, motion, vision, etc. 25 | Pose2d estimatedPose = new Pose2d(); 26 | 27 | // Actual position defines wherever the robot is actually at 28 | // at any time. It is unknowable in real life. The simulation 29 | // generates this as its primary output. 30 | Pose2d actualPose = new Pose2d(); 31 | 32 | public PoseTelemetry(QuadSwerveSim swerveDt, SwerveDrivePoseEstimator m_poseEstimator) { 33 | this.swerveDt = swerveDt; 34 | this.m_poseEstimator = m_poseEstimator; 35 | SmartDashboard.putData("Field", field); 36 | field.setRobotPose(SwerveConstants.DFLT_START_POSE); 37 | } 38 | 39 | public void setActualPose(Pose2d act) { 40 | actualPose = act; 41 | } 42 | 43 | public void update(double time) { 44 | // Check if the user moved the robot with the Field2D 45 | // widget, and reset the model if so. 46 | Pose2d startPose = field.getRobotPose(); 47 | Transform2d deltaPose = startPose.minus(endPose); 48 | if(deltaPose.getRotation().getDegrees() > 0.01 || deltaPose.getTranslation().getNorm() > 0.01){ 49 | swerveDt.modelReset(startPose); 50 | } 51 | 52 | if (RobotBase.isSimulation()) { 53 | //field.getObject("Robot").setPose(actualPose); 54 | } 55 | field.setRobotPose(m_poseEstimator.getEstimatedPosition()); 56 | 57 | endPose = field.getRobotPose(); 58 | } 59 | 60 | public Pose2d getFieldPose() { 61 | return field.getRobotObject().getPose(); 62 | } 63 | 64 | public Field2d getField() { 65 | return field; 66 | } 67 | 68 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SdsModuleConfigurations.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | public final class SdsModuleConfigurations { 4 | public static final ModuleConfiguration MK3_STANDARD = new ModuleConfiguration( 5 | 0.1016, 6 | (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 60.0), 7 | true, 8 | (15.0 / 32.0) * (10.0 / 60.0), 9 | true 10 | ); 11 | public static final ModuleConfiguration MK3_FAST = new ModuleConfiguration( 12 | 0.1016, 13 | (16.0 / 48.0) * (28.0 / 16.0) * (15.0 / 60.0), 14 | true, 15 | (15.0 / 32.0) * (10.0 / 60.0), 16 | true 17 | ); 18 | 19 | public static final ModuleConfiguration MK4_L1 = new ModuleConfiguration( 20 | 0.10033, 21 | (14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0), 22 | true, 23 | (15.0 / 32.0) * (10.0 / 60.0), 24 | true 25 | ); 26 | public static final ModuleConfiguration MK4_L2 = new ModuleConfiguration( 27 | 0.10033, 28 | (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0), 29 | true, 30 | (15.0 / 32.0) * (10.0 / 60.0), 31 | true 32 | ); 33 | public static final ModuleConfiguration MK4_L3 = new ModuleConfiguration( 34 | 0.10033, 35 | (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0), 36 | true, 37 | (15.0 / 32.0) * (10.0 / 60.0), 38 | true 39 | ); 40 | public static final ModuleConfiguration MK4_L4 = new ModuleConfiguration( 41 | 0.10033, 42 | (16.0 / 48.0) * (28.0 / 16.0) * (15.0 / 45.0), 43 | true, 44 | (15.0 / 32.0) * (10.0 / 60.0), 45 | true 46 | ); 47 | 48 | public static final ModuleConfiguration MK4I_L1 = new ModuleConfiguration( 49 | 0.10033, 50 | (14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0), 51 | true, 52 | (14.0 / 50.0) * (10.0 / 60.0), 53 | false 54 | ); 55 | public static final ModuleConfiguration MK4I_L2 = new ModuleConfiguration( 56 | 0.10033, 57 | (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0), 58 | true, 59 | (14.0 / 50.0) * (10.0 / 60.0), 60 | false 61 | ); 62 | public static final ModuleConfiguration MK4I_L3 = new ModuleConfiguration( 63 | 0.10033, 64 | (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0), 65 | true, 66 | (14.0 / 50.0) * (10.0 / 60.0), 67 | false 68 | ); 69 | 70 | private SdsModuleConfigurations() { 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/ForceAtPose2d.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Transform2d; 5 | import java.util.Objects; 6 | 7 | public class ForceAtPose2d { 8 | public Force2d m_force; 9 | public Pose2d m_pos; 10 | 11 | /** 12 | * Constructs a ForceAtDistance2d that's all zeroed out. 13 | */ 14 | public ForceAtPose2d() { 15 | this(new Force2d(), new Pose2d()); 16 | } 17 | 18 | /** 19 | * Constructs from a given force and position that force acts at. 20 | * @param force The force 21 | * @param pos The position the force is acting at 22 | */ 23 | public ForceAtPose2d( Force2d force, Pose2d pos) { 24 | m_force = force; 25 | m_pos = pos; 26 | } 27 | 28 | /** 29 | * @param centerOfRotation Pose at the center of rotation that the 30 | * torque should be calculated from 31 | * @return Returns the torque associated with this force at distance. 32 | * positive is counter-clockwise, negative is clockwise 33 | */ 34 | public double getTorque(Pose2d centerOfRotation) { 35 | Transform2d transCORtoF = new Transform2d(centerOfRotation, m_pos); 36 | 37 | //Align the force to the reference frame of the center of rotation 38 | Force2d alignedForce = getForceInRefFrame(centerOfRotation); 39 | 40 | //Calculate the lever arm the force acts at 41 | Vector2d leverArm = new Vector2d(transCORtoF.getX(), transCORtoF.getY()); 42 | return leverArm.cross(alignedForce.getVector2d()); 43 | } 44 | 45 | public Force2d getForceInRefFrame(Pose2d refFrame) { 46 | Transform2d trans = new Transform2d(refFrame, m_pos); 47 | return m_force.rotateBy(trans.getRotation()); 48 | } 49 | 50 | @Override 51 | public String toString() { 52 | return String.format("ForceAtDistance2D(X: %.2fN, Y: %.2fN)", m_force.getX(), m_force.getY()); 53 | } 54 | 55 | /** 56 | * Checks equality between this Force2d and another object. 57 | * 58 | * @param obj The other object. 59 | * @return Whether the two objects are equal or not. 60 | */ 61 | @Override 62 | public boolean equals(Object obj) { 63 | if (obj instanceof ForceAtPose2d) { 64 | return Math.abs(((ForceAtPose2d) obj).m_force.getX() - m_force.getX()) < 1E-9 65 | && Math.abs(((ForceAtPose2d) obj).m_force.getY() - m_force.getY()) < 1E-9 66 | && Math.abs(((ForceAtPose2d) obj).m_pos.getX() - m_pos.getX()) < 1E-9 67 | && Math.abs(((ForceAtPose2d) obj).m_pos.getY() - m_pos.getY()) < 1E-9; 68 | } 69 | return false; 70 | } 71 | 72 | @Override 73 | public int hashCode() { 74 | return Objects.hash(m_force.getX(), m_force.getY(), m_pos.getX(), m_pos.getY()); 75 | } 76 | } -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Resolve any "." and ".." in APP_HOME to make it shorter. 33 | for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi 34 | 35 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 36 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 37 | 38 | @rem Find java.exe 39 | if defined JAVA_HOME goto findJavaFromJavaHome 40 | 41 | set JAVA_EXE=java.exe 42 | %JAVA_EXE% -version >NUL 2>&1 43 | if "%ERRORLEVEL%" == "0" goto execute 44 | 45 | echo. 46 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 47 | echo. 48 | echo Please set the JAVA_HOME variable in your environment to match the 49 | echo location of your Java installation. 50 | 51 | goto fail 52 | 53 | :findJavaFromJavaHome 54 | set JAVA_HOME=%JAVA_HOME:"=% 55 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 56 | 57 | if exist "%JAVA_EXE%" goto execute 58 | 59 | echo. 60 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 61 | echo. 62 | echo Please set the JAVA_HOME variable in your environment to match the 63 | echo location of your Java installation. 64 | 65 | goto fail 66 | 67 | :execute 68 | @rem Setup the command line 69 | 70 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 71 | 72 | 73 | @rem Execute Gradle 74 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* 75 | 76 | :end 77 | @rem End local scope for the variables with windows NT shell 78 | if "%ERRORLEVEL%"=="0" goto mainEnd 79 | 80 | :fail 81 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 82 | rem the _cmd.exe /c_ return code! 83 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 84 | exit /b 1 85 | 86 | :mainEnd 87 | if "%OS%"=="Windows_NT" endlocal 88 | 89 | :omega 90 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode 2 | 3 | ### C++ ### 4 | # Prerequisites 5 | *.d 6 | 7 | # Compiled Object files 8 | *.slo 9 | *.lo 10 | *.o 11 | *.obj 12 | 13 | # Precompiled Headers 14 | *.gch 15 | *.pch 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | *.dll 21 | 22 | # Fortran module files 23 | *.mod 24 | *.smod 25 | 26 | # Compiled Static libraries 27 | *.lai 28 | *.la 29 | *.a 30 | *.lib 31 | 32 | # Executables 33 | *.exe 34 | *.out 35 | *.app 36 | 37 | ### Java ### 38 | # Compiled class file 39 | *.class 40 | 41 | # Log file 42 | *.log 43 | 44 | # BlueJ files 45 | *.ctxt 46 | 47 | # Mobile Tools for Java (J2ME) 48 | .mtj.tmp/ 49 | 50 | # Package Files # 51 | *.jar 52 | *.war 53 | *.nar 54 | *.ear 55 | *.zip 56 | *.tar.gz 57 | *.rar 58 | 59 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 60 | hs_err_pid* 61 | 62 | ### Linux ### 63 | *~ 64 | 65 | # temporary files which can be created if a process still has a handle open of a deleted file 66 | .fuse_hidden* 67 | 68 | # KDE directory preferences 69 | .directory 70 | 71 | # Linux trash folder which might appear on any partition or disk 72 | .Trash-* 73 | 74 | # .nfs files are created when an open file is removed but is still being accessed 75 | .nfs* 76 | 77 | ### macOS ### 78 | # General 79 | .DS_Store 80 | .AppleDouble 81 | .LSOverride 82 | 83 | # Icon must end with two \r 84 | Icon 85 | 86 | # Thumbnails 87 | ._* 88 | 89 | # Files that might appear in the root of a volume 90 | .DocumentRevisions-V100 91 | .fseventsd 92 | .Spotlight-V100 93 | .TemporaryItems 94 | .Trashes 95 | .VolumeIcon.icns 96 | .com.apple.timemachine.donotpresent 97 | 98 | # Directories potentially created on remote AFP share 99 | .AppleDB 100 | .AppleDesktop 101 | Network Trash Folder 102 | Temporary Items 103 | .apdisk 104 | 105 | ### VisualStudioCode ### 106 | .vscode/* 107 | !.vscode/settings.json 108 | !.vscode/tasks.json 109 | !.vscode/launch.json 110 | !.vscode/extensions.json 111 | 112 | ### Windows ### 113 | # Windows thumbnail cache files 114 | Thumbs.db 115 | ehthumbs.db 116 | ehthumbs_vista.db 117 | 118 | # Dump file 119 | *.stackdump 120 | 121 | # Folder config file 122 | [Dd]esktop.ini 123 | 124 | # Recycle Bin used on file shares 125 | $RECYCLE.BIN/ 126 | 127 | # Windows Installer files 128 | *.cab 129 | *.msi 130 | *.msix 131 | *.msm 132 | *.msp 133 | 134 | # Windows shortcuts 135 | *.lnk 136 | 137 | ### Gradle ### 138 | .gradle 139 | /build/ 140 | 141 | # Ignore Gradle GUI config 142 | gradle-app.setting 143 | 144 | # Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) 145 | !gradle-wrapper.jar 146 | 147 | # Cache of project 148 | .gradletasknamecache 149 | 150 | # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 151 | # gradle/wrapper/gradle-wrapper.properties 152 | 153 | # # VS Code Specific Java Settings 154 | .classpath 155 | .project 156 | .settings/ 157 | bin/ 158 | 159 | 160 | # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode 161 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/MotorGearboxWheelSim.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.system.plant.DCMotor; 4 | import edu.wpi.first.math.util.Units; 5 | 6 | class MotorGearboxWheelSim { 7 | 8 | DCMotor m_motor; 9 | double m_gearRatio; 10 | double m_wheelRadiusM; 11 | double m_curGroundForceN; 12 | double m_wheelRotationsRad; 13 | double m_gearboxFricCoefNmPerRadPerSec; 14 | double m_prevWheelRotationalSpeedRadPerSec; 15 | 16 | 17 | double m_wheelSpeedRPM; 18 | double m_motorSpeedRPM; 19 | 20 | /** 21 | * 1D simulation of a motor, gearbox, and wheel. 22 | * Provides an output torque, given an input speed and voltage. 23 | * @param motor Motor set used to provide power input. 24 | * @param gearRatio Gearbox Ratio. Gearboxes which slow 25 | * down motors (ie all of them) should have this greater than 1.0. 26 | * @param wheelRadiusM Radius of the wheel in meters 27 | * @param gearboxFricCoefNmPerRadPerSec Kinetic Friction Losses 28 | * in the gearbox (expressed in units of Nm of "fighting" force per 29 | * radian per second of motor speed). Set to 0 if you're awesome 30 | * with white lithium grease, make it positive if your team maybe 31 | * forget the grease sometimes. 32 | */ 33 | MotorGearboxWheelSim(DCMotor motor, 34 | double gearRatio, 35 | double wheelRadiusM, 36 | double gearboxFricCoefNmPerRadPerSec) { 37 | this.m_motor = motor; 38 | this.m_gearRatio = gearRatio; 39 | this.m_wheelRadiusM = wheelRadiusM; 40 | this.m_gearboxFricCoefNmPerRadPerSec = gearboxFricCoefNmPerRadPerSec; 41 | } 42 | 43 | public void update(double groundVelocityMPS, double motorVoltage, double dtSeconds) { 44 | 45 | double wheelRotSpd = groundVelocityMPS / m_wheelRadiusM; 46 | double motorRotSpd = wheelRotSpd * m_gearRatio; 47 | 48 | double motorTq = m_motor.KtNMPerAmp * m_motor.getCurrent(motorRotSpd, 49 | motorVoltage); 50 | 51 | double gboxFricTq = motorRotSpd * m_gearboxFricCoefNmPerRadPerSec; 52 | 53 | //div by 1/torque ratio, or mul by gear ratio 54 | double curWheelTq = motorTq * m_gearRatio - gboxFricTq; 55 | 56 | m_curGroundForceN = curWheelTq / m_wheelRadiusM / 2; 57 | 58 | //Trapezoidal integration 59 | m_wheelRotationsRad += (wheelRotSpd + m_prevWheelRotationalSpeedRadPerSec) / 2 60 | * dtSeconds; 61 | 62 | m_prevWheelRotationalSpeedRadPerSec = wheelRotSpd; 63 | 64 | m_wheelSpeedRPM = Units.radiansPerSecondToRotationsPerMinute(wheelRotSpd); 65 | m_motorSpeedRPM = Units.radiansPerSecondToRotationsPerMinute(motorRotSpd); 66 | } 67 | 68 | public double getPositionRev() { 69 | return m_wheelRotationsRad / 2 / Math.PI; 70 | } 71 | 72 | public double getGroundForce_N() { 73 | return m_curGroundForceN; 74 | } 75 | 76 | public double getWheelSpeed_RPM() { 77 | return m_wheelSpeedRPM; 78 | } 79 | 80 | public double getMotorSpeed_RPM() { 81 | return m_motorSpeedRPM; 82 | } 83 | 84 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/Mk4ModuleConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import java.util.Objects; 4 | 5 | import edu.wpi.first.math.system.plant.DCMotor; 6 | 7 | /** 8 | * Additional Mk4 module configuration parameters. 9 | *

10 | * The configuration parameters here are used to customize the behavior of the Mk4 swerve module. 11 | * Each setting is initialized to a default that should be adequate for most use cases. 12 | */ 13 | public class Mk4ModuleConfiguration { 14 | private double nominalVoltage = 12.0; 15 | private double driveCurrentLimit = 80.0; 16 | private double steerCurrentLimit = 20.0; 17 | 18 | private DCMotor driveMotor; 19 | private DCMotor steerMotor; 20 | 21 | public double getNominalVoltage() { 22 | return nominalVoltage; 23 | } 24 | 25 | public void setNominalVoltage(double nominalVoltage) { 26 | this.nominalVoltage = nominalVoltage; 27 | } 28 | 29 | public double getDriveCurrentLimit() { 30 | return driveCurrentLimit; 31 | } 32 | 33 | public void setDriveCurrentLimit(double driveCurrentLimit) { 34 | this.driveCurrentLimit = driveCurrentLimit; 35 | } 36 | 37 | public double getSteerCurrentLimit() { 38 | return steerCurrentLimit; 39 | } 40 | 41 | public void setSteerCurrentLimit(double steerCurrentLimit) { 42 | this.steerCurrentLimit = steerCurrentLimit; 43 | } 44 | 45 | /** 46 | * @return Gets the type of the drive motor. 47 | */ 48 | public DCMotor getDriveMotor() { 49 | return driveMotor; 50 | } 51 | 52 | /** 53 | * @return Gets the type of the steer motor. 54 | */ 55 | public DCMotor getSteerMotor() { 56 | return steerMotor; 57 | } 58 | 59 | /** 60 | * Sets the type of the drive motor. 61 | * @param motor The DCMotor to be used as the Drive Motor 62 | */ 63 | public void setDriveMotor(DCMotor motor) { 64 | driveMotor = motor; 65 | } 66 | 67 | /** 68 | * Sets the type of the steer motor. 69 | * @param motor The DCMotor to be used as the Steer Motor 70 | */ 71 | public void setSteerMotor(DCMotor motor) { 72 | steerMotor = motor; 73 | } 74 | 75 | @Override 76 | public boolean equals(Object o) { 77 | if (this == o) return true; 78 | if (o == null || getClass() != o.getClass()) return false; 79 | Mk4ModuleConfiguration that = (Mk4ModuleConfiguration) o; 80 | return Double.compare(that.getNominalVoltage(), getNominalVoltage()) == 0 && Double.compare(that.getDriveCurrentLimit(), getDriveCurrentLimit()) == 0 && Double.compare(that.getSteerCurrentLimit(), getSteerCurrentLimit()) == 0; 81 | } 82 | 83 | @Override 84 | public int hashCode() { 85 | return Objects.hash(getNominalVoltage(), getDriveCurrentLimit(), getSteerCurrentLimit()); 86 | } 87 | 88 | @Override 89 | public String toString() { 90 | return "Mk4ModuleConfiguration{" + 91 | "nominalVoltage=" + nominalVoltage + 92 | ", driveCurrentLimit=" + driveCurrentLimit + 93 | ", steerCurrentLimit=" + steerCurrentLimit + 94 | ", steerMotor=" + steerMotor + 95 | ", driveMotor=" + driveMotor + 96 | '}'; 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/CanCoderFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import com.ctre.phoenix.ErrorCode; 4 | import com.ctre.phoenix.sensors.AbsoluteSensorRange; 5 | import com.ctre.phoenix.sensors.CANCoder; 6 | import com.ctre.phoenix.sensors.CANCoderConfiguration; 7 | import com.ctre.phoenix.sensors.CANCoderStatusFrame; 8 | 9 | import edu.wpi.first.wpilibj.Timer; 10 | import frc.swervelib.AbsoluteEncoder; 11 | import frc.swervelib.AbsoluteEncoderFactory; 12 | 13 | public class CanCoderFactoryBuilder { 14 | private Direction direction = Direction.COUNTER_CLOCKWISE; 15 | private int periodMilliseconds = 10; 16 | private static double angle = 0; 17 | 18 | public CanCoderFactoryBuilder withReadingUpdatePeriod(int periodMilliseconds) { 19 | this.periodMilliseconds = periodMilliseconds; 20 | return this; 21 | } 22 | 23 | public CanCoderFactoryBuilder withDirection(Direction direction) { 24 | this.direction = direction; 25 | return this; 26 | } 27 | 28 | public AbsoluteEncoderFactory build() { 29 | return configuration -> { 30 | CANCoderConfiguration config = new CANCoderConfiguration(); 31 | config.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; 32 | config.magnetOffsetDegrees = Math.toDegrees(configuration.getOffset()); 33 | config.sensorDirection = direction == Direction.CLOCKWISE; 34 | config.initializationStrategy = configuration.getInitStrategy(); 35 | 36 | CANCoder encoder = new CANCoder(configuration.getId()); 37 | encoder.configAllSettings(config, 250); 38 | 39 | encoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, periodMilliseconds, 250); 40 | 41 | return new EncoderImplementation(encoder); 42 | }; 43 | } 44 | 45 | private static class EncoderImplementation implements AbsoluteEncoder { 46 | private final CANCoder encoder; 47 | 48 | private EncoderImplementation(CANCoder encoder) { 49 | this.encoder = encoder; 50 | } 51 | 52 | @Override 53 | public double getAbsoluteAngleRetry() { 54 | double time = Timer.getFPGATimestamp(); 55 | boolean success = false; 56 | boolean timeout = false; 57 | do { 58 | angle = getAbsoluteAngle(); 59 | success = encoder.getLastError() == ErrorCode.OK; 60 | timeout = Timer.getFPGATimestamp() - time > 8; 61 | } while (!success && !timeout); 62 | 63 | return angle; 64 | } 65 | 66 | @Override 67 | public double getAbsoluteAngle() { 68 | angle = Math.toRadians(encoder.getPosition()); 69 | 70 | angle %= 2.0 * Math.PI; 71 | if (angle < 0.0) { 72 | angle += 2.0 * Math.PI; 73 | } 74 | 75 | return angle; 76 | } 77 | 78 | @Override 79 | public void setAbsoluteEncoder(double position, double velocity) { 80 | // Position is in revolutions. Velocity is in RPM 81 | // CANCoder wants steps for postion. Steps per 100ms for velocity. CANCoder has 4096 CPR. 82 | encoder.getSimCollection().setRawPosition((int) (position * 4096)); 83 | // Divide by 600 to go from RPM to Rotations per 100ms. CANCoder has 4096 CPR. 84 | encoder.getSimCollection().setVelocity((int) (velocity / 600 * 4096)); 85 | } 86 | } 87 | 88 | public enum Direction { 89 | CLOCKWISE, 90 | COUNTER_CLOCKWISE 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ModuleConfiguration.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import java.util.Objects; 4 | 5 | /** 6 | * A swerve module configuration. 7 | *

8 | * A configuration represents a unique mechanical configuration of a module. For example, the Swerve Drive Specialties 9 | * Mk3 swerve module has two configurations, standard and fast, and therefore should have two configurations 10 | * ({@link SdsModuleConfigurations#MK3_STANDARD} and {@link SdsModuleConfigurations#MK3_FAST} respectively). 11 | */ 12 | public class ModuleConfiguration { 13 | private final double wheelDiameter; 14 | private final double driveReduction; 15 | private final boolean driveInverted; 16 | 17 | private final double steerReduction; 18 | private final boolean steerInverted; 19 | 20 | /** 21 | * Creates a new module configuration. 22 | * 23 | * @param wheelDiameter The diameter of the module's wheel in meters. 24 | * @param driveReduction The overall drive reduction of the module. Multiplying motor rotations by this value 25 | * should result in wheel rotations. 26 | * @param driveInverted Whether the drive motor should be inverted. If there is an odd number of gea reductions 27 | * this is typically true. 28 | * @param steerReduction The overall steer reduction of the module. Multiplying motor rotations by this value 29 | * should result in rotations of the steering pulley. 30 | * @param steerInverted Whether the steer motor should be inverted. If there is an odd number of gear reductions 31 | * this is typically true. 32 | */ 33 | public ModuleConfiguration(double wheelDiameter, double driveReduction, boolean driveInverted, 34 | double steerReduction, boolean steerInverted) { 35 | this.wheelDiameter = wheelDiameter; 36 | this.driveReduction = driveReduction; 37 | this.driveInverted = driveInverted; 38 | this.steerReduction = steerReduction; 39 | this.steerInverted = steerInverted; 40 | } 41 | 42 | /** 43 | * @return Gets the diameter of the wheel in meters. 44 | */ 45 | public double getWheelDiameter() { 46 | return wheelDiameter; 47 | } 48 | 49 | /** 50 | * @return Gets the overall reduction of the drive system. 51 | *

52 | * If this value is multiplied by drive motor rotations the result would be drive wheel rotations. 53 | */ 54 | public double getDriveReduction() { 55 | return driveReduction; 56 | } 57 | 58 | /** 59 | * @return Gets if the drive motor should be inverted. 60 | */ 61 | public boolean isDriveInverted() { 62 | return driveInverted; 63 | } 64 | 65 | /** 66 | * @return Gets the overall reduction of the steer system. 67 | *

68 | * If this value is multiplied by steering motor rotations the result would be steering pulley rotations. 69 | */ 70 | public double getSteerReduction() { 71 | return steerReduction; 72 | } 73 | 74 | /** 75 | * @return Gets if the steering motor should be inverted. 76 | */ 77 | public boolean isSteerInverted() { 78 | return steerInverted; 79 | } 80 | 81 | @Override 82 | public boolean equals(Object o) { 83 | if (this == o) return true; 84 | if (o == null || getClass() != o.getClass()) return false; 85 | ModuleConfiguration that = (ModuleConfiguration) o; 86 | return Double.compare(that.getWheelDiameter(), getWheelDiameter()) == 0 && 87 | Double.compare(that.getDriveReduction(), getDriveReduction()) == 0 && 88 | isDriveInverted() == that.isDriveInverted() && 89 | Double.compare(that.getSteerReduction(), getSteerReduction()) == 0 && 90 | isSteerInverted() == that.isSteerInverted(); 91 | } 92 | 93 | @Override 94 | public int hashCode() { 95 | return Objects.hash( 96 | getWheelDiameter(), 97 | getDriveReduction(), 98 | isDriveInverted(), 99 | getSteerReduction(), 100 | isSteerInverted() 101 | ); 102 | } 103 | 104 | @Override 105 | public String toString() { 106 | return "ModuleConfiguration{" + 107 | "wheelDiameter=" + wheelDiameter + 108 | ", driveReduction=" + driveReduction + 109 | ", driveInverted=" + driveInverted + 110 | ", steerReduction=" + steerReduction + 111 | ", steerInverted=" + steerInverted + 112 | '}'; 113 | } 114 | } 115 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/rev/NeoDriveControllerFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.rev; 2 | 3 | import com.revrobotics.CANSparkMax; 4 | import com.revrobotics.CANSparkMaxLowLevel; 5 | import com.revrobotics.RelativeEncoder; 6 | import com.revrobotics.SparkMaxPIDController; 7 | import com.revrobotics.CANSparkMax.ControlType; 8 | 9 | import edu.wpi.first.math.system.plant.DCMotor; 10 | import frc.swervelib.DriveController; 11 | import frc.swervelib.DriveControllerFactory; 12 | import frc.swervelib.ModuleConfiguration; 13 | 14 | public final class NeoDriveControllerFactoryBuilder { 15 | // PID configuration 16 | private double pidProportional = Double.NaN; 17 | private double pidIntegral = Double.NaN; 18 | private double pidDerivative = Double.NaN; 19 | 20 | private double nominalVoltage = Double.NaN; 21 | private double currentLimit = Double.NaN; 22 | 23 | public NeoDriveControllerFactoryBuilder withPidConstants(double proportional, double integral, double derivative) { 24 | this.pidProportional = proportional; 25 | this.pidIntegral = integral; 26 | this.pidDerivative = derivative; 27 | return this; 28 | } 29 | 30 | public boolean hasPidConstants() { 31 | return Double.isFinite(pidProportional) && Double.isFinite(pidIntegral) && Double.isFinite(pidDerivative); 32 | } 33 | 34 | public NeoDriveControllerFactoryBuilder withVoltageCompensation(double nominalVoltage) { 35 | this.nominalVoltage = nominalVoltage; 36 | return this; 37 | } 38 | 39 | public boolean hasVoltageCompensation() { 40 | return Double.isFinite(nominalVoltage); 41 | } 42 | 43 | public NeoDriveControllerFactoryBuilder withCurrentLimit(double currentLimit) { 44 | this.currentLimit = currentLimit; 45 | return this; 46 | } 47 | 48 | public boolean hasCurrentLimit() { 49 | return Double.isFinite(currentLimit); 50 | } 51 | 52 | public DriveControllerFactory build() { 53 | return new FactoryImplementation(); 54 | } 55 | 56 | private class FactoryImplementation implements DriveControllerFactory { 57 | @Override 58 | public ControllerImplementation create(Integer id, ModuleConfiguration moduleConfiguration) { 59 | CANSparkMax motor = new CANSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless); 60 | motor.setInverted(moduleConfiguration.isDriveInverted()); 61 | 62 | // Setup voltage compensation 63 | if (hasVoltageCompensation()) { 64 | motor.enableVoltageCompensation(nominalVoltage); 65 | } 66 | 67 | if (hasCurrentLimit()) { 68 | motor.setSmartCurrentLimit((int) currentLimit); 69 | } 70 | 71 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 100); 72 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); 73 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20); 74 | // Set neutral mode to brake 75 | motor.setIdleMode(CANSparkMax.IdleMode.kBrake); 76 | 77 | // Setup encoder 78 | RelativeEncoder encoder = motor.getEncoder(); 79 | double positionConversionFactor = Math.PI * moduleConfiguration.getWheelDiameter() * moduleConfiguration.getDriveReduction(); 80 | encoder.setPositionConversionFactor(positionConversionFactor); 81 | encoder.setVelocityConversionFactor(positionConversionFactor / 60.0); 82 | 83 | SparkMaxPIDController controller = motor.getPIDController(); 84 | if (hasPidConstants()) { 85 | controller.setP(pidProportional); 86 | controller.setI(pidIntegral); 87 | controller.setD(pidDerivative); 88 | } 89 | controller.setFeedbackDevice(encoder); 90 | 91 | return new ControllerImplementation(motor, encoder); 92 | } 93 | } 94 | 95 | private static class ControllerImplementation implements DriveController { 96 | private final CANSparkMax motor; 97 | private final SparkMaxPIDController controller; 98 | private final RelativeEncoder encoder; 99 | 100 | private ControllerImplementation(CANSparkMax motor, RelativeEncoder encoder) { 101 | this.motor = motor; 102 | this.controller = motor.getPIDController(); 103 | this.encoder = encoder; 104 | } 105 | 106 | @Override 107 | public void setReferenceVoltage(double voltage) { 108 | motor.setVoltage(voltage); 109 | } 110 | 111 | @Override 112 | public void setVelocity(double velocity) { 113 | controller.setReference(velocity, ControlType.kVelocity); 114 | } 115 | 116 | @Override 117 | public void resetEncoder() { 118 | encoder.setPosition(0); 119 | } 120 | 121 | @Override 122 | public void setDriveEncoder(double position, double velocity) { 123 | motor.getEncoder().setPosition(position); 124 | //motor.getEncoder().setVelocity() 125 | } 126 | 127 | @Override 128 | public DCMotor getDriveMotor() { 129 | return DCMotor.getNEO(1); 130 | } 131 | 132 | @Override 133 | public double getStateVelocity() { 134 | return encoder.getVelocity(); 135 | } 136 | 137 | @Override 138 | public double getOutputVoltage() { 139 | return motor.getBusVoltage() * motor.getAppliedOutput(); 140 | } 141 | } 142 | } 143 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/Force2d.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.Matrix; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.numbers.N1; 6 | import edu.wpi.first.math.numbers.N2; 7 | import org.ejml.simple.SimpleMatrix; 8 | 9 | public class Force2d { 10 | Matrix m_matrix; 11 | 12 | /** 13 | * Constructs a Force2d with X and Y components equal to zero. 14 | */ 15 | public Force2d() { 16 | this(0.0, 0.0); 17 | } 18 | 19 | /** 20 | * Constructs a Force2d with the X and Y components equal to the 21 | * provided values. 22 | * 23 | * @param x The x component of the force. 24 | * @param y The y component of the force. 25 | */ 26 | public Force2d( double x, double y) { 27 | m_matrix = new Matrix<>(new SimpleMatrix(2, 1)); 28 | m_matrix.set(0, 0, x); 29 | m_matrix.set(1, 0, y); 30 | } 31 | 32 | /** 33 | * Constructs a Force2d with the provided force magnitude and angle. This is 34 | * essentially converting from polar coordinates to Cartesian coordinates. 35 | * 36 | * @param mag The magnititude of the force 37 | * @param angle The angle from the x-axis to the force vector. 38 | */ 39 | public Force2d(double mag, Rotation2d angle) { 40 | this(mag * angle.getCos(), mag * angle.getSin()); 41 | } 42 | 43 | /** 44 | * Constructs a Force2d with the provided 2-element column matrix as the x/y components. 45 | * 46 | * @param m 2 row, 1 column input matrix 47 | */ 48 | public Force2d(Matrix m) { 49 | m_matrix = m; 50 | } 51 | 52 | /** 53 | * Constructs a Force2d with the provided vector assumed to represent the force. 54 | * 55 | * @param forceVec vector which represents some force in two dimensions 56 | */ 57 | public Force2d(Vector2d forceVec) { 58 | this(forceVec.x, forceVec.y); 59 | } 60 | 61 | /** 62 | * @return Returns the X component of the force. 63 | */ 64 | public double getX() { 65 | return m_matrix.get(0, 0); 66 | } 67 | 68 | /** 69 | * @return Returns the Y component of the force. 70 | */ 71 | public double getY() { 72 | return m_matrix.get(1, 0); 73 | } 74 | 75 | /** 76 | * @return Returns the norm, or distance from the origin to the force. 77 | */ 78 | public double getNorm() { 79 | return m_matrix.normF(); 80 | } 81 | 82 | /** 83 | * @return Gets a unit vector in the direction this force points. 84 | */ 85 | public Vector2d getUnitVector() { 86 | return new Vector2d(this.getX() / this.getNorm(), this.getY() / this.getNorm()); 87 | } 88 | 89 | /** 90 | * Applies a rotation to the force in 2d space. 91 | * 92 | *

This multiplies the force vector by a counterclockwise rotation 93 | * matrix of the given angle. 94 | * [x_new] [other.cos, -other.sin][x] 95 | * [y_new] = [other.sin, other.cos][y] 96 | * 97 | *

For example, rotating a Force2d of {2, 0} by 90 degrees will return a 98 | * Force2d of {0, 2}. 99 | * 100 | * @param angle The rotation to rotate the force by. 101 | * @return The new rotated force. 102 | */ 103 | public Force2d rotateBy(Rotation2d angle) { 104 | return new Force2d( 105 | this.getX() * angle.getCos() - this.getY() * angle.getSin(), 106 | this.getX() * angle.getSin() + this.getY() * angle.getCos() 107 | ); 108 | } 109 | 110 | /** 111 | * Adds two forces in 2d space and returns the sum. This is similar to 112 | * vector addition. 113 | * 114 | *

For example, Force2d{1.0, 2.5} + Force2d{2.0, 5.5} = 115 | * Force2d{3.0, 8.0} 116 | * 117 | * @param other The force to add. 118 | * @return The sum of the forces. 119 | */ 120 | public Force2d plus(Force2d other) { 121 | return new Force2d(this.m_matrix.plus(other.m_matrix)); 122 | } 123 | 124 | /** 125 | * Accumulates another force into this force. Returns nothing, acts "in-place" on this force. 126 | * @param other The force to add. 127 | */ 128 | public void accum(Force2d other) { 129 | this.m_matrix = this.m_matrix.plus(other.m_matrix); 130 | } 131 | 132 | /** 133 | * Subtracts the other force from the other force and returns the 134 | * difference. 135 | * 136 | *

For example, Force2d{5.0, 4.0} - Force2d{1.0, 2.0} = 137 | * Force2d{4.0, 2.0} 138 | * 139 | * @param other The force to subtract. 140 | * @return The difference between the two forces. 141 | */ 142 | public Force2d minus(Force2d other) { 143 | return new Force2d(this.m_matrix.minus(other.m_matrix)); 144 | } 145 | 146 | /** 147 | * Returns the inverse of the current force. This is equivalent to 148 | * rotating by 180 degrees, flipping the point over both axes, or simply 149 | * negating both components of the force. 150 | * 151 | * @return The inverse of the current force. 152 | */ 153 | public Force2d unaryMinus() { 154 | return new Force2d(this.m_matrix.times(-1.0)); 155 | } 156 | 157 | /** 158 | * Multiplies the force by a scalar and returns the new force. 159 | * 160 | *

For example, Force2d{2.0, 2.5} * 2 = Force2d{4.0, 5.0} 161 | * 162 | * @param scalar The scalar to multiply by. 163 | * @return The scaled force. 164 | */ 165 | public Force2d times(double scalar) { 166 | return new Force2d(this.m_matrix.times(scalar)); 167 | } 168 | 169 | /** 170 | * Divides the force by a scalar and returns the new force. 171 | * 172 | *

For example, Force2d{2.0, 2.5} / 2 = Force2d{1.0, 1.25} 173 | * 174 | * @param scalar The scalar to multiply by. 175 | * @return The reference to the new mutated object. 176 | */ 177 | public Force2d div(double scalar) { 178 | return new Force2d(this.m_matrix.div(scalar)); 179 | } 180 | 181 | /** 182 | * @return Creates a Vector2d object from the force this object represents. 183 | */ 184 | public Vector2d getVector2d() { 185 | return new Vector2d(this.getX(), this.getY()); 186 | } 187 | 188 | @Override 189 | public String toString() { 190 | return String.format("Force2d(X: %.2f, Y: %.2f)", this.getX(), this.getY()); 191 | } 192 | 193 | /** 194 | * Checks equality between this Force2d and another object. 195 | * 196 | * @param obj The other object. 197 | * @return Whether the two objects are equal or not. 198 | */ 199 | @Override 200 | public boolean equals(Object obj) { 201 | if (obj instanceof Force2d) { 202 | return this.m_matrix.isEqual(((Force2d) obj).m_matrix, 1E-9); 203 | } else { 204 | return false; 205 | } 206 | } 207 | } 208 | -------------------------------------------------------------------------------- /config.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | nativeUtils.addWpiNativeUtils() 4 | nativeUtils.withRoboRIO() 5 | 6 | nativeUtils { 7 | wpi { 8 | configureDependencies { 9 | wpiVersion = "2022.+" 10 | niLibVersion = "2022.2.3" 11 | opencvVersion = "4.5.2-1" 12 | googleTestVersion = "1.9.0-5-437e100-1" 13 | imguiVersion = "1.86-1" 14 | wpimathVersion = "2022.+" 15 | } 16 | } 17 | } 18 | 19 | nativeUtils.wpi.addWarnings() 20 | nativeUtils.wpi.addWarningsAsErrors() 21 | 22 | nativeUtils.setSinglePrintPerPlatform() 23 | 24 | model { 25 | components { 26 | all { 27 | targetPlatform nativeUtils.wpi.platforms.roborio 28 | } 29 | } 30 | // Uncomment this, and remove above lines to enable builds for all platforms 31 | // components { 32 | // all { 33 | // nativeUtils.useAllPlatforms(it) 34 | // } 35 | // } 36 | binaries { 37 | withType(NativeBinarySpec).all { 38 | nativeUtils.usePlatformArguments(it) 39 | } 40 | } 41 | } 42 | 43 | ext.appendDebugPathToBinaries = { binaries-> 44 | binaries.withType(StaticLibraryBinarySpec) { 45 | if (it.buildType.name.contains('debug')) { 46 | def staticFileDir = it.staticLibraryFile.parentFile 47 | def staticFileName = it.staticLibraryFile.name 48 | def staticFileExtension = staticFileName.substring(staticFileName.lastIndexOf('.')) 49 | staticFileName = staticFileName.substring(0, staticFileName.lastIndexOf('.')) 50 | staticFileName = staticFileName + 'd' + staticFileExtension 51 | def newStaticFile = new File(staticFileDir, staticFileName) 52 | it.staticLibraryFile = newStaticFile 53 | } 54 | } 55 | binaries.withType(SharedLibraryBinarySpec) { 56 | if (it.buildType.name.contains('debug')) { 57 | def sharedFileDir = it.sharedLibraryFile.parentFile 58 | def sharedFileName = it.sharedLibraryFile.name 59 | def sharedFileExtension = sharedFileName.substring(sharedFileName.lastIndexOf('.')) 60 | sharedFileName = sharedFileName.substring(0, sharedFileName.lastIndexOf('.')) 61 | sharedFileName = sharedFileName + 'd' + sharedFileExtension 62 | def newSharedFile = new File(sharedFileDir, sharedFileName) 63 | 64 | def sharedLinkFileDir = it.sharedLibraryLinkFile.parentFile 65 | def sharedLinkFileName = it.sharedLibraryLinkFile.name 66 | def sharedLinkFileExtension = sharedLinkFileName.substring(sharedLinkFileName.lastIndexOf('.')) 67 | sharedLinkFileName = sharedLinkFileName.substring(0, sharedLinkFileName.lastIndexOf('.')) 68 | sharedLinkFileName = sharedLinkFileName + 'd' + sharedLinkFileExtension 69 | def newLinkFile = new File(sharedLinkFileDir, sharedLinkFileName) 70 | 71 | it.sharedLibraryLinkFile = newLinkFile 72 | it.sharedLibraryFile = newSharedFile 73 | } 74 | } 75 | } 76 | 77 | ext.createComponentZipTasks = { components, names, base, type, project, func -> 78 | def stringNames = names.collect {it.toString()} 79 | def configMap = [:] 80 | components.each { 81 | if (it in NativeLibrarySpec && stringNames.contains(it.name)) { 82 | it.binaries.each { 83 | if (!it.buildable) return 84 | def target = nativeUtils.getPublishClassifier(it) 85 | if (configMap.containsKey(target)) { 86 | configMap.get(target).add(it) 87 | } else { 88 | configMap.put(target, []) 89 | configMap.get(target).add(it) 90 | } 91 | } 92 | } 93 | } 94 | def taskList = [] 95 | def outputsFolder = file("$project.buildDir/outputs") 96 | configMap.each { key, value -> 97 | def task = project.tasks.create(base + "-${key}", type) { 98 | description = 'Creates component archive for platform ' + key 99 | destinationDirectory = outputsFolder 100 | classifier = key 101 | archiveBaseName = '_M_' + base 102 | duplicatesStrategy = 'exclude' 103 | 104 | from(licenseFile) { 105 | into '/' 106 | } 107 | 108 | func(it, value) 109 | } 110 | taskList.add(task) 111 | 112 | project.build.dependsOn task 113 | 114 | project.artifacts { 115 | task 116 | } 117 | addTaskToCopyAllOutputs(task) 118 | } 119 | return taskList 120 | } 121 | 122 | ext.createAllCombined = { list, name, base, type, project -> 123 | def outputsFolder = file("$project.buildDir/outputs") 124 | 125 | def task = project.tasks.create(base + "-all", type) { 126 | description = "Creates component archive for all classifiers" 127 | destinationDirectory = outputsFolder 128 | classifier = "all" 129 | archiveBaseName = base 130 | duplicatesStrategy = 'exclude' 131 | 132 | list.each { 133 | if (it.name.endsWith('debug')) return 134 | from project.zipTree(it.archivePath) 135 | dependsOn it 136 | } 137 | } 138 | 139 | project.build.dependsOn task 140 | 141 | project.artifacts { 142 | task 143 | } 144 | 145 | return task 146 | 147 | } 148 | 149 | ext.includeStandardZipFormat = { task, value -> 150 | value.each { binary -> 151 | if (binary.buildable) { 152 | if (binary instanceof SharedLibraryBinarySpec) { 153 | task.dependsOn binary.tasks.link 154 | task.from(new File(binary.sharedLibraryFile.absolutePath + ".debug")) { 155 | into nativeUtils.getPlatformPath(binary) + '/shared' 156 | } 157 | def sharedPath = binary.sharedLibraryFile.absolutePath 158 | sharedPath = sharedPath.substring(0, sharedPath.length() - 4) 159 | 160 | task.from(new File(sharedPath + '.pdb')) { 161 | into nativeUtils.getPlatformPath(binary) + '/shared' 162 | } 163 | task.from(binary.sharedLibraryFile) { 164 | into nativeUtils.getPlatformPath(binary) + '/shared' 165 | } 166 | task.from(binary.sharedLibraryLinkFile) { 167 | into nativeUtils.getPlatformPath(binary) + '/shared' 168 | } 169 | } else if (binary instanceof StaticLibraryBinarySpec) { 170 | task.dependsOn binary.tasks.createStaticLib 171 | task.from(binary.staticLibraryFile) { 172 | into nativeUtils.getPlatformPath(binary) + '/static' 173 | } 174 | } 175 | } 176 | } 177 | } 178 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import com.ctre.phoenix.motorcontrol.NeutralMode; 4 | import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; 5 | import com.ctre.phoenix.motorcontrol.TalonFXControlMode; 6 | import com.ctre.phoenix.motorcontrol.TalonFXInvertType; 7 | import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; 8 | import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 9 | 10 | import edu.wpi.first.math.system.plant.DCMotor; 11 | import edu.wpi.first.wpilibj.RobotBase; 12 | import frc.swervelib.DriveController; 13 | import frc.swervelib.DriveControllerFactory; 14 | import frc.swervelib.ModuleConfiguration; 15 | 16 | public final class Falcon500DriveControllerFactoryBuilder { 17 | private static final double TICKS_PER_ROTATION = 2048.0; 18 | 19 | private static final int CAN_TIMEOUT_MS = 250; 20 | private static final int STATUS_FRAME_GENERAL_PERIOD_MS = 250; 21 | private static final int CAN_TIMEOUT_MS_SIM = 100; 22 | private static final int STATUS_FRAME_GENERAL_PERIOD_MS_SIM = 100; 23 | 24 | // PID configuration 25 | private double proportionalConstant = Double.NaN; 26 | private double integralConstant = Double.NaN; 27 | private double derivativeConstant = Double.NaN; 28 | 29 | private double nominalVoltage = Double.NaN; 30 | private double currentLimit = Double.NaN; 31 | 32 | public Falcon500DriveControllerFactoryBuilder withPidConstants(double proportional, double integral, double derivative) { 33 | this.proportionalConstant = proportional; 34 | this.integralConstant = integral; 35 | this.derivativeConstant = derivative; 36 | return this; 37 | } 38 | 39 | public boolean hasPidConstants() { 40 | return Double.isFinite(proportionalConstant) && Double.isFinite(integralConstant) && Double.isFinite(derivativeConstant); 41 | } 42 | 43 | public Falcon500DriveControllerFactoryBuilder withVoltageCompensation(double nominalVoltage) { 44 | this.nominalVoltage = nominalVoltage; 45 | return this; 46 | } 47 | 48 | public boolean hasVoltageCompensation() { 49 | return Double.isFinite(nominalVoltage); 50 | } 51 | 52 | public DriveControllerFactory build() { 53 | return new FactoryImplementation(); 54 | } 55 | 56 | public Falcon500DriveControllerFactoryBuilder withCurrentLimit(double currentLimit) { 57 | this.currentLimit = currentLimit; 58 | return this; 59 | } 60 | 61 | public boolean hasCurrentLimit() { 62 | return Double.isFinite(currentLimit); 63 | } 64 | 65 | private class FactoryImplementation implements DriveControllerFactory { 66 | @Override 67 | public ControllerImplementation create(Integer driveConfiguration, ModuleConfiguration moduleConfiguration) { 68 | TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); 69 | 70 | double sensorPositionCoefficient = Math.PI * moduleConfiguration.getWheelDiameter() * moduleConfiguration.getDriveReduction() / TICKS_PER_ROTATION; 71 | double sensorVelocityCoefficient = sensorPositionCoefficient * 10.0; 72 | 73 | if (hasPidConstants()) { 74 | motorConfiguration.slot0.kP = proportionalConstant; 75 | motorConfiguration.slot0.kI = integralConstant; 76 | motorConfiguration.slot0.kD = derivativeConstant; 77 | } 78 | 79 | if (hasVoltageCompensation()) { 80 | motorConfiguration.voltageCompSaturation = nominalVoltage; 81 | } 82 | 83 | if (hasCurrentLimit()) { 84 | motorConfiguration.supplyCurrLimit.currentLimit = currentLimit; 85 | motorConfiguration.supplyCurrLimit.enable = true; 86 | } 87 | 88 | WPI_TalonFX motor = new WPI_TalonFX(driveConfiguration); 89 | motor.configAllSettings(motorConfiguration); 90 | 91 | if (hasVoltageCompensation()) { 92 | // Enable voltage compensation 93 | motor.enableVoltageCompensation(true); 94 | } 95 | 96 | motor.setNeutralMode(NeutralMode.Brake); 97 | 98 | motor.setInverted(moduleConfiguration.isDriveInverted() ? TalonFXInvertType.Clockwise : TalonFXInvertType.CounterClockwise); 99 | motor.setSensorPhase(true); 100 | 101 | // Reduce CAN status frame rates 102 | if (RobotBase.isSimulation()) { 103 | motor.setStatusFramePeriod( 104 | StatusFrameEnhanced.Status_1_General, 105 | STATUS_FRAME_GENERAL_PERIOD_MS_SIM, 106 | CAN_TIMEOUT_MS_SIM 107 | ); 108 | } 109 | else { 110 | motor.setStatusFramePeriod( 111 | StatusFrameEnhanced.Status_1_General, 112 | STATUS_FRAME_GENERAL_PERIOD_MS, 113 | CAN_TIMEOUT_MS 114 | ); 115 | } 116 | 117 | return new ControllerImplementation(motor, sensorVelocityCoefficient); 118 | } 119 | } 120 | 121 | private class ControllerImplementation implements DriveController { 122 | private final WPI_TalonFX motor; 123 | private final double sensorVelocityCoefficient; 124 | private final double nominalVoltage = hasVoltageCompensation() ? Falcon500DriveControllerFactoryBuilder.this.nominalVoltage : 12.0; 125 | 126 | private ControllerImplementation(WPI_TalonFX motor, double sensorVelocityCoefficient) { 127 | this.motor = motor; 128 | this.sensorVelocityCoefficient = sensorVelocityCoefficient; 129 | } 130 | 131 | @Override 132 | public void setReferenceVoltage(double voltage) { 133 | motor.set(TalonFXControlMode.PercentOutput, voltage / nominalVoltage); 134 | } 135 | 136 | @Override 137 | public void setVelocity(double velocity) { 138 | motor.set(TalonFXControlMode.Velocity, velocity / sensorVelocityCoefficient); 139 | } 140 | 141 | @Override 142 | public void setDriveEncoder(double position, double velocity) { 143 | // Position is in revolutions. Velocity is in RPM 144 | // TalonFX wants steps for postion. Steps per 100ms for velocity. Falcon integrated encoder has 2048 CPR. 145 | motor.getSimCollection().setIntegratedSensorRawPosition((int) (position * 2048)); 146 | // Divide by 600 to go from RPM to Rotations per 100ms. Multiply by encoder ticks per revolution. 147 | motor.getSimCollection().setIntegratedSensorVelocity((int) (velocity / 600 * 2048)); 148 | } 149 | 150 | @Override 151 | public void resetEncoder() { 152 | motor.setSelectedSensorPosition(0); 153 | } 154 | 155 | @Override 156 | public DCMotor getDriveMotor() { 157 | return DCMotor.getFalcon500(1); 158 | } 159 | 160 | @Override 161 | public double getStateVelocity() { 162 | return motor.getSelectedSensorVelocity() * sensorVelocityCoefficient; 163 | } 164 | 165 | @Override 166 | public double getOutputVoltage() { 167 | return motor.getMotorOutputVoltage(); 168 | } 169 | } 170 | } 171 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveModuleFactory.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import edu.wpi.first.networktables.NetworkTableEntry; 4 | import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 5 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; 6 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 7 | 8 | public class SwerveModuleFactory { 9 | private final ModuleConfiguration moduleConfiguration; 10 | private final DriveControllerFactory driveControllerFactory; 11 | private final SteerControllerFactory steerControllerFactory; 12 | 13 | public SwerveModuleFactory(ModuleConfiguration moduleConfiguration, 14 | DriveControllerFactory driveControllerFactory, 15 | SteerControllerFactory steerControllerFactory) { 16 | this.moduleConfiguration = moduleConfiguration; 17 | this.driveControllerFactory = driveControllerFactory; 18 | this.steerControllerFactory = steerControllerFactory; 19 | } 20 | 21 | public SwerveModule create(DriveConfiguration driveConfiguration, SteerConfiguration steerConfiguration, String namePrefix) { 22 | var driveController = driveControllerFactory.create(driveConfiguration, moduleConfiguration); 23 | var steerController = steerControllerFactory.create(steerConfiguration, moduleConfiguration); 24 | 25 | return new ModuleImplementation(driveController, steerController, namePrefix); 26 | } 27 | 28 | public SwerveModule create(ShuffleboardLayout container, DriveConfiguration driveConfiguration, SteerConfiguration steerConfiguration, String namePrefix) { 29 | var driveController = driveControllerFactory.create( 30 | container, 31 | driveConfiguration, 32 | moduleConfiguration 33 | ); 34 | var steerContainer = steerControllerFactory.create( 35 | container, 36 | steerConfiguration, 37 | moduleConfiguration 38 | ); 39 | 40 | return new ModuleImplementation(driveController, steerContainer,namePrefix); 41 | } 42 | 43 | private class ModuleImplementation implements SwerveModule { 44 | private final DriveController driveController; 45 | private final SteerController steerController; 46 | 47 | 48 | private ShuffleboardTab tab = Shuffleboard.getTab("SwerveDt"); 49 | private NetworkTableEntry driveVoltageCmdEntry; 50 | private NetworkTableEntry driveVelocityCmdEntry; 51 | private NetworkTableEntry steerAngleCmdEntry; 52 | 53 | private ModuleImplementation(DriveController driveController, SteerController steerController, String namePrefix) { 54 | this.driveController = driveController; 55 | this.steerController = steerController; 56 | 57 | this.driveVoltageCmdEntry = tab.add(namePrefix + "Wheel Voltage Cmd V", 0).getEntry(); 58 | this.driveVelocityCmdEntry = tab.add(namePrefix + "Wheel Velocity Cmd RPM", 0).getEntry(); 59 | this.steerAngleCmdEntry = tab.add(namePrefix + "Azmth Des Angle Deg", 0).getEntry(); 60 | 61 | 62 | } 63 | 64 | @Override 65 | public void resetWheelEncoder() { 66 | driveController.resetEncoder(); 67 | } 68 | 69 | @Override 70 | public double getDriveVelocity() { 71 | return driveController.getStateVelocity(); 72 | } 73 | 74 | @Override 75 | public double getSteerAngle() { 76 | return steerController.getStateAngle(); 77 | } 78 | 79 | @Override 80 | public ModuleConfiguration getModuleConfiguration() { 81 | return moduleConfiguration; 82 | } 83 | 84 | @Override 85 | public DriveController getDriveController() { 86 | return driveController; 87 | } 88 | 89 | @Override 90 | public SteerController getSteerController() { 91 | return steerController; 92 | } 93 | 94 | @Override 95 | public AbsoluteEncoder getAbsoluteEncoder() { 96 | return steerController.getAbsoluteEncoder(); 97 | } 98 | 99 | 100 | @Override 101 | public void set(double driveVoltage, double steerAngle) { 102 | steerAngle %= (2.0 * Math.PI); 103 | if (steerAngle < 0.0) { 104 | steerAngle += 2.0 * Math.PI; 105 | } 106 | 107 | double difference = steerAngle - getSteerAngle(); 108 | // Change the target angle so the difference is in the range [-pi, pi) instead of [0, 2pi) 109 | if (difference >= Math.PI) { 110 | steerAngle -= 2.0 * Math.PI; 111 | } else if (difference < -Math.PI) { 112 | steerAngle += 2.0 * Math.PI; 113 | } 114 | difference = steerAngle - getSteerAngle(); // Recalculate difference 115 | 116 | // If the difference is greater than 90 deg or less than -90 deg the drive can be inverted so the total 117 | // movement of the module is less than 90 deg 118 | if (difference > Math.PI / 2.0 || difference < -Math.PI / 2.0) { 119 | // Only need to add 180 deg here because the target angle will be put back into the range [0, 2pi) 120 | steerAngle += Math.PI; 121 | driveVoltage *= -1.0; 122 | } 123 | 124 | // Put the target angle back into the range [0, 2pi) 125 | steerAngle %= (2.0 * Math.PI); 126 | if (steerAngle < 0.0) { 127 | steerAngle += 2.0 * Math.PI; 128 | } 129 | 130 | driveController.setReferenceVoltage(driveVoltage); 131 | steerController.setReferenceAngle(steerAngle); 132 | 133 | this.driveVoltageCmdEntry.setDouble(driveVoltage); 134 | this.steerAngleCmdEntry.setDouble(steerAngle*180/Math.PI); 135 | } 136 | 137 | @Override 138 | public void setVelocity(double driveVelocity, double steerAngle) { 139 | steerAngle %= (2.0 * Math.PI); 140 | if (steerAngle < 0.0) { 141 | steerAngle += 2.0 * Math.PI; 142 | } 143 | 144 | double difference = steerAngle - getSteerAngle(); 145 | // Change the target angle so the difference is in the range [-pi, pi) instead of [0, 2pi) 146 | if (difference >= Math.PI) { 147 | steerAngle -= 2.0 * Math.PI; 148 | } else if (difference < -Math.PI) { 149 | steerAngle += 2.0 * Math.PI; 150 | } 151 | difference = steerAngle - getSteerAngle(); // Recalculate difference 152 | 153 | // If the difference is greater than 90 deg or less than -90 deg the drive can be inverted so the total 154 | // movement of the module is less than 90 deg 155 | if (difference > Math.PI / 2.0 || difference < -Math.PI / 2.0) { 156 | // Only need to add 180 deg here because the target angle will be put back into the range [0, 2pi) 157 | steerAngle += Math.PI; 158 | driveVelocity *= -1.0; 159 | } 160 | 161 | // Put the target angle back into the range [0, 2pi) 162 | steerAngle %= (2.0 * Math.PI); 163 | if (steerAngle < 0.0) { 164 | steerAngle += 2.0 * Math.PI; 165 | } 166 | 167 | driveController.setVelocity(driveVelocity); 168 | steerController.setReferenceAngle(steerAngle); 169 | 170 | this.driveVelocityCmdEntry.setDouble(driveVelocity); 171 | this.steerAngleCmdEntry.setDouble(steerAngle*180/Math.PI); 172 | } 173 | } 174 | } 175 | -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/QuadSwerveSim.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Transform2d; 6 | import edu.wpi.first.math.geometry.Translation2d; 7 | import edu.wpi.first.math.geometry.Twist2d; 8 | import java.util.ArrayList; 9 | import java.util.Arrays; 10 | import java.util.List; 11 | 12 | 13 | public class QuadSwerveSim { 14 | 15 | public static final int FL = 0; // Front Left Module Index 16 | public static final int FR = 1; // Front Right Module Index 17 | public static final int BL = 2; // Back Left Module Index 18 | public static final int BR = 3; // Back Right Module Index 19 | public static final int NUM_MODULES = 4; 20 | 21 | List m_modules; 22 | 23 | Vector2d m_accelPrev = new Vector2d(); 24 | Vector2d m_velPrev = new Vector2d(); 25 | double m_rotAccelPrev = 0; 26 | double m_rotVelPrev = 0; 27 | 28 | public final List m_robotToModuleTL; 29 | public final List m_robotToModule; 30 | 31 | Pose2d m_curPose = new Pose2d(); 32 | 33 | double m_robotMasskg; 34 | double m_robotMOI; 35 | 36 | /** 37 | * Simulates a drivetrain powered by four swerve modules. 38 | * @param wheelBaseWidthM left-to-right distance between the wheel/ground 39 | * contact patches, in meters 40 | * @param wheelBaseLengthM front-to-back distance between the wheel/ground 41 | * contact patches, in meters 42 | * @param robotMasskg Total mass of the robot, in kilograms. 43 | * @param robotMOI Effective moment of inertia of the robot, in kilogram-meters-squared 44 | * @param modules Set of swerve modules controlling the drivetrain. 45 | * Order must be 0=FL, 1=FR, 2=BL, 3=BR 46 | */ 47 | public QuadSwerveSim( 48 | double wheelBaseWidthM, 49 | double wheelBaseLengthM, 50 | double robotMasskg, 51 | double robotMOI, 52 | List modules 53 | ) { 54 | this.m_modules = modules; 55 | 56 | m_robotToModuleTL = Arrays.asList( 57 | new Translation2d( wheelBaseWidthM / 2, wheelBaseLengthM / 2), 58 | new Translation2d( wheelBaseWidthM / 2, -wheelBaseLengthM / 2), 59 | new Translation2d(-wheelBaseWidthM / 2, wheelBaseLengthM / 2), 60 | new Translation2d(-wheelBaseWidthM / 2, -wheelBaseLengthM / 2) 61 | ); 62 | 63 | m_robotToModule = Arrays.asList( 64 | new Transform2d(m_robotToModuleTL.get(FL), new Rotation2d(0.0)), 65 | new Transform2d(m_robotToModuleTL.get(FR), new Rotation2d(0.0)), 66 | new Transform2d(m_robotToModuleTL.get(BL), new Rotation2d(0.0)), 67 | new Transform2d(m_robotToModuleTL.get(BR), new Rotation2d(0.0)) 68 | ); 69 | 70 | this.m_robotMasskg = robotMasskg; 71 | this.m_robotMOI = robotMOI; 72 | 73 | } 74 | 75 | /** 76 | * Resets the robot to a non-moving state somewhere on the field. 77 | * @param pose Position on the field to reset the robot to. 78 | */ 79 | public void modelReset(Pose2d pose) { 80 | m_accelPrev = new Vector2d(); 81 | m_velPrev = new Vector2d(); 82 | m_rotAccelPrev = 0; 83 | m_rotVelPrev = 0; 84 | for (int idx = 0; idx < NUM_MODULES; idx++) { 85 | m_modules.get(idx).reset(pose.transformBy(m_robotToModule.get(idx))); 86 | } 87 | m_curPose = pose; 88 | } 89 | 90 | /** 91 | * Steps the module simulation forward by one discrete step. 92 | * @param dtSeconds size of the discrete step to take 93 | */ 94 | public void update(double dtSeconds) { 95 | 96 | Pose2d fieldRF = new Pose2d(); // global origin 97 | Transform2d fieldToRobot = new Transform2d(fieldRF, m_curPose); 98 | 99 | //////////////////////////////////////////////////////////////// 100 | // Component-Force Calculations to populate the free-body diagram 101 | 102 | // Calculate each module's new position, and step it through simulation. 103 | for (int idx = 0; idx < NUM_MODULES; idx++) { 104 | Pose2d tmp = fieldRF.transformBy(fieldToRobot); 105 | Pose2d modPose = tmp.transformBy(m_robotToModule.get(idx)); 106 | m_modules.get(idx).setModulePose(modPose); 107 | m_modules.get(idx).update(dtSeconds); 108 | } 109 | 110 | // Force on frame from wheel motive forces (along-tread) 111 | ArrayList wheelMotiveForces = new ArrayList(NUM_MODULES); 112 | for (int idx = 0; idx < NUM_MODULES; idx++) { 113 | wheelMotiveForces.add(m_modules.get(idx).getWheelMotiveForce()); 114 | } 115 | 116 | // Friction Model 117 | Force2d preFricNetForce = new Force2d(); 118 | wheelMotiveForces.forEach((ForceAtPose2d mf) -> { 119 | //Add up all the forces that friction gets a chance to fight against 120 | preFricNetForce.accum(mf.getForceInRefFrame(m_curPose)); 121 | }); 122 | 123 | Force2d sidekickForce = new Force2d(0, 0); //TODO - make a generic "external force" input? 124 | 125 | preFricNetForce.accum(sidekickForce); 126 | 127 | //calculate force on the robot, "pre friction" (pf) 128 | ForceAtPose2d pfRobotForce = new ForceAtPose2d(preFricNetForce, m_curPose); 129 | 130 | // Calculate the forces from cross-tread friction at each module 131 | ArrayList xtreadFricFrc = new ArrayList(NUM_MODULES); 132 | for (int idx = 0; idx < NUM_MODULES; idx++) { 133 | SwerveModuleSim mod = m_modules.get(idx); 134 | //Assume force evenly applied to all modules. 135 | double ffrac = 1.0 / NUM_MODULES; 136 | Force2d pfModForce = pfRobotForce.getForceInRefFrame(mod.getPose()).times(ffrac); 137 | xtreadFricFrc.add(mod.getCrossTreadFricForce(pfModForce, dtSeconds)); 138 | } 139 | 140 | //////////////////////////////////////////////////////////////// 141 | // Combine forces in free-body diagram 142 | 143 | // Using all the above force components, do Sum of Forces 144 | Force2d forceOnRobotCenter = preFricNetForce; 145 | 146 | xtreadFricFrc.forEach((ForceAtPose2d f) -> { 147 | forceOnRobotCenter.accum(f.getForceInRefFrame(m_curPose)); 148 | }); 149 | 150 | ForceAtPose2d netForce = new ForceAtPose2d(forceOnRobotCenter, m_curPose); 151 | 152 | Force2d robotForceInFieldRefFrame = netForce.getForceInRefFrame(fieldRF); 153 | 154 | //Sum of Torques 155 | double netTorque = 0; 156 | 157 | for (int idx = 0; idx < NUM_MODULES; idx++) { 158 | netTorque += wheelMotiveForces.get(idx).getTorque(m_curPose); 159 | netTorque += xtreadFricFrc.get(idx).getTorque(m_curPose); 160 | } 161 | 162 | 163 | //////////////////////////////////////////////////////////////// 164 | // Apply Newton's 2nd law to get motion from forces 165 | 166 | //a = F/m in field frame 167 | Vector2d accel = robotForceInFieldRefFrame.times(1 / m_robotMasskg).getVector2d(); 168 | 169 | //Trapezoidal integration 170 | Vector2d velocity = new Vector2d( m_velPrev.x + (accel.x + m_accelPrev.x) / 2 * dtSeconds, 171 | m_velPrev.y + (accel.y + m_accelPrev.y) / 2 * dtSeconds); 172 | 173 | //Trapezoidal integration 174 | Translation2d posChange = new Translation2d( (velocity.x + m_velPrev.x) / 2 * dtSeconds, 175 | (velocity.y + m_velPrev.y) / 2 * dtSeconds); 176 | 177 | //Twist needs to be relative to robot reference frame 178 | posChange = posChange.rotateBy(m_curPose.getRotation().unaryMinus()); 179 | 180 | m_velPrev = velocity; 181 | m_accelPrev = accel; 182 | 183 | //alpha = T/I in field frame 184 | double rotAccel = netTorque / m_robotMOI; 185 | double rotVel = m_rotVelPrev + (rotAccel + m_rotAccelPrev) / 2 * dtSeconds; 186 | double rotPosChange = (rotVel + m_rotVelPrev) / 2 * dtSeconds; 187 | 188 | m_rotVelPrev = rotVel; 189 | m_rotAccelPrev = rotAccel; 190 | 191 | Twist2d motionThisLoop = new Twist2d(posChange.getX(), posChange.getY(), rotPosChange); 192 | 193 | m_curPose = m_curPose.exp(motionThisLoop); 194 | } 195 | 196 | /** 197 | * @return Returns the current pose of the drivetrain. 198 | */ 199 | public Pose2d getCurPose() { 200 | return m_curPose; 201 | } 202 | 203 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/rev/NeoSteerControllerFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.rev; 2 | 3 | import com.revrobotics.*; 4 | import com.revrobotics.CANSparkMax.ControlType; 5 | 6 | import frc.swervelib.*; 7 | import edu.wpi.first.math.system.plant.DCMotor; 8 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; 9 | 10 | public final class NeoSteerControllerFactoryBuilder { 11 | // PID configuration 12 | private double pidProportional = Double.NaN; 13 | private double pidIntegral = Double.NaN; 14 | private double pidDerivative = Double.NaN; 15 | 16 | private double nominalVoltage = Double.NaN; 17 | private double currentLimit = Double.NaN; 18 | 19 | public NeoSteerControllerFactoryBuilder withPidConstants(double proportional, double integral, double derivative) { 20 | this.pidProportional = proportional; 21 | this.pidIntegral = integral; 22 | this.pidDerivative = derivative; 23 | return this; 24 | } 25 | 26 | public boolean hasPidConstants() { 27 | return Double.isFinite(pidProportional) && Double.isFinite(pidIntegral) && Double.isFinite(pidDerivative); 28 | } 29 | 30 | public NeoSteerControllerFactoryBuilder withVoltageCompensation(double nominalVoltage) { 31 | this.nominalVoltage = nominalVoltage; 32 | return this; 33 | } 34 | 35 | public boolean hasVoltageCompensation() { 36 | return Double.isFinite(nominalVoltage); 37 | } 38 | 39 | public NeoSteerControllerFactoryBuilder withCurrentLimit(double currentLimit) { 40 | this.currentLimit = currentLimit; 41 | return this; 42 | } 43 | 44 | public boolean hasCurrentLimit() { 45 | return Double.isFinite(currentLimit); 46 | } 47 | 48 | public SteerControllerFactory> build(AbsoluteEncoderFactory encoderFactory) { 49 | return new FactoryImplementation<>(encoderFactory); 50 | } 51 | 52 | public class FactoryImplementation implements SteerControllerFactory> { 53 | private final AbsoluteEncoderFactory encoderFactory; 54 | 55 | public FactoryImplementation(AbsoluteEncoderFactory encoderFactory) { 56 | this.encoderFactory = encoderFactory; 57 | } 58 | 59 | @Override 60 | public void addDashboardEntries(ShuffleboardContainer container, ControllerImplementation controller) { 61 | SteerControllerFactory.super.addDashboardEntries(container, controller); 62 | container.addNumber("Absolute Encoder Angle", () -> Math.toDegrees(controller.absoluteEncoder.getAbsoluteAngle())); 63 | } 64 | 65 | @Override 66 | public ControllerImplementation create(NeoSteerConfiguration steerConfiguration, ModuleConfiguration moduleConfiguration) { 67 | AbsoluteEncoder absoluteEncoder = encoderFactory.create(steerConfiguration.getEncoderConfiguration()); 68 | 69 | CANSparkMax motor = new CANSparkMax(steerConfiguration.getMotorPort(), CANSparkMaxLowLevel.MotorType.kBrushless); 70 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 100); 71 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20); 72 | motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 20); 73 | motor.setIdleMode(CANSparkMax.IdleMode.kBrake); 74 | motor.setInverted(!moduleConfiguration.isSteerInverted()); 75 | if (hasVoltageCompensation()) { 76 | motor.enableVoltageCompensation(nominalVoltage); 77 | } 78 | if (hasCurrentLimit()) { 79 | motor.setSmartCurrentLimit((int) Math.round(currentLimit)); 80 | } 81 | 82 | RelativeEncoder integratedEncoder = motor.getEncoder(); 83 | integratedEncoder.setPositionConversionFactor(2.0 * Math.PI * moduleConfiguration.getSteerReduction()); 84 | integratedEncoder.setVelocityConversionFactor(2.0 * Math.PI * moduleConfiguration.getSteerReduction() / 60.0); 85 | integratedEncoder.setPosition(absoluteEncoder.getAbsoluteAngle()); 86 | 87 | SparkMaxPIDController controller = motor.getPIDController(); 88 | if (hasPidConstants()) { 89 | controller.setP(pidProportional); 90 | controller.setI(pidIntegral); 91 | controller.setD(pidDerivative); 92 | } 93 | controller.setFeedbackDevice(integratedEncoder); 94 | 95 | return new ControllerImplementation(motor, absoluteEncoder); 96 | } 97 | } 98 | 99 | public static class ControllerImplementation implements SteerController { 100 | private static final int ENCODER_RESET_ITERATIONS = 500; 101 | private static final double ENCODER_RESET_MAX_ANGULAR_VELOCITY = Math.toRadians(0.5); 102 | 103 | @SuppressWarnings({"FieldCanBeLocal", "unused"}) 104 | private final CANSparkMax motor; 105 | private final SparkMaxPIDController controller; 106 | private final RelativeEncoder motorEncoder; 107 | private final AbsoluteEncoder absoluteEncoder; 108 | 109 | private double referenceAngleRadians = 0; 110 | 111 | private double resetIteration = 0; 112 | 113 | public ControllerImplementation(CANSparkMax motor, AbsoluteEncoder absoluteEncoder) { 114 | this.motor = motor; 115 | this.controller = motor.getPIDController(); 116 | this.motorEncoder = motor.getEncoder(); 117 | this.absoluteEncoder = absoluteEncoder; 118 | } 119 | 120 | @Override 121 | public double getReferenceAngle() { 122 | return referenceAngleRadians; 123 | } 124 | 125 | @Override 126 | public void setReferenceAngle(double referenceAngleRadians) { 127 | double currentAngleRadians = motorEncoder.getPosition(); 128 | 129 | // Reset the NEO's encoder periodically when the module is not rotating. 130 | // Sometimes (~5% of the time) when we initialize, the absolute encoder isn't fully set up, and we don't 131 | // end up getting a good reading. If we reset periodically this won't matter anymore. 132 | if (motorEncoder.getVelocity() < ENCODER_RESET_MAX_ANGULAR_VELOCITY) { 133 | if (++resetIteration >= ENCODER_RESET_ITERATIONS) { 134 | resetIteration = 0; 135 | double absoluteAngle = absoluteEncoder.getAbsoluteAngle(); 136 | motorEncoder.setPosition(absoluteAngle); 137 | currentAngleRadians = absoluteAngle; 138 | } 139 | } else { 140 | resetIteration = 0; 141 | } 142 | 143 | double currentAngleRadiansMod = currentAngleRadians % (2.0 * Math.PI); 144 | if (currentAngleRadiansMod < 0.0) { 145 | currentAngleRadiansMod += 2.0 * Math.PI; 146 | } 147 | 148 | // The reference angle has the range [0, 2pi) but the Neo's encoder can go above that 149 | double adjustedReferenceAngleRadians = referenceAngleRadians + currentAngleRadians - currentAngleRadiansMod; 150 | if (referenceAngleRadians - currentAngleRadiansMod > Math.PI) { 151 | adjustedReferenceAngleRadians -= 2.0 * Math.PI; 152 | } else if (referenceAngleRadians - currentAngleRadiansMod < -Math.PI) { 153 | adjustedReferenceAngleRadians += 2.0 * Math.PI; 154 | } 155 | 156 | this.referenceAngleRadians = referenceAngleRadians; 157 | 158 | controller.setReference(adjustedReferenceAngleRadians, ControlType.kPosition); 159 | } 160 | 161 | @Override 162 | public DCMotor getSteerMotor() { 163 | return DCMotor.getNEO(1); 164 | } 165 | 166 | @Override 167 | public AbsoluteEncoder getAbsoluteEncoder() { 168 | return absoluteEncoder; 169 | } 170 | 171 | @Override 172 | public void setSteerEncoder(double position, double velocity) { 173 | motor.getEncoder().setPosition(position); 174 | } 175 | 176 | @Override 177 | public double getStateAngle() { 178 | double motorAngleRadians = motorEncoder.getPosition(); 179 | motorAngleRadians %= 2.0 * Math.PI; 180 | if (motorAngleRadians < 0.0) { 181 | motorAngleRadians += 2.0 * Math.PI; 182 | } 183 | 184 | return motorAngleRadians; 185 | } 186 | 187 | @Override 188 | public double getOutputVoltage() { 189 | return motor.getBusVoltage() * motor.getAppliedOutput(); 190 | } 191 | } 192 | } 193 | -------------------------------------------------------------------------------- /vendordeps/Phoenix.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "Phoenix.json", 3 | "name": "CTRE-Phoenix", 4 | "version": "5.21.1", 5 | "frcYear": 2022, 6 | "uuid": "ab676553-b602-441f-a38d-f1296eff6537", 7 | "mavenUrls": [ 8 | "https://maven.ctr-electronics.com/release/" 9 | ], 10 | "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "com.ctre.phoenix", 14 | "artifactId": "api-java", 15 | "version": "5.21.1" 16 | }, 17 | { 18 | "groupId": "com.ctre.phoenix", 19 | "artifactId": "wpiapi-java", 20 | "version": "5.21.1" 21 | } 22 | ], 23 | "jniDependencies": [ 24 | { 25 | "groupId": "com.ctre.phoenix", 26 | "artifactId": "cci", 27 | "version": "5.21.1", 28 | "isJar": false, 29 | "skipInvalidPlatforms": true, 30 | "validPlatforms": [ 31 | "linuxathena" 32 | ] 33 | }, 34 | { 35 | "groupId": "com.ctre.phoenix.sim", 36 | "artifactId": "cci-sim", 37 | "version": "5.21.1", 38 | "isJar": false, 39 | "skipInvalidPlatforms": true, 40 | "validPlatforms": [ 41 | "windowsx86-64", 42 | "linuxx86-64", 43 | "osxx86-64" 44 | ] 45 | }, 46 | { 47 | "groupId": "com.ctre.phoenix.sim", 48 | "artifactId": "simTalonSRX", 49 | "version": "5.21.1", 50 | "isJar": false, 51 | "skipInvalidPlatforms": true, 52 | "validPlatforms": [ 53 | "windowsx86-64", 54 | "linuxx86-64", 55 | "osxx86-64" 56 | ] 57 | }, 58 | { 59 | "groupId": "com.ctre.phoenix.sim", 60 | "artifactId": "simTalonFX", 61 | "version": "5.21.1", 62 | "isJar": false, 63 | "skipInvalidPlatforms": true, 64 | "validPlatforms": [ 65 | "windowsx86-64", 66 | "linuxx86-64", 67 | "osxx86-64" 68 | ] 69 | }, 70 | { 71 | "groupId": "com.ctre.phoenix.sim", 72 | "artifactId": "simVictorSPX", 73 | "version": "5.21.1", 74 | "isJar": false, 75 | "skipInvalidPlatforms": true, 76 | "validPlatforms": [ 77 | "windowsx86-64", 78 | "linuxx86-64", 79 | "osxx86-64" 80 | ] 81 | }, 82 | { 83 | "groupId": "com.ctre.phoenix.sim", 84 | "artifactId": "simPigeonIMU", 85 | "version": "5.21.1", 86 | "isJar": false, 87 | "skipInvalidPlatforms": true, 88 | "validPlatforms": [ 89 | "windowsx86-64", 90 | "linuxx86-64", 91 | "osxx86-64" 92 | ] 93 | }, 94 | { 95 | "groupId": "com.ctre.phoenix.sim", 96 | "artifactId": "simCANCoder", 97 | "version": "5.21.1", 98 | "isJar": false, 99 | "skipInvalidPlatforms": true, 100 | "validPlatforms": [ 101 | "windowsx86-64", 102 | "linuxx86-64", 103 | "osxx86-64" 104 | ] 105 | } 106 | ], 107 | "cppDependencies": [ 108 | { 109 | "groupId": "com.ctre.phoenix", 110 | "artifactId": "wpiapi-cpp", 111 | "version": "5.21.1", 112 | "libName": "CTRE_Phoenix_WPI", 113 | "headerClassifier": "headers", 114 | "sharedLibrary": true, 115 | "skipInvalidPlatforms": true, 116 | "binaryPlatforms": [ 117 | "linuxathena" 118 | ] 119 | }, 120 | { 121 | "groupId": "com.ctre.phoenix", 122 | "artifactId": "api-cpp", 123 | "version": "5.21.1", 124 | "libName": "CTRE_Phoenix", 125 | "headerClassifier": "headers", 126 | "sharedLibrary": true, 127 | "skipInvalidPlatforms": true, 128 | "binaryPlatforms": [ 129 | "linuxathena" 130 | ] 131 | }, 132 | { 133 | "groupId": "com.ctre.phoenix", 134 | "artifactId": "cci", 135 | "version": "5.21.1", 136 | "libName": "CTRE_PhoenixCCI", 137 | "headerClassifier": "headers", 138 | "sharedLibrary": true, 139 | "skipInvalidPlatforms": true, 140 | "binaryPlatforms": [ 141 | "linuxathena" 142 | ] 143 | }, 144 | { 145 | "groupId": "com.ctre.phoenix.sim", 146 | "artifactId": "wpiapi-cpp-sim", 147 | "version": "5.21.1", 148 | "libName": "CTRE_Phoenix_WPISim", 149 | "headerClassifier": "headers", 150 | "sharedLibrary": true, 151 | "skipInvalidPlatforms": true, 152 | "binaryPlatforms": [ 153 | "windowsx86-64", 154 | "linuxx86-64", 155 | "osxx86-64" 156 | ] 157 | }, 158 | { 159 | "groupId": "com.ctre.phoenix.sim", 160 | "artifactId": "api-cpp-sim", 161 | "version": "5.21.1", 162 | "libName": "CTRE_PhoenixSim", 163 | "headerClassifier": "headers", 164 | "sharedLibrary": true, 165 | "skipInvalidPlatforms": true, 166 | "binaryPlatforms": [ 167 | "windowsx86-64", 168 | "linuxx86-64", 169 | "osxx86-64" 170 | ] 171 | }, 172 | { 173 | "groupId": "com.ctre.phoenix.sim", 174 | "artifactId": "cci-sim", 175 | "version": "5.21.1", 176 | "libName": "CTRE_PhoenixCCISim", 177 | "headerClassifier": "headers", 178 | "sharedLibrary": true, 179 | "skipInvalidPlatforms": true, 180 | "binaryPlatforms": [ 181 | "windowsx86-64", 182 | "linuxx86-64", 183 | "osxx86-64" 184 | ] 185 | }, 186 | { 187 | "groupId": "com.ctre.phoenix.sim", 188 | "artifactId": "simTalonSRX", 189 | "version": "5.21.1", 190 | "libName": "CTRE_SimTalonSRX", 191 | "headerClassifier": "headers", 192 | "sharedLibrary": true, 193 | "skipInvalidPlatforms": true, 194 | "binaryPlatforms": [ 195 | "windowsx86-64", 196 | "linuxx86-64", 197 | "osxx86-64" 198 | ] 199 | }, 200 | { 201 | "groupId": "com.ctre.phoenix.sim", 202 | "artifactId": "simTalonFX", 203 | "version": "5.21.1", 204 | "libName": "CTRE_SimTalonFX", 205 | "headerClassifier": "headers", 206 | "sharedLibrary": true, 207 | "skipInvalidPlatforms": true, 208 | "binaryPlatforms": [ 209 | "windowsx86-64", 210 | "linuxx86-64", 211 | "osxx86-64" 212 | ] 213 | }, 214 | { 215 | "groupId": "com.ctre.phoenix.sim", 216 | "artifactId": "simVictorSPX", 217 | "version": "5.21.1", 218 | "libName": "CTRE_SimVictorSPX", 219 | "headerClassifier": "headers", 220 | "sharedLibrary": true, 221 | "skipInvalidPlatforms": true, 222 | "binaryPlatforms": [ 223 | "windowsx86-64", 224 | "linuxx86-64", 225 | "osxx86-64" 226 | ] 227 | }, 228 | { 229 | "groupId": "com.ctre.phoenix.sim", 230 | "artifactId": "simPigeonIMU", 231 | "version": "5.21.1", 232 | "libName": "CTRE_SimPigeonIMU", 233 | "headerClassifier": "headers", 234 | "sharedLibrary": true, 235 | "skipInvalidPlatforms": true, 236 | "binaryPlatforms": [ 237 | "windowsx86-64", 238 | "linuxx86-64", 239 | "osxx86-64" 240 | ] 241 | }, 242 | { 243 | "groupId": "com.ctre.phoenix.sim", 244 | "artifactId": "simCANCoder", 245 | "version": "5.21.1", 246 | "libName": "CTRE_SimCANCoder", 247 | "headerClassifier": "headers", 248 | "sharedLibrary": true, 249 | "skipInvalidPlatforms": true, 250 | "binaryPlatforms": [ 251 | "windowsx86-64", 252 | "linuxx86-64", 253 | "osxx86-64" 254 | ] 255 | } 256 | ] 257 | } -------------------------------------------------------------------------------- /src/main/java/frc/wpiClasses/SwerveModuleSim.java: -------------------------------------------------------------------------------- 1 | package frc.wpiClasses; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.system.plant.DCMotor; 6 | 7 | public class SwerveModuleSim { 8 | 9 | private SimpleMotorWithMassModel m_azmthMotor; 10 | private MotorGearboxWheelSim m_wheelMotor; 11 | 12 | private final double m_azimuthEncGearRatio; //Motor-to-azimuth-encoder reduction 13 | private final double m_wheelEncGearRatio; //Motor-to-wheel-encoder reduction 14 | private final double m_treadStaticFricForce; 15 | private final double m_treadKineticFricForce; 16 | //TODO - make the "how much grease" factor configurable? 17 | private final double m_wheelGearboxLossFactor = 0.01; 18 | 19 | Pose2d m_prevModulePose = null; 20 | Pose2d m_curModulePose = null; 21 | //Positive = in curAngle_deg, Negative = opposite of curAngle_deg 22 | double m_curLinearSpeedMpS = 0; 23 | //0 = toward front, 90 = toward left, 180 = toward back, 270 = toward right 24 | Rotation2d m_curAzmthAngle = Rotation2d.fromDegrees(0); 25 | 26 | double m_crossTreadFricForceMag = 0; 27 | double m_crossTreadVelMag = 0; 28 | double m_crossTreadForceMag = 0; 29 | 30 | double m_wheelVoltage; 31 | double m_azmthVoltage; 32 | 33 | /** 34 | * Simulates a single swerve module's behavior. 35 | * @param azimuthMotor Motor driving the azimuth angle portion of the mechanism 36 | * @param wheelMotor Motor driving the wheel to rotate 37 | * @param wheelRadiusM Wheel radius, in meters 38 | * @param azimuthGearRatio Motor rotations per one azimuth module 39 | * rotation. Should be greater than zero 40 | * @param wheelGearRatio Motor rotations per one wheel rotation. 41 | * Should be greater than zero 42 | * @param azimuthEncGearRatio Encoder rotations per one azimuth module rotation. 43 | * Should be 1.0 if you have a good swerve module. 44 | * @param wheelEncGearRatio Encoder rotations per one wheel rotation. 45 | * @param treadStaticCoefFric Static coefficient of friction of the wheel 46 | * tread and ground interface 47 | * @param treadKineticCoefFric Kinetic coefficient of friction of the wheel 48 | * tread and ground interface 49 | * @param moduleNormalForce Downward force the robot and its mechanisms exert on this module. 50 | * @param azimuthEffectiveMOI Effective moment of inertia of the azimuth module. 51 | * The Azimuth motion is modeled as a simple flywheel. 52 | */ 53 | public SwerveModuleSim( 54 | DCMotor azimuthMotor, 55 | DCMotor wheelMotor, 56 | double wheelRadiusM, 57 | double azimuthGearRatio, 58 | double wheelGearRatio, 59 | double azimuthEncGearRatio, 60 | double wheelEncGearRatio, 61 | double treadStaticCoefFric, 62 | double treadKineticCoefFric, 63 | double moduleNormalForce, 64 | double azimuthEffectiveMOI 65 | ) { 66 | this.m_azmthMotor = new SimpleMotorWithMassModel(azimuthMotor, 67 | azimuthGearRatio, 68 | azimuthEffectiveMOI); 69 | this.m_wheelMotor = new MotorGearboxWheelSim(wheelMotor, 70 | wheelGearRatio, 71 | wheelRadiusM, 72 | m_wheelGearboxLossFactor); 73 | 74 | this.m_azimuthEncGearRatio = azimuthEncGearRatio; 75 | this.m_wheelEncGearRatio = wheelEncGearRatio; 76 | this.m_treadStaticFricForce = treadStaticCoefFric * moduleNormalForce; 77 | this.m_treadKineticFricForce = treadKineticCoefFric * moduleNormalForce; 78 | 79 | this.reset(new Pose2d()); 80 | } 81 | 82 | /** 83 | * Sets the voltages applied to each motor in the mechanism 84 | * @param wheelVoltage voltage applied to the wheel motor 85 | * @param azmthVoltage voltage applied to the azimuth motor 86 | */ 87 | public void setInputVoltages(double wheelVoltage, double azmthVoltage) { 88 | this.m_wheelVoltage = wheelVoltage; 89 | this.m_azmthVoltage = azmthVoltage; 90 | } 91 | 92 | /** 93 | * @return Returns the current physical rotation count of the azimuth encoder axle 94 | * shaft in total rotations since last reset - not wrapped. 95 | * IE, if you spin your module exactly two and a half times in the 96 | * positive direction, this will return 2.5 (not 0.5). 97 | */ 98 | public double getAzimuthEncoderPositionRev() { 99 | return m_azmthMotor.getMechanismPositionRev() * m_azimuthEncGearRatio; 100 | } 101 | 102 | /** 103 | * @return Returns the total number of physical rotations the wheel has done 104 | * since the last reset - not wrapped. IE, if your wheel rotated through 105 | * 20.5 rotations, this function would return 20.5 (not 0.5). 106 | */ 107 | public double getWheelEncoderPositionRev() { 108 | return m_wheelMotor.getPositionRev() * m_wheelEncGearRatio; 109 | } 110 | 111 | public double getAzimuthMotorPositionRev(){ 112 | return m_azmthMotor.getMechanismPositionRev(); 113 | } 114 | 115 | public double getAzimuthMotorVelocityRPM(){ 116 | return m_azmthMotor.getMechanismSpeed_RPM(); 117 | } 118 | 119 | public double getAzimuthEncoderVelocityRPM(){ 120 | return m_azmthMotor.getMechanismSpeed_RPM() * m_azimuthEncGearRatio; 121 | } 122 | 123 | public double getWheelEncoderVelocityRPM(){ 124 | return m_azmthMotor.getMechanismSpeed_RPM() * m_wheelEncGearRatio; 125 | } 126 | 127 | void reset(Pose2d initModulePose) { 128 | m_prevModulePose = m_curModulePose = initModulePose; 129 | m_curLinearSpeedMpS = 0; 130 | m_curAzmthAngle = Rotation2d.fromDegrees(0); 131 | } 132 | 133 | void update(double dtSeconds) { 134 | 135 | Vector2d azimuthUnitVec = new Vector2d(1 , 0); 136 | azimuthUnitVec.rotate(m_curAzmthAngle.getDegrees()); 137 | 138 | // Assume the wheel does not lose traction along its wheel direction (on-tread) 139 | double velocityAlongAzimuth = getModRelTransVel(dtSeconds).dot(azimuthUnitVec); 140 | 141 | m_wheelMotor.update(velocityAlongAzimuth, m_wheelVoltage, dtSeconds); 142 | m_azmthMotor.update(m_azmthVoltage, dtSeconds); 143 | 144 | // Assume idealized azimuth control - no "twist" force at contact 145 | // patch from friction or robot motion. 146 | m_curAzmthAngle = Rotation2d.fromDegrees(m_azmthMotor.getMechanismPositionRev() * 360); 147 | } 148 | 149 | 150 | /** Get a vector of the velocity of the module's contact patch moving across the field. */ 151 | Vector2d getModRelTransVel(double dtSeconds) { 152 | double xvel = (m_curModulePose.getTranslation().getX() 153 | - m_prevModulePose.getTranslation().getX()) / dtSeconds; 154 | double yvel = (m_curModulePose.getTranslation().getY() 155 | - m_prevModulePose.getTranslation().getY()) / dtSeconds; 156 | Vector2d moduleTranslationVec = new Vector2d(xvel , yvel); 157 | moduleTranslationVec.rotate(-1.0 * m_curModulePose.getRotation().getDegrees()); 158 | return moduleTranslationVec; 159 | } 160 | 161 | /** 162 | * Given a net force on a particular module, calculate the friction force 163 | * generated by the tread interacting with the ground in the direction 164 | * perpendicular to the wheel's rotation. 165 | * @param netForce input force applied to the module 166 | */ 167 | ForceAtPose2d getCrossTreadFricForce(Force2d netForce, double dtSeconds) { 168 | 169 | //Project net force onto cross-tread vector 170 | Vector2d crossTreadUnitVector = new Vector2d(0 , 1); 171 | crossTreadUnitVector.rotate(m_curAzmthAngle.getDegrees()); 172 | m_crossTreadVelMag = getModRelTransVel(dtSeconds).dot(crossTreadUnitVector); 173 | m_crossTreadForceMag = netForce.getVector2d().dot(crossTreadUnitVector); 174 | 175 | Force2d fricForce = new Force2d(); 176 | 177 | boolean useKinFric = Math.abs(m_crossTreadForceMag) > m_treadStaticFricForce 178 | || Math.abs(m_crossTreadVelMag) > 0.001; 179 | 180 | if (useKinFric) { 181 | // Force is great enough to overcome static friction, or we're already moving 182 | // In either case, use kinetic frictional model 183 | m_crossTreadFricForceMag = -1.0 * Math.signum(m_crossTreadVelMag) 184 | * m_treadKineticFricForce; 185 | } else { 186 | // Static Friction Model 187 | m_crossTreadFricForceMag = -1.0 * m_crossTreadForceMag; 188 | } 189 | 190 | fricForce = new Force2d(crossTreadUnitVector); 191 | fricForce = fricForce.times(m_crossTreadFricForceMag); 192 | 193 | return new ForceAtPose2d(fricForce, m_curModulePose); 194 | } 195 | 196 | 197 | /** Gets the modules on-axis (along wheel direction) force, 198 | * which comes from the rotation of the motor. */ 199 | ForceAtPose2d getWheelMotiveForce() { 200 | return new ForceAtPose2d(new Force2d(m_wheelMotor.getGroundForce_N(), m_curAzmthAngle), 201 | m_curModulePose); 202 | } 203 | 204 | /** Set the motion of each module in the field reference frame. */ 205 | void setModulePose(Pose2d curPos) { 206 | //Handle init'ing module position history to current on first pass 207 | if (m_prevModulePose == null) { 208 | m_prevModulePose = curPos; 209 | } else { 210 | m_prevModulePose = m_curModulePose; 211 | } 212 | 213 | m_curModulePose = curPos; 214 | } 215 | 216 | Pose2d getPose() { 217 | return m_curModulePose; 218 | } 219 | 220 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib.ctre; 2 | 3 | import com.ctre.phoenix.motorcontrol.*; 4 | import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; 5 | import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 6 | 7 | import frc.swervelib.*; 8 | import edu.wpi.first.math.system.plant.DCMotor; 9 | import edu.wpi.first.wpilibj.RobotBase; 10 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; 11 | 12 | public final class Falcon500SteerControllerFactoryBuilder { 13 | private static final int CAN_TIMEOUT_MS = 250; 14 | private static final int STATUS_FRAME_GENERAL_PERIOD_MS = 250; 15 | 16 | private static final double TICKS_PER_ROTATION = 2048.0; 17 | 18 | // PID configuration 19 | private double proportionalConstant = Double.NaN; 20 | private double integralConstant = Double.NaN; 21 | private double derivativeConstant = Double.NaN; 22 | 23 | // Motion magic configuration 24 | private double velocityConstant = Double.NaN; 25 | private double accelerationConstant = Double.NaN; 26 | private double staticConstant = Double.NaN; 27 | 28 | private double nominalVoltage = Double.NaN; 29 | private double currentLimit = Double.NaN; 30 | 31 | public Falcon500SteerControllerFactoryBuilder withPidConstants(double proportional, double integral, double derivative) { 32 | this.proportionalConstant = proportional; 33 | this.integralConstant = integral; 34 | this.derivativeConstant = derivative; 35 | return this; 36 | } 37 | 38 | public boolean hasPidConstants() { 39 | return Double.isFinite(proportionalConstant) && Double.isFinite(integralConstant) && Double.isFinite(derivativeConstant); 40 | } 41 | 42 | public Falcon500SteerControllerFactoryBuilder withMotionMagic(double velocityConstant, double accelerationConstant, double staticConstant) { 43 | this.velocityConstant = velocityConstant; 44 | this.accelerationConstant = accelerationConstant; 45 | this.staticConstant = staticConstant; 46 | return this; 47 | } 48 | 49 | public boolean hasMotionMagic() { 50 | return Double.isFinite(velocityConstant) && Double.isFinite(accelerationConstant) && Double.isFinite(staticConstant); 51 | } 52 | 53 | public Falcon500SteerControllerFactoryBuilder withVoltageCompensation(double nominalVoltage) { 54 | this.nominalVoltage = nominalVoltage; 55 | return this; 56 | } 57 | 58 | public boolean hasVoltageCompensation() { 59 | return Double.isFinite(nominalVoltage); 60 | } 61 | 62 | public Falcon500SteerControllerFactoryBuilder withCurrentLimit(double currentLimit) { 63 | this.currentLimit = currentLimit; 64 | return this; 65 | } 66 | 67 | public boolean hasCurrentLimit() { 68 | return Double.isFinite(currentLimit); 69 | } 70 | 71 | public SteerControllerFactory> build(AbsoluteEncoderFactory absoluteEncoderFactory) { 72 | return new FactoryImplementation<>(absoluteEncoderFactory); 73 | } 74 | 75 | private class FactoryImplementation implements SteerControllerFactory> { 76 | private final AbsoluteEncoderFactory encoderFactory; 77 | 78 | private FactoryImplementation(AbsoluteEncoderFactory encoderFactory) { 79 | this.encoderFactory = encoderFactory; 80 | } 81 | 82 | @Override 83 | public void addDashboardEntries(ShuffleboardContainer container, ControllerImplementation controller) { 84 | SteerControllerFactory.super.addDashboardEntries(container, controller); 85 | container.addNumber("Absolute Encoder Angle", () -> Math.toDegrees(controller.absoluteEncoder.getAbsoluteAngle())); 86 | } 87 | 88 | @Override 89 | public ControllerImplementation create(Falcon500SteerConfiguration steerConfiguration, ModuleConfiguration moduleConfiguration) { 90 | AbsoluteEncoder absoluteEncoder = encoderFactory.create(steerConfiguration.getEncoderConfiguration()); 91 | 92 | final double sensorPositionCoefficient = 2.0 * Math.PI / TICKS_PER_ROTATION * moduleConfiguration.getSteerReduction(); 93 | final double sensorVelocityCoefficient = sensorPositionCoefficient * 10.0; 94 | 95 | TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); 96 | if (hasPidConstants()) { 97 | motorConfiguration.slot0.kP = proportionalConstant; 98 | motorConfiguration.slot0.kI = integralConstant; 99 | motorConfiguration.slot0.kD = derivativeConstant; 100 | } 101 | if (hasMotionMagic()) { 102 | if (hasVoltageCompensation()) { 103 | motorConfiguration.slot0.kF = (1023.0 * sensorVelocityCoefficient / nominalVoltage) * velocityConstant; 104 | } 105 | // TODO: What should be done if no nominal voltage is configured? Use a default voltage? 106 | 107 | // TODO: Make motion magic max voltages configurable or dynamically determine optimal values 108 | motorConfiguration.motionCruiseVelocity = 2.0 / velocityConstant / sensorVelocityCoefficient; 109 | motorConfiguration.motionAcceleration = (8.0 - 2.0) / accelerationConstant / sensorVelocityCoefficient; 110 | } 111 | if (hasVoltageCompensation()) { 112 | motorConfiguration.voltageCompSaturation = nominalVoltage; 113 | } 114 | if (hasCurrentLimit()) { 115 | motorConfiguration.supplyCurrLimit.currentLimit = currentLimit; 116 | motorConfiguration.supplyCurrLimit.enable = true; 117 | } 118 | 119 | WPI_TalonFX motor = new WPI_TalonFX(steerConfiguration.getMotorPort()); 120 | motor.configAllSettings(motorConfiguration, CAN_TIMEOUT_MS); 121 | 122 | if (hasVoltageCompensation()) { 123 | motor.enableVoltageCompensation(true); 124 | } 125 | motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, CAN_TIMEOUT_MS); 126 | motor.setSensorPhase(true); 127 | motor.setInverted(moduleConfiguration.isSteerInverted() ? TalonFXInvertType.CounterClockwise : TalonFXInvertType.Clockwise); 128 | motor.setNeutralMode(NeutralMode.Brake); 129 | 130 | motor.setSelectedSensorPosition(absoluteEncoder.getAbsoluteAngleRetry() / sensorPositionCoefficient, 0, CAN_TIMEOUT_MS); 131 | 132 | // Reduce CAN status frame rates on real robots 133 | // Don't do this in simulation, or it causes lag and quantization of the voltage 134 | // signals which cause the sim model to be inaccurate and unstable. 135 | motor.setStatusFramePeriod( 136 | StatusFrameEnhanced.Status_1_General, 137 | RobotBase.isSimulation()?20:STATUS_FRAME_GENERAL_PERIOD_MS, 138 | CAN_TIMEOUT_MS 139 | ); 140 | 141 | return new ControllerImplementation(motor, 142 | sensorPositionCoefficient, 143 | sensorVelocityCoefficient, 144 | hasMotionMagic() ? TalonFXControlMode.MotionMagic : TalonFXControlMode.Position, 145 | absoluteEncoder); 146 | } 147 | } 148 | 149 | private static class ControllerImplementation implements SteerController { 150 | private final WPI_TalonFX motor; 151 | private final double motorEncoderPositionCoefficient; 152 | private final TalonFXControlMode motorControlMode; 153 | public final AbsoluteEncoder absoluteEncoder; 154 | 155 | private double referenceAngleRadians = 0.0; 156 | 157 | private ControllerImplementation(WPI_TalonFX motor, 158 | double motorEncoderPositionCoefficient, 159 | double motorEncoderVelocityCoefficient, 160 | TalonFXControlMode motorControlMode, 161 | AbsoluteEncoder absoluteEncoder) { 162 | this.motor = motor; 163 | this.motorEncoderPositionCoefficient = motorEncoderPositionCoefficient; 164 | this.motorControlMode = motorControlMode; 165 | this.absoluteEncoder = absoluteEncoder; 166 | } 167 | 168 | @Override 169 | public double getReferenceAngle() { 170 | return referenceAngleRadians; 171 | } 172 | 173 | @Override 174 | public void setReferenceAngle(double referenceAngleRadians) { 175 | double currentAngleRadians = motor.getSelectedSensorPosition() * motorEncoderPositionCoefficient; 176 | 177 | double currentAngleRadiansMod = currentAngleRadians % (2.0 * Math.PI); 178 | if (currentAngleRadiansMod < 0.0) { 179 | currentAngleRadiansMod += 2.0 * Math.PI; 180 | } 181 | 182 | // The reference angle has the range [0, 2pi) but the Falcon's encoder can go above that 183 | double adjustedReferenceAngleRadians = referenceAngleRadians + currentAngleRadians - currentAngleRadiansMod; 184 | if (referenceAngleRadians - currentAngleRadiansMod > Math.PI) { 185 | adjustedReferenceAngleRadians -= 2.0 * Math.PI; 186 | } else if (referenceAngleRadians - currentAngleRadiansMod < -Math.PI) { 187 | adjustedReferenceAngleRadians += 2.0 * Math.PI; 188 | } 189 | 190 | motor.set(motorControlMode, adjustedReferenceAngleRadians / motorEncoderPositionCoefficient); 191 | 192 | 193 | this.referenceAngleRadians = referenceAngleRadians; 194 | } 195 | 196 | @Override 197 | public void setSteerEncoder(double position, double velocity) { 198 | // Position is in revolutions. Velocity is in RPM 199 | // TalonFX wants steps for postion. Steps per 100ms for velocity. Falcon integrated encoder has 2048 CPR. 200 | motor.getSimCollection().setIntegratedSensorRawPosition((int) (position * 2048)); 201 | // Divide by 600 to go from RPM to Rotations per 100ms. Multiply by encoder ticks per revolution. 202 | motor.getSimCollection().setIntegratedSensorVelocity((int) (velocity / 600 * 2048)); 203 | } 204 | 205 | @Override 206 | public double getStateAngle() { 207 | double motorAngleRadians = motor.getSelectedSensorPosition() * motorEncoderPositionCoefficient; 208 | motorAngleRadians %= 2.0 * Math.PI; 209 | if (motorAngleRadians < 0.0) { 210 | motorAngleRadians += 2.0 * Math.PI; 211 | } 212 | 213 | return motorAngleRadians; 214 | } 215 | 216 | @Override 217 | public DCMotor getSteerMotor() { 218 | return DCMotor.getFalcon500(1); 219 | } 220 | 221 | @Override 222 | public AbsoluteEncoder getAbsoluteEncoder() { 223 | return absoluteEncoder; 224 | } 225 | 226 | @Override 227 | public double getOutputVoltage() { 228 | return motor.getMotorOutputVoltage(); 229 | } 230 | } 231 | } 232 | -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/SwerveDrivetrainModel.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import java.util.ArrayList; 4 | 5 | import com.pathplanner.lib.PathPlannerTrajectory; 6 | import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; 7 | import com.pathplanner.lib.commands.PPSwerveControllerCommand; 8 | 9 | import edu.wpi.first.math.VecBuilder; 10 | import edu.wpi.first.math.controller.HolonomicDriveController; 11 | import edu.wpi.first.math.controller.ProfiledPIDController; 12 | import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 13 | import edu.wpi.first.math.geometry.Pose2d; 14 | import edu.wpi.first.math.geometry.Rotation2d; 15 | import edu.wpi.first.math.geometry.Transform2d; 16 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 17 | import edu.wpi.first.math.kinematics.SwerveModuleState; 18 | import edu.wpi.first.math.util.Units; 19 | import edu.wpi.first.wpilibj.RobotBase; 20 | import edu.wpi.first.wpilibj.Timer; 21 | import edu.wpi.first.wpilibj.smartdashboard.Field2d; 22 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 23 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 24 | import edu.wpi.first.wpilibj2.command.Command; 25 | import frc.wpiClasses.QuadSwerveSim; 26 | import frc.wpiClasses.SwerveModuleSim; 27 | 28 | public class SwerveDrivetrainModel { 29 | 30 | QuadSwerveSim swerveDt; 31 | ArrayList realModules = new ArrayList(QuadSwerveSim.NUM_MODULES); 32 | ArrayList modules = new ArrayList(QuadSwerveSim.NUM_MODULES); 33 | 34 | ArrayList steerMotorControllers = new ArrayList(QuadSwerveSim.NUM_MODULES); 35 | ArrayList driveMotorControllers = new ArrayList(QuadSwerveSim.NUM_MODULES); 36 | ArrayList steerEncoders = new ArrayList(QuadSwerveSim.NUM_MODULES); 37 | 38 | Gyroscope gyro; 39 | 40 | Pose2d endPose; 41 | PoseTelemetry dtPoseView; 42 | 43 | SwerveDrivePoseEstimator m_poseEstimator; 44 | Pose2d curEstPose = new Pose2d(SwerveConstants.DFLT_START_POSE.getTranslation(), SwerveConstants.DFLT_START_POSE.getRotation()); 45 | Pose2d fieldPose = new Pose2d(); // Field-referenced orign 46 | boolean pointedDownfield = false; 47 | double curSpeed = 0; 48 | SwerveModuleState[] states; 49 | ProfiledPIDController thetaController = 50 | new ProfiledPIDController( 51 | SwerveConstants.THETACONTROLLERkP, 0, 0, SwerveConstants.THETACONTROLLERCONSTRAINTS); 52 | 53 | HolonomicDriveController m_holo; 54 | 55 | private static final SendableChooser orientationChooser = new SendableChooser<>(); 56 | 57 | private double forwardSlow = 1.0; 58 | private double strafeSlow = 1.0; 59 | private double rotateSlow = 1.0; 60 | 61 | public SwerveDrivetrainModel(ArrayList realModules, Gyroscope gyro){ 62 | this.gyro = gyro; 63 | this.realModules = realModules; 64 | 65 | if (RobotBase.isSimulation()) { 66 | modules.add(Mk4SwerveModuleHelper.createSim(realModules.get(0))); 67 | modules.add(Mk4SwerveModuleHelper.createSim(realModules.get(1))); 68 | modules.add(Mk4SwerveModuleHelper.createSim(realModules.get(2))); 69 | modules.add(Mk4SwerveModuleHelper.createSim(realModules.get(3))); 70 | } 71 | 72 | thetaController.enableContinuousInput(-Math.PI, Math.PI); 73 | 74 | endPose = SwerveConstants.DFLT_START_POSE; 75 | 76 | swerveDt = new QuadSwerveSim(SwerveConstants.TRACKWIDTH_METERS, 77 | SwerveConstants.TRACKLENGTH_METERS, 78 | SwerveConstants.MASS_kg, 79 | SwerveConstants.MOI_KGM2, 80 | modules); 81 | 82 | // Trustworthiness of the internal model of how motors should be moving 83 | // Measured in expected standard deviation (meters of position and degrees of 84 | // rotation) 85 | var stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)); 86 | 87 | // Trustworthiness of gyro in radians of standard deviation. 88 | var localMeasurementStdDevs = VecBuilder.fill(Units.degreesToRadians(0.1)); 89 | 90 | // Trustworthiness of the vision system 91 | // Measured in expected standard deviation (meters of position and degrees of 92 | // rotation) 93 | var visionMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); 94 | 95 | m_poseEstimator = new SwerveDrivePoseEstimator(getGyroscopeRotation(), SwerveConstants.DFLT_START_POSE, 96 | SwerveConstants.KINEMATICS, stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs, 97 | SimConstants.CTRLS_SAMPLE_RATE_SEC); 98 | 99 | setKnownPose(SwerveConstants.DFLT_START_POSE); 100 | 101 | dtPoseView = new PoseTelemetry(swerveDt, m_poseEstimator); 102 | 103 | // Control Orientation Chooser 104 | orientationChooser.setDefaultOption("Field Oriented", "Field Oriented"); 105 | orientationChooser.addOption("Robot Oriented", "Robot Oriented"); 106 | SmartDashboard.putData("Orientation Chooser", orientationChooser); 107 | 108 | m_holo = new HolonomicDriveController(SwerveConstants.XPIDCONTROLLER, SwerveConstants.YPIDCONTROLLER, thetaController); 109 | } 110 | 111 | /** 112 | * Handles discontinuous jumps in robot pose. Used at the start of 113 | * autonomous, if the user manually drags the robot across the field in the 114 | * Field2d widget, or something similar to that. 115 | * @param pose The new pose the robot is "jumping" to. 116 | */ 117 | public void modelReset(Pose2d pose){ 118 | swerveDt.modelReset(pose); 119 | } 120 | 121 | /** 122 | * Advance the simulation forward by one step 123 | * @param isDisabled Boolean that indicates if the robot is in the disabled mode 124 | * @param batteryVoltage Amount of voltage available to the drivetrain at the current step. 125 | */ 126 | public void update(boolean isDisabled, double batteryVoltage){ 127 | // Calculate and update input voltages to each motor. 128 | if(isDisabled){ 129 | for(int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++){ 130 | modules.get(idx).setInputVoltages(0.0, 0.0); 131 | } 132 | } else { 133 | for(int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++){ 134 | double steerVolts = realModules.get(idx).getSteerController().getOutputVoltage(); 135 | double wheelVolts = realModules.get(idx).getDriveController().getOutputVoltage(); 136 | modules.get(idx).setInputVoltages(wheelVolts, steerVolts); 137 | } 138 | } 139 | 140 | //Update the main drivetrain plant model 141 | swerveDt.update(SimConstants.SIM_SAMPLE_RATE_SEC); 142 | 143 | // Update each encoder 144 | for(int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++){ 145 | double azmthShaftPos = modules.get(idx).getAzimuthEncoderPositionRev(); 146 | double steerMotorPos = modules.get(idx).getAzimuthMotorPositionRev(); 147 | double wheelPos = modules.get(idx).getWheelEncoderPositionRev(); 148 | 149 | double azmthShaftVel = modules.get(idx).getAzimuthEncoderVelocityRPM(); 150 | double steerVelocity = modules.get(idx).getAzimuthMotorVelocityRPM(); 151 | double wheelVelocity = modules.get(idx).getWheelEncoderVelocityRPM(); 152 | 153 | realModules.get(idx).getAbsoluteEncoder().setAbsoluteEncoder(azmthShaftPos, azmthShaftVel); 154 | realModules.get(idx).getSteerController().setSteerEncoder(steerMotorPos, steerVelocity); 155 | realModules.get(idx).getDriveController().setDriveEncoder(wheelPos, wheelVelocity); 156 | } 157 | 158 | // Update associated devices based on drivetrain motion 159 | gyro.setAngle(-swerveDt.getCurPose().getRotation().getDegrees()); 160 | 161 | // Based on gyro and measured module speeds and positions, estimate where our 162 | // robot should have moved to. 163 | Pose2d prevEstPose = curEstPose; 164 | if (states != null) { 165 | curEstPose = m_poseEstimator.getEstimatedPosition(); 166 | 167 | // Calculate a "speedometer" velocity in ft/sec 168 | Transform2d chngPose = new Transform2d(prevEstPose, curEstPose); 169 | curSpeed = Units.metersToFeet(chngPose.getTranslation().getNorm()) / SimConstants.CTRLS_SAMPLE_RATE_SEC; 170 | } 171 | } 172 | 173 | /** 174 | * Sets the swerve ModuleStates. 175 | * 176 | * @param desiredStates The desired SwerveModule states. 177 | */ 178 | public void setModuleStates(SwerveModuleState[] desiredStates) { 179 | states = desiredStates; 180 | } 181 | 182 | /** 183 | * Sets the swerve ModuleStates. 184 | * 185 | * @param chassisSpeeds The desired SwerveModule states. 186 | */ 187 | public void setModuleStates(ChassisSpeeds chassisSpeeds) { 188 | states = SwerveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds); 189 | } 190 | 191 | public void setModuleStates(SwerveInput input) { 192 | input = handleStationary(input); 193 | 194 | switch (orientationChooser.getSelected()) { 195 | case "Field Oriented": 196 | states = SwerveConstants.KINEMATICS.toSwerveModuleStates( 197 | ChassisSpeeds.fromFieldRelativeSpeeds( 198 | input.m_translationX * SwerveConstants.MAX_FWD_REV_SPEED_MPS * forwardSlow, 199 | input.m_translationY * SwerveConstants.MAX_STRAFE_SPEED_MPS * strafeSlow, 200 | input.m_rotation * SwerveConstants.MAX_ROTATE_SPEED_RAD_PER_SEC * rotateSlow, 201 | getGyroscopeRotation() 202 | ) 203 | ); 204 | break; 205 | case "Robot Oriented": 206 | states = SwerveConstants.KINEMATICS.toSwerveModuleStates( 207 | new ChassisSpeeds( 208 | input.m_translationX * SwerveConstants.MAX_FWD_REV_SPEED_MPS * forwardSlow, 209 | input.m_translationY * SwerveConstants.MAX_STRAFE_SPEED_MPS * strafeSlow, 210 | input.m_rotation * SwerveConstants.MAX_ROTATE_SPEED_RAD_PER_SEC * rotateSlow 211 | ) 212 | ); 213 | break; 214 | } 215 | } 216 | 217 | public SwerveModuleState[] getSwerveModuleStates() { 218 | return states; 219 | } 220 | 221 | public Pose2d getPose(){ 222 | return m_poseEstimator.getEstimatedPosition(); 223 | } 224 | 225 | public void setKnownPose(Pose2d in) { 226 | resetWheelEncoders(); 227 | gyro.zeroGyroscope(in.getRotation().getDegrees()); 228 | m_poseEstimator.resetPosition(in, getGyroscopeRotation()); 229 | curEstPose = in; 230 | } 231 | 232 | public void setKnownState(PathPlannerState initialState) { 233 | Pose2d startingPose = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); 234 | setKnownPose(startingPose); 235 | } 236 | 237 | public void zeroGyroscope() { 238 | gyro.zeroGyroscope(0.0); 239 | } 240 | 241 | public Rotation2d getGyroscopeRotation() { 242 | SmartDashboard.putNumber("Gyro Angle", gyro.getGyroHeading().getDegrees()); 243 | return gyro.getGyroHeading(); 244 | } 245 | 246 | public Boolean getGyroReady() { 247 | return gyro.getGyroReady(); 248 | } 249 | 250 | public void updateTelemetry(){ 251 | dtPoseView.update(Timer.getFPGATimestamp()*1000); 252 | } 253 | 254 | public Field2d getField() { 255 | return dtPoseView.getField(); 256 | } 257 | 258 | public void resetWheelEncoders() { 259 | for(int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++){ 260 | realModules.get(idx).resetWheelEncoder(); 261 | } 262 | } 263 | 264 | public Command createCommandForTrajectory(PathPlannerTrajectory trajectory, SwerveSubsystem m_drive) { 265 | PPSwerveControllerCommand swerveControllerCommand = 266 | new PPSwerveControllerCommand( 267 | trajectory, 268 | () -> getPose(), // Functional interface to feed supplier 269 | SwerveConstants.KINEMATICS, 270 | 271 | // Position controllers 272 | SwerveConstants.XPIDCONTROLLER, 273 | SwerveConstants.YPIDCONTROLLER, 274 | thetaController, 275 | commandStates -> this.states = commandStates, 276 | m_drive); 277 | return swerveControllerCommand.andThen(() -> setModuleStates(new SwerveInput(0,0,0))); 278 | } 279 | 280 | public ArrayList getRealModules() { 281 | return realModules; 282 | } 283 | 284 | public ArrayList getModules() { 285 | return modules; 286 | } 287 | 288 | public void goToPose(Pose2d desiredPose, double angle) { 289 | setModuleStates(m_holo.calculate(getPose(), desiredPose, 1, Rotation2d.fromDegrees(angle))); 290 | } 291 | 292 | public void setMaxSpeeds(double forwardSlow, double strafeSlow, double rotateSlow) { 293 | this.forwardSlow = forwardSlow; 294 | this.strafeSlow = strafeSlow; 295 | this.rotateSlow = rotateSlow; 296 | } 297 | 298 | private SwerveInput handleStationary(SwerveInput input) { 299 | if (input.m_rotation == 0 && input.m_translationX == 0 && input.m_translationY == 0) { 300 | // Hopefully this will turn all of the modules to the "turning" configuration so being pushed is more difficult 301 | input.m_rotation = 0.0; //001; 302 | } 303 | return input; 304 | } 305 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/Mk4iSwerveModuleHelper.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import frc.swervelib.ctre.*; 4 | import frc.swervelib.rev.NeoDriveControllerFactoryBuilder; 5 | import frc.swervelib.rev.NeoSteerConfiguration; 6 | import frc.swervelib.rev.NeoSteerControllerFactoryBuilder; 7 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; 8 | 9 | public final class Mk4iSwerveModuleHelper { 10 | private Mk4iSwerveModuleHelper() { 11 | } 12 | 13 | private static DriveControllerFactory getFalcon500DriveFactory(Mk4ModuleConfiguration configuration) { 14 | return new Falcon500DriveControllerFactoryBuilder() 15 | .withVoltageCompensation(configuration.getNominalVoltage()) 16 | .withCurrentLimit(configuration.getDriveCurrentLimit()) 17 | .build(); 18 | } 19 | 20 | private static SteerControllerFactory> getFalcon500SteerFactory(Mk4ModuleConfiguration configuration) { 21 | return new Falcon500SteerControllerFactoryBuilder() 22 | .withVoltageCompensation(configuration.getNominalVoltage()) 23 | .withPidConstants(0.2, 0.0, 0.1) 24 | .withCurrentLimit(configuration.getSteerCurrentLimit()) 25 | .build(new CanCoderFactoryBuilder() 26 | .withReadingUpdatePeriod(100) 27 | .build()); 28 | } 29 | 30 | private static DriveControllerFactory getNeoDriveFactory(Mk4ModuleConfiguration configuration) { 31 | return new NeoDriveControllerFactoryBuilder() 32 | .withVoltageCompensation(configuration.getNominalVoltage()) 33 | .withCurrentLimit(configuration.getDriveCurrentLimit()) 34 | .build(); 35 | } 36 | 37 | private static SteerControllerFactory> getNeoSteerFactory(Mk4ModuleConfiguration configuration) { 38 | return new NeoSteerControllerFactoryBuilder() 39 | .withVoltageCompensation(configuration.getNominalVoltage()) 40 | .withPidConstants(1.0, 0.0, 0.1) 41 | .withCurrentLimit(configuration.getSteerCurrentLimit()) 42 | .build(new CanCoderFactoryBuilder() 43 | .withReadingUpdatePeriod(100) 44 | .build()); 45 | } 46 | 47 | /** 48 | * Creates a Mk4i swerve module that uses Falcon 500s for driving and steering. 49 | * Module information is displayed in the specified ShuffleBoard container. 50 | * 51 | * @param container The container to display module information in. 52 | * @param configuration Module configuration parameters to use. 53 | * @param gearRatio The gearing configuration the module is in. 54 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 55 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 56 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 57 | * @param steerOffset The offset of the CANCoder in radians. 58 | * @return The configured swerve module. 59 | */ 60 | public static SwerveModule createFalcon500( 61 | ShuffleboardLayout container, 62 | Mk4ModuleConfiguration configuration, 63 | GearRatio gearRatio, 64 | int driveMotorPort, 65 | int steerMotorPort, 66 | int steerEncoderPort, 67 | double steerOffset, 68 | String namePrefix 69 | ) { 70 | return new SwerveModuleFactory<>( 71 | gearRatio.getConfiguration(), 72 | getFalcon500DriveFactory(configuration), 73 | getFalcon500SteerFactory(configuration) 74 | ).create( 75 | container, 76 | driveMotorPort, 77 | new Falcon500SteerConfiguration<>( 78 | steerMotorPort, 79 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 80 | ), 81 | namePrefix 82 | ); 83 | } 84 | 85 | /** 86 | * Creates a Mk4i swerve module that uses Falcon 500s for driving and steering. 87 | * Module information is displayed in the specified ShuffleBoard container. 88 | * 89 | * @param container The container to display module information in. 90 | * @param gearRatio The gearing configuration the module is in. 91 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 92 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 93 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 94 | * @param steerOffset The offset of the CANCoder in radians. 95 | * @return The configured swerve module. 96 | */ 97 | public static SwerveModule createFalcon500( 98 | ShuffleboardLayout container, 99 | GearRatio gearRatio, 100 | int driveMotorPort, 101 | int steerMotorPort, 102 | int steerEncoderPort, 103 | double steerOffset, 104 | String namePrefix 105 | ) { 106 | return createFalcon500(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 107 | } 108 | 109 | /** 110 | * Creates a Mk4i swerve module that uses Falcon 500s for driving and steering. 111 | * 112 | * @param configuration Module configuration parameters to use. 113 | * @param gearRatio The gearing configuration the module is in. 114 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 115 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 116 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 117 | * @param steerOffset The offset of the CANCoder in radians. 118 | * @return The configured swerve module. 119 | */ 120 | public static SwerveModule createFalcon500( 121 | Mk4ModuleConfiguration configuration, 122 | GearRatio gearRatio, 123 | int driveMotorPort, 124 | int steerMotorPort, 125 | int steerEncoderPort, 126 | double steerOffset, 127 | String namePrefix 128 | ) { 129 | return new SwerveModuleFactory<>( 130 | gearRatio.getConfiguration(), 131 | getFalcon500DriveFactory(configuration), 132 | getFalcon500SteerFactory(configuration) 133 | ).create( 134 | driveMotorPort, 135 | new Falcon500SteerConfiguration<>( 136 | steerMotorPort, 137 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 138 | ), 139 | namePrefix 140 | ); 141 | } 142 | 143 | /** 144 | * Creates a Mk4i swerve module that uses Falcon 500s for driving and steering. 145 | * 146 | * @param gearRatio The gearing configuration the module is in. 147 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 148 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 149 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 150 | * @param steerOffset The offset of the CANCoder in radians. 151 | * @return The configured swerve module. 152 | */ 153 | public static SwerveModule createFalcon500( 154 | GearRatio gearRatio, 155 | int driveMotorPort, 156 | int steerMotorPort, 157 | int steerEncoderPort, 158 | double steerOffset, 159 | String namePrefix 160 | ) { 161 | return createFalcon500(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 162 | } 163 | 164 | /** 165 | * Creates a Mk4i swerve module that uses NEOs for driving and steering. 166 | * Module information is displayed in the specified ShuffleBoard container. 167 | * 168 | * @param container The container to display module information in. 169 | * @param configuration Module configuration parameters to use. 170 | * @param gearRatio The gearing configuration the module is in. 171 | * @param driveMotorPort The CAN ID of the drive NEO. 172 | * @param steerMotorPort The CAN ID of the steer NEO. 173 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 174 | * @param steerOffset The offset of the CANCoder in radians. 175 | * @return The configured swerve module. 176 | */ 177 | public static SwerveModule createNeo( 178 | ShuffleboardLayout container, 179 | Mk4ModuleConfiguration configuration, 180 | GearRatio gearRatio, 181 | int driveMotorPort, 182 | int steerMotorPort, 183 | int steerEncoderPort, 184 | double steerOffset, 185 | String namePrefix 186 | ) { 187 | return new SwerveModuleFactory<>( 188 | gearRatio.getConfiguration(), 189 | getNeoDriveFactory(configuration), 190 | getNeoSteerFactory(configuration) 191 | ).create( 192 | container, 193 | driveMotorPort, 194 | new NeoSteerConfiguration<>( 195 | steerMotorPort, 196 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 197 | ), 198 | namePrefix 199 | ); 200 | } 201 | 202 | /** 203 | * Creates a Mk4i swerve module that uses NEOs for driving and steering. 204 | * Module information is displayed in the specified ShuffleBoard container. 205 | * 206 | * @param container The container to display module information in. 207 | * @param gearRatio The gearing configuration the module is in. 208 | * @param driveMotorPort The CAN ID of the drive NEO. 209 | * @param steerMotorPort The CAN ID of the steer NEO. 210 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 211 | * @param steerOffset The offset of the CANCoder in radians. 212 | * @return The configured swerve module. 213 | */ 214 | public static SwerveModule createNeo( 215 | ShuffleboardLayout container, 216 | GearRatio gearRatio, 217 | int driveMotorPort, 218 | int steerMotorPort, 219 | int steerEncoderPort, 220 | double steerOffset, 221 | String namePrefix 222 | ) { 223 | return createNeo(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 224 | } 225 | 226 | /** 227 | * Creates a Mk4i swerve module that uses NEOs for driving and steering. 228 | * 229 | * @param configuration Module configuration parameters to use. 230 | * @param gearRatio The gearing configuration the module is in. 231 | * @param driveMotorPort The CAN ID of the drive NEO. 232 | * @param steerMotorPort The CAN ID of the steer NEO. 233 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 234 | * @param steerOffset The offset of the CANCoder in radians. 235 | * @return The configured swerve module. 236 | */ 237 | public static SwerveModule createNeo( 238 | Mk4ModuleConfiguration configuration, 239 | GearRatio gearRatio, 240 | int driveMotorPort, 241 | int steerMotorPort, 242 | int steerEncoderPort, 243 | double steerOffset, 244 | String namePrefix 245 | ) { 246 | return new SwerveModuleFactory<>( 247 | gearRatio.getConfiguration(), 248 | getNeoDriveFactory(configuration), 249 | getNeoSteerFactory(configuration) 250 | ).create( 251 | driveMotorPort, 252 | new NeoSteerConfiguration<>( 253 | steerMotorPort, 254 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 255 | ), 256 | namePrefix 257 | ); 258 | } 259 | 260 | /** 261 | * Creates a Mk4i swerve module that uses NEOs for driving and steering. 262 | * 263 | * @param gearRatio The gearing configuration the module is in. 264 | * @param driveMotorPort The CAN ID of the drive NEO. 265 | * @param steerMotorPort The CAN ID of the steer NEO. 266 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 267 | * @param steerOffset The offset of the CANCoder in radians. 268 | * @return The configured swerve module. 269 | */ 270 | public static SwerveModule createNeo( 271 | GearRatio gearRatio, 272 | int driveMotorPort, 273 | int steerMotorPort, 274 | int steerEncoderPort, 275 | double steerOffset, 276 | String namePrefix 277 | ) { 278 | return createNeo(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 279 | } 280 | 281 | /** 282 | * Creates a Mk4i swerve module that uses a Falcon 500 for driving and a NEO for steering. 283 | * Module information is displayed in the specified ShuffleBoard container. 284 | * 285 | * @param container The container to display module information in. 286 | * @param configuration Module configuration parameters to use. 287 | * @param gearRatio The gearing configuration the module is in. 288 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 289 | * @param steerMotorPort The CAN ID of the steer NEO. 290 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 291 | * @param steerOffset The offset of the CANCoder in radians. 292 | * @return The configured swerve module. 293 | */ 294 | public static SwerveModule createFalcon500Neo( 295 | ShuffleboardLayout container, 296 | Mk4ModuleConfiguration configuration, 297 | GearRatio gearRatio, 298 | int driveMotorPort, 299 | int steerMotorPort, 300 | int steerEncoderPort, 301 | double steerOffset, 302 | String namePrefix 303 | ) { 304 | return new SwerveModuleFactory<>( 305 | gearRatio.getConfiguration(), 306 | getFalcon500DriveFactory(configuration), 307 | getNeoSteerFactory(configuration) 308 | ).create( 309 | container, 310 | driveMotorPort, 311 | new NeoSteerConfiguration<>( 312 | steerMotorPort, 313 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 314 | ), 315 | namePrefix 316 | ); 317 | } 318 | 319 | /** 320 | * Creates a Mk4i swerve module that uses a Falcon 500 for driving and a NEO for steering. 321 | * Module information is displayed in the specified ShuffleBoard container. 322 | * 323 | * @param container The container to display module information in. 324 | * @param gearRatio The gearing configuration the module is in. 325 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 326 | * @param steerMotorPort The CAN ID of the steer NEO. 327 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 328 | * @param steerOffset The offset of the CANCoder in radians. 329 | * @return The configured swerve module. 330 | */ 331 | public static SwerveModule createFalcon500Neo( 332 | ShuffleboardLayout container, 333 | GearRatio gearRatio, 334 | int driveMotorPort, 335 | int steerMotorPort, 336 | int steerEncoderPort, 337 | double steerOffset, 338 | String namePrefix 339 | ) { 340 | return createFalcon500Neo(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 341 | } 342 | 343 | /** 344 | * Creates a Mk4i swerve module that uses a Falcon 500 for driving and a NEO for steering. 345 | * 346 | * @param configuration Module configuration parameters to use. 347 | * @param gearRatio The gearing configuration the module is in. 348 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 349 | * @param steerMotorPort The CAN ID of the steer NEO. 350 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 351 | * @param steerOffset The offset of the CANCoder in radians. 352 | * @return The configured swerve module. 353 | */ 354 | public static SwerveModule createFalcon500Neo( 355 | Mk4ModuleConfiguration configuration, 356 | GearRatio gearRatio, 357 | int driveMotorPort, 358 | int steerMotorPort, 359 | int steerEncoderPort, 360 | double steerOffset, 361 | String namePrefix 362 | ) { 363 | return new SwerveModuleFactory<>( 364 | gearRatio.getConfiguration(), 365 | getFalcon500DriveFactory(configuration), 366 | getNeoSteerFactory(configuration) 367 | ).create( 368 | driveMotorPort, 369 | new NeoSteerConfiguration<>( 370 | steerMotorPort, 371 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 372 | ), 373 | namePrefix 374 | ); 375 | } 376 | 377 | /** 378 | * Creates a Mk4i swerve module that uses a Falcon 500 for driving and a NEO for steering. 379 | * 380 | * @param gearRatio The gearing configuration the module is in. 381 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 382 | * @param steerMotorPort The CAN ID of the steer NEO. 383 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 384 | * @param steerOffset The offset of the CANCoder in radians. 385 | * @return The configured swerve module. 386 | */ 387 | public static SwerveModule createFalcon500Neo( 388 | GearRatio gearRatio, 389 | int driveMotorPort, 390 | int steerMotorPort, 391 | int steerEncoderPort, 392 | double steerOffset, 393 | String namePrefix 394 | ) { 395 | return createFalcon500Neo(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 396 | } 397 | 398 | /** 399 | * Creates a Mk4i swerve module that uses a NEO for driving and a Falcon 500 for steering. 400 | * Module information is displayed in the specified ShuffleBoard container. 401 | * 402 | * @param container The container to display module information in. 403 | * @param configuration Module configuration parameters to use. 404 | * @param gearRatio The gearing configuration the module is in. 405 | * @param driveMotorPort The CAN ID of the drive NEO. 406 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 407 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 408 | * @param steerOffset The offset of the CANCoder in radians. 409 | * @return The configured swerve module. 410 | */ 411 | public static SwerveModule createNeoFalcon500( 412 | ShuffleboardLayout container, 413 | Mk4ModuleConfiguration configuration, 414 | GearRatio gearRatio, 415 | int driveMotorPort, 416 | int steerMotorPort, 417 | int steerEncoderPort, 418 | double steerOffset, 419 | String namePrefix 420 | ) { 421 | return new SwerveModuleFactory<>( 422 | gearRatio.getConfiguration(), 423 | getNeoDriveFactory(configuration), 424 | getFalcon500SteerFactory(configuration) 425 | ).create( 426 | container, 427 | driveMotorPort, 428 | new Falcon500SteerConfiguration<>( 429 | steerMotorPort, 430 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 431 | ), 432 | namePrefix 433 | ); 434 | } 435 | 436 | /** 437 | * Creates a Mk4i swerve module that uses a NEO for driving and a Falcon 500 for steering. 438 | * Module information is displayed in the specified ShuffleBoard container. 439 | * 440 | * @param container The container to display module information in. 441 | * @param gearRatio The gearing configuration the module is in. 442 | * @param driveMotorPort The CAN ID of the drive NEO. 443 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 444 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 445 | * @param steerOffset The offset of the CANCoder in radians. 446 | * @return The configured swerve module. 447 | */ 448 | public static SwerveModule createNeoFalcon500( 449 | ShuffleboardLayout container, 450 | GearRatio gearRatio, 451 | int driveMotorPort, 452 | int steerMotorPort, 453 | int steerEncoderPort, 454 | double steerOffset, 455 | String namePrefix 456 | ) { 457 | return createNeoFalcon500(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 458 | } 459 | 460 | /** 461 | * Creates a Mk4i swerve module that uses a NEO for driving and a Falcon 500 for steering. 462 | * 463 | * @param configuration Module configuration parameters to use. 464 | * @param gearRatio The gearing configuration the module is in. 465 | * @param driveMotorPort The CAN ID of the drive NEO. 466 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 467 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 468 | * @param steerOffset The offset of the CANCoder in radians. 469 | * @return The configured swerve module. 470 | */ 471 | public static SwerveModule createNeoFalcon500( 472 | Mk4ModuleConfiguration configuration, 473 | GearRatio gearRatio, 474 | int driveMotorPort, 475 | int steerMotorPort, 476 | int steerEncoderPort, 477 | double steerOffset, 478 | String namePrefix 479 | ) { 480 | return new SwerveModuleFactory<>( 481 | gearRatio.getConfiguration(), 482 | getNeoDriveFactory(configuration), 483 | getFalcon500SteerFactory(configuration) 484 | ).create( 485 | driveMotorPort, 486 | new Falcon500SteerConfiguration<>( 487 | steerMotorPort, 488 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 489 | ), 490 | namePrefix 491 | ); 492 | } 493 | 494 | /** 495 | * Creates a Mk4i swerve module that uses a NEO for driving and a Falcon 500 for steering. 496 | * 497 | * @param gearRatio The gearing configuration the module is in. 498 | * @param driveMotorPort The CAN ID of the drive NEO. 499 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 500 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 501 | * @param steerOffset The offset of the CANCoder in radians. 502 | * @return The configured swerve module. 503 | */ 504 | public static SwerveModule createNeoFalcon500( 505 | GearRatio gearRatio, 506 | int driveMotorPort, 507 | int steerMotorPort, 508 | int steerEncoderPort, 509 | double steerOffset, 510 | String namePrefix 511 | ) { 512 | return createNeoFalcon500(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 513 | } 514 | 515 | public enum GearRatio { 516 | L1(SdsModuleConfigurations.MK4I_L1), 517 | L2(SdsModuleConfigurations.MK4I_L2), 518 | L3(SdsModuleConfigurations.MK4I_L3); 519 | 520 | private final ModuleConfiguration configuration; 521 | 522 | GearRatio(ModuleConfiguration configuration) { 523 | this.configuration = configuration; 524 | } 525 | 526 | public ModuleConfiguration getConfiguration() { 527 | return configuration; 528 | } 529 | } 530 | } -------------------------------------------------------------------------------- /src/main/java/frc/swervelib/Mk4SwerveModuleHelper.java: -------------------------------------------------------------------------------- 1 | package frc.swervelib; 2 | 3 | import frc.swervelib.ctre.*; 4 | import frc.swervelib.rev.NeoDriveControllerFactoryBuilder; 5 | import frc.swervelib.rev.NeoSteerConfiguration; 6 | import frc.swervelib.rev.NeoSteerControllerFactoryBuilder; 7 | import frc.wpiClasses.QuadSwerveSim; 8 | import frc.wpiClasses.SwerveModuleSim; 9 | import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; 10 | 11 | public final class Mk4SwerveModuleHelper { 12 | private Mk4SwerveModuleHelper() { 13 | } 14 | 15 | private static DriveControllerFactory getFalcon500DriveFactory(Mk4ModuleConfiguration configuration) { 16 | return new Falcon500DriveControllerFactoryBuilder() 17 | .withVoltageCompensation(configuration.getNominalVoltage()) 18 | .withCurrentLimit(configuration.getDriveCurrentLimit()) 19 | .build(); 20 | } 21 | 22 | private static SteerControllerFactory> getFalcon500SteerFactory(Mk4ModuleConfiguration configuration) { 23 | return new Falcon500SteerControllerFactoryBuilder() 24 | .withVoltageCompensation(configuration.getNominalVoltage()) 25 | .withPidConstants(0.2, 0.0, 0.1) 26 | .withCurrentLimit(configuration.getSteerCurrentLimit()) 27 | .build(new CanCoderFactoryBuilder() 28 | .withReadingUpdatePeriod(100) 29 | .build()); 30 | } 31 | 32 | private static DriveControllerFactory getNeoDriveFactory(Mk4ModuleConfiguration configuration) { 33 | return new NeoDriveControllerFactoryBuilder() 34 | .withVoltageCompensation(configuration.getNominalVoltage()) 35 | .withCurrentLimit(configuration.getDriveCurrentLimit()) 36 | .build(); 37 | } 38 | 39 | private static SteerControllerFactory> getNeoSteerFactory(Mk4ModuleConfiguration configuration) { 40 | return new NeoSteerControllerFactoryBuilder() 41 | .withVoltageCompensation(configuration.getNominalVoltage()) 42 | .withPidConstants(1.0, 0.0, 0.1) 43 | .withCurrentLimit(configuration.getSteerCurrentLimit()) 44 | .build(new CanCoderFactoryBuilder() 45 | .withReadingUpdatePeriod(100) 46 | .build()); 47 | } 48 | 49 | /** 50 | * Creates a Mk4 swerve module that uses Falcon 500s for driving and steering. 51 | * Module information is displayed in the specified ShuffleBoard container. 52 | * 53 | * @param container The container to display module information in. 54 | * @param configuration Module configuration parameters to use. 55 | * @param gearRatio The gearing configuration the module is in. 56 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 57 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 58 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 59 | * @param steerOffset The offset of the CANCoder in radians. 60 | * @param namePrefix The name of the swerve module for unique identification 61 | * @return The configured swerve module. 62 | */ 63 | public static SwerveModule createFalcon500( 64 | ShuffleboardLayout container, 65 | Mk4ModuleConfiguration configuration, 66 | GearRatio gearRatio, 67 | int driveMotorPort, 68 | int steerMotorPort, 69 | int steerEncoderPort, 70 | double steerOffset, 71 | String namePrefix 72 | ) { 73 | return new SwerveModuleFactory<>( 74 | gearRatio.getConfiguration(), 75 | getFalcon500DriveFactory(configuration), 76 | getFalcon500SteerFactory(configuration) 77 | ).create( 78 | container, 79 | driveMotorPort, 80 | new Falcon500SteerConfiguration<>( 81 | steerMotorPort, 82 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 83 | ), namePrefix 84 | 85 | ); 86 | } 87 | 88 | /** 89 | * Creates a Mk4 swerve module that uses Falcon 500s for driving and steering. 90 | * Module information is displayed in the specified ShuffleBoard container. 91 | * 92 | * @param container The container to display module information in. 93 | * @param gearRatio The gearing configuration the module is in. 94 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 95 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 96 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 97 | * @param steerOffset The offset of the CANCoder in radians. 98 | * @param namePrefix The name of the swerve module for unique identification 99 | * @return The configured swerve module. 100 | */ 101 | public static SwerveModule createFalcon500( 102 | ShuffleboardLayout container, 103 | GearRatio gearRatio, 104 | int driveMotorPort, 105 | int steerMotorPort, 106 | int steerEncoderPort, 107 | double steerOffset, String namePrefix 108 | ) { 109 | return createFalcon500(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 110 | } 111 | 112 | /** 113 | * Creates a Mk4 swerve module that uses Falcon 500s for driving and steering. 114 | * 115 | * @param configuration Module configuration parameters to use. 116 | * @param gearRatio The gearing configuration the module is in. 117 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 118 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 119 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 120 | * @param steerOffset The offset of the CANCoder in radians. 121 | * @param namePrefix The name of the swerve module for unique identification 122 | * @return The configured swerve module. 123 | */ 124 | public static SwerveModule createFalcon500( 125 | Mk4ModuleConfiguration configuration, 126 | GearRatio gearRatio, 127 | int driveMotorPort, 128 | int steerMotorPort, 129 | int steerEncoderPort, 130 | double steerOffset, String namePrefix 131 | ) { 132 | return new SwerveModuleFactory<>( 133 | gearRatio.getConfiguration(), 134 | getFalcon500DriveFactory(configuration), 135 | getFalcon500SteerFactory(configuration) 136 | ).create( 137 | driveMotorPort, 138 | new Falcon500SteerConfiguration<>( 139 | steerMotorPort, 140 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 141 | ), namePrefix 142 | ); 143 | } 144 | 145 | /** 146 | * Creates a Mk4 swerve module that uses Falcon 500s for driving and steering. 147 | * 148 | * @param gearRatio The gearing configuration the module is in. 149 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 150 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 151 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 152 | * @param steerOffset The offset of the CANCoder in radians. 153 | * @param namePrefix The name of the swerve module for unique identification 154 | * @return The configured swerve module. 155 | */ 156 | public static SwerveModule createFalcon500( 157 | GearRatio gearRatio, 158 | int driveMotorPort, 159 | int steerMotorPort, 160 | int steerEncoderPort, 161 | double steerOffset, String namePrefix 162 | ) { 163 | return createFalcon500(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 164 | } 165 | 166 | /** 167 | * Creates a Mk4 swerve module that uses NEOs for driving and steering. 168 | * Module information is displayed in the specified ShuffleBoard container. 169 | * 170 | * @param container The container to display module information in. 171 | * @param configuration Module configuration parameters to use. 172 | * @param gearRatio The gearing configuration the module is in. 173 | * @param driveMotorPort The CAN ID of the drive NEO. 174 | * @param steerMotorPort The CAN ID of the steer NEO. 175 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 176 | * @param steerOffset The offset of the CANCoder in radians. 177 | * @param namePrefix The name of the swerve module for unique identification 178 | * @return The configured swerve module. 179 | */ 180 | public static SwerveModule createNeo( 181 | ShuffleboardLayout container, 182 | Mk4ModuleConfiguration configuration, 183 | GearRatio gearRatio, 184 | int driveMotorPort, 185 | int steerMotorPort, 186 | int steerEncoderPort, 187 | double steerOffset, String namePrefix 188 | ) { 189 | return new SwerveModuleFactory<>( 190 | gearRatio.getConfiguration(), 191 | getNeoDriveFactory(configuration), 192 | getNeoSteerFactory(configuration) 193 | ).create( 194 | container, 195 | driveMotorPort, 196 | new NeoSteerConfiguration<>( 197 | steerMotorPort, 198 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 199 | ),namePrefix 200 | ); 201 | } 202 | 203 | /** 204 | * Creates a Mk4 swerve module that uses NEOs for driving and steering. 205 | * Module information is displayed in the specified ShuffleBoard container. 206 | * 207 | * @param container The container to display module information in. 208 | * @param gearRatio The gearing configuration the module is in. 209 | * @param driveMotorPort The CAN ID of the drive NEO. 210 | * @param steerMotorPort The CAN ID of the steer NEO. 211 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 212 | * @param steerOffset The offset of the CANCoder in radians. 213 | * @param namePrefix The name of the swerve module for unique identification 214 | * @return The configured swerve module. 215 | */ 216 | public static SwerveModule createNeo( 217 | ShuffleboardLayout container, 218 | GearRatio gearRatio, 219 | int driveMotorPort, 220 | int steerMotorPort, 221 | int steerEncoderPort, 222 | double steerOffset, String namePrefix 223 | ) { 224 | return createNeo(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 225 | } 226 | 227 | /** 228 | * Creates a Mk4 swerve module that uses NEOs for driving and steering. 229 | * 230 | * @param configuration Module configuration parameters to use. 231 | * @param gearRatio The gearing configuration the module is in. 232 | * @param driveMotorPort The CAN ID of the drive NEO. 233 | * @param steerMotorPort The CAN ID of the steer NEO. 234 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 235 | * @param steerOffset The offset of the CANCoder in radians. 236 | * @param namePrefix The name of the swerve module for unique identification 237 | * @return The configured swerve module. 238 | */ 239 | public static SwerveModule createNeo( 240 | Mk4ModuleConfiguration configuration, 241 | GearRatio gearRatio, 242 | int driveMotorPort, 243 | int steerMotorPort, 244 | int steerEncoderPort, 245 | double steerOffset, String namePrefix 246 | ) { 247 | return new SwerveModuleFactory<>( 248 | gearRatio.getConfiguration(), 249 | getNeoDriveFactory(configuration), 250 | getNeoSteerFactory(configuration) 251 | ).create( 252 | driveMotorPort, 253 | new NeoSteerConfiguration<>( 254 | steerMotorPort, 255 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 256 | ), namePrefix 257 | ); 258 | } 259 | 260 | /** 261 | * Creates a Mk4 swerve module that uses NEOs for driving and steering. 262 | * 263 | * @param gearRatio The gearing configuration the module is in. 264 | * @param driveMotorPort The CAN ID of the drive NEO. 265 | * @param steerMotorPort The CAN ID of the steer NEO. 266 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 267 | * @param steerOffset The offset of the CANCoder in radians. 268 | * @param namePrefix The name of the swerve module for unique identification 269 | * @return The configured swerve module. 270 | */ 271 | public static SwerveModule createNeo( 272 | GearRatio gearRatio, 273 | int driveMotorPort, 274 | int steerMotorPort, 275 | int steerEncoderPort, 276 | double steerOffset, String namePrefix 277 | ) { 278 | return createNeo(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 279 | } 280 | 281 | /** 282 | * Creates a Mk4 swerve module that uses a Falcon 500 for driving and a NEO for steering. 283 | * Module information is displayed in the specified ShuffleBoard container. 284 | * 285 | * @param container The container to display module information in. 286 | * @param configuration Module configuration parameters to use. 287 | * @param gearRatio The gearing configuration the module is in. 288 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 289 | * @param steerMotorPort The CAN ID of the steer NEO. 290 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 291 | * @param steerOffset The offset of the CANCoder in radians. 292 | * @param namePrefix The name of the swerve module for unique identification 293 | * @return The configured swerve module. 294 | */ 295 | public static SwerveModule createFalcon500Neo( 296 | ShuffleboardLayout container, 297 | Mk4ModuleConfiguration configuration, 298 | GearRatio gearRatio, 299 | int driveMotorPort, 300 | int steerMotorPort, 301 | int steerEncoderPort, 302 | double steerOffset, String namePrefix 303 | ) { 304 | return new SwerveModuleFactory<>( 305 | gearRatio.getConfiguration(), 306 | getFalcon500DriveFactory(configuration), 307 | getNeoSteerFactory(configuration) 308 | ).create( 309 | container, 310 | driveMotorPort, 311 | new NeoSteerConfiguration<>( 312 | steerMotorPort, 313 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 314 | ), namePrefix 315 | ); 316 | } 317 | 318 | /** 319 | * Creates a Mk4 swerve module that uses a Falcon 500 for driving and a NEO for steering. 320 | * Module information is displayed in the specified ShuffleBoard container. 321 | * 322 | * @param container The container to display module information in. 323 | * @param gearRatio The gearing configuration the module is in. 324 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 325 | * @param steerMotorPort The CAN ID of the steer NEO. 326 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 327 | * @param steerOffset The offset of the CANCoder in radians. 328 | * @param namePrefix The name of the swerve module for unique identification 329 | * @return The configured swerve module. 330 | */ 331 | public static SwerveModule createFalcon500Neo( 332 | ShuffleboardLayout container, 333 | GearRatio gearRatio, 334 | int driveMotorPort, 335 | int steerMotorPort, 336 | int steerEncoderPort, 337 | double steerOffset, String namePrefix 338 | ) { 339 | return createFalcon500Neo(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 340 | } 341 | 342 | /** 343 | * Creates a Mk4 swerve module that uses a Falcon 500 for driving and a NEO for steering. 344 | * 345 | * @param configuration Module configuration parameters to use. 346 | * @param gearRatio The gearing configuration the module is in. 347 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 348 | * @param steerMotorPort The CAN ID of the steer NEO. 349 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 350 | * @param steerOffset The offset of the CANCoder in radians. 351 | * @param namePrefix The name of the swerve module for unique identification 352 | * @return The configured swerve module. 353 | */ 354 | public static SwerveModule createFalcon500Neo( 355 | Mk4ModuleConfiguration configuration, 356 | GearRatio gearRatio, 357 | int driveMotorPort, 358 | int steerMotorPort, 359 | int steerEncoderPort, 360 | double steerOffset, String namePrefix 361 | ) { 362 | return new SwerveModuleFactory<>( 363 | gearRatio.getConfiguration(), 364 | getFalcon500DriveFactory(configuration), 365 | getNeoSteerFactory(configuration) 366 | ).create( 367 | driveMotorPort, 368 | new NeoSteerConfiguration<>( 369 | steerMotorPort, 370 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 371 | ), namePrefix 372 | ); 373 | } 374 | 375 | /** 376 | * Creates a Mk4 swerve module that uses a Falcon 500 for driving and a NEO for steering. 377 | * 378 | * @param gearRatio The gearing configuration the module is in. 379 | * @param driveMotorPort The CAN ID of the drive Falcon 500. 380 | * @param steerMotorPort The CAN ID of the steer NEO. 381 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 382 | * @param steerOffset The offset of the CANCoder in radians. 383 | * @param namePrefix The name of the swerve module for unique identification 384 | * @return The configured swerve module. 385 | */ 386 | public static SwerveModule createFalcon500Neo( 387 | GearRatio gearRatio, 388 | int driveMotorPort, 389 | int steerMotorPort, 390 | int steerEncoderPort, 391 | double steerOffset, String namePrefix 392 | ) { 393 | return createFalcon500Neo(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 394 | } 395 | 396 | /** 397 | * Creates a Mk4 swerve module that uses a NEO for driving and a Falcon 500 for steering. 398 | * Module information is displayed in the specified ShuffleBoard container. 399 | * 400 | * @param container The container to display module information in. 401 | * @param configuration Module configuration parameters to use. 402 | * @param gearRatio The gearing configuration the module is in. 403 | * @param driveMotorPort The CAN ID of the drive NEO. 404 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 405 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 406 | * @param steerOffset The offset of the CANCoder in radians. 407 | * @param namePrefix The name of the swerve module for unique identification 408 | * @return The configured swerve module. 409 | */ 410 | public static SwerveModule createNeoFalcon500( 411 | ShuffleboardLayout container, 412 | Mk4ModuleConfiguration configuration, 413 | GearRatio gearRatio, 414 | int driveMotorPort, 415 | int steerMotorPort, 416 | int steerEncoderPort, 417 | double steerOffset, String namePrefix 418 | ) { 419 | return new SwerveModuleFactory<>( 420 | gearRatio.getConfiguration(), 421 | getNeoDriveFactory(configuration), 422 | getFalcon500SteerFactory(configuration) 423 | ).create( 424 | container, 425 | driveMotorPort, 426 | new Falcon500SteerConfiguration<>( 427 | steerMotorPort, 428 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 429 | ), namePrefix 430 | ); 431 | } 432 | 433 | /** 434 | * Creates a Mk4 swerve module that uses a NEO for driving and a Falcon 500 for steering. 435 | * Module information is displayed in the specified ShuffleBoard container. 436 | * 437 | * @param container The container to display module information in. 438 | * @param gearRatio The gearing configuration the module is in. 439 | * @param driveMotorPort The CAN ID of the drive NEO. 440 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 441 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 442 | * @param steerOffset The offset of the CANCoder in radians. 443 | * @param namePrefix The name of the swerve module for unique identification 444 | * @return The configured swerve module. 445 | */ 446 | public static SwerveModule createNeoFalcon500( 447 | ShuffleboardLayout container, 448 | GearRatio gearRatio, 449 | int driveMotorPort, 450 | int steerMotorPort, 451 | int steerEncoderPort, 452 | double steerOffset, String namePrefix 453 | ) { 454 | return createNeoFalcon500(container, new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 455 | } 456 | 457 | /** 458 | * Creates a Mk4 swerve module that uses a NEO for driving and a Falcon 500 for steering. 459 | * 460 | * @param configuration Module configuration parameters to use. 461 | * @param gearRatio The gearing configuration the module is in. 462 | * @param driveMotorPort The CAN ID of the drive NEO. 463 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 464 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 465 | * @param steerOffset The offset of the CANCoder in radians. 466 | * @param namePrefix The name of the swerve module for unique identification 467 | * @return The configured swerve module. 468 | */ 469 | public static SwerveModule createNeoFalcon500( 470 | Mk4ModuleConfiguration configuration, 471 | GearRatio gearRatio, 472 | int driveMotorPort, 473 | int steerMotorPort, 474 | int steerEncoderPort, 475 | double steerOffset, String namePrefix 476 | ) { 477 | return new SwerveModuleFactory<>( 478 | gearRatio.getConfiguration(), 479 | getNeoDriveFactory(configuration), 480 | getFalcon500SteerFactory(configuration) 481 | ).create( 482 | driveMotorPort, 483 | new Falcon500SteerConfiguration<>( 484 | steerMotorPort, 485 | new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) 486 | ), namePrefix 487 | ); 488 | } 489 | 490 | /** 491 | * Creates a Mk4 swerve module that uses a NEO for driving and a Falcon 500 for steering. 492 | * 493 | * @param gearRatio The gearing configuration the module is in. 494 | * @param driveMotorPort The CAN ID of the drive NEO. 495 | * @param steerMotorPort The CAN ID of the steer Falcon 500. 496 | * @param steerEncoderPort The CAN ID of the steer CANCoder. 497 | * @param steerOffset The offset of the CANCoder in radians. 498 | * @param namePrefix The name of the swerve module for unique identification 499 | * @return The configured swerve module. 500 | */ 501 | public static SwerveModule createNeoFalcon500( 502 | GearRatio gearRatio, 503 | int driveMotorPort, 504 | int steerMotorPort, 505 | int steerEncoderPort, 506 | double steerOffset, String namePrefix 507 | ) { 508 | return createNeoFalcon500(new Mk4ModuleConfiguration(), gearRatio, driveMotorPort, steerMotorPort, steerEncoderPort, steerOffset, namePrefix); 509 | } 510 | 511 | public enum GearRatio { 512 | L1(SdsModuleConfigurations.MK4_L1), 513 | L2(SdsModuleConfigurations.MK4_L2), 514 | L3(SdsModuleConfigurations.MK4_L3), 515 | L4(SdsModuleConfigurations.MK4_L4); 516 | 517 | private final ModuleConfiguration configuration; 518 | 519 | GearRatio(ModuleConfiguration configuration) { 520 | this.configuration = configuration; 521 | } 522 | 523 | public ModuleConfiguration getConfiguration() { 524 | return configuration; 525 | } 526 | } 527 | 528 | public static SwerveModuleSim createSim(SwerveModule module) { 529 | ModuleConfiguration modConfig = module.getModuleConfiguration(); 530 | return new SwerveModuleSim(module.getSteerController().getSteerMotor(), 531 | module.getDriveController().getDriveMotor(), 532 | modConfig.getWheelDiameter() / 2, 533 | 1 / modConfig.getSteerReduction(), 534 | 1 / modConfig.getDriveReduction(), 535 | 1.0, // CANCoder is directly on the shaft 536 | 1 / modConfig.getDriveReduction(), 537 | 1.1, 538 | 0.8, 539 | SwerveConstants.MASS_kg * 9.81 / QuadSwerveSim.NUM_MODULES, 540 | 0.01 541 | ); 542 | } 543 | } 544 | --------------------------------------------------------------------------------