001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.mechanisms.swerve; 008 009import com.ctre.phoenix6.configs.CANcoderConfiguration; 010import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; 011import com.ctre.phoenix6.configs.CurrentLimitsConfigs; 012import com.ctre.phoenix6.configs.FeedbackConfigs; 013import com.ctre.phoenix6.configs.MagnetSensorConfigs; 014import com.ctre.phoenix6.configs.MotionMagicConfigs; 015import com.ctre.phoenix6.configs.MotorOutputConfigs; 016import com.ctre.phoenix6.configs.Slot0Configs; 017import com.ctre.phoenix6.configs.TalonFXConfiguration; 018import com.ctre.phoenix6.configs.TorqueCurrentConfigs; 019import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; 020import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; 021import com.ctre.phoenix6.signals.NeutralModeValue; 022 023/** 024 * Constants that are common across the swerve modules, used 025 * for creating instances of module-specific {@link SwerveModuleConstants}. 026 */ 027public class SwerveModuleConstantsFactory { 028 /** Gear ratio between the drive motor and the wheel. */ 029 public double DriveMotorGearRatio = 0; 030 /** 031 * Gear ratio between the steer motor and the CANcoder. 032 * For example, the SDS Mk4 has a steering ratio of 12.8. 033 */ 034 public double SteerMotorGearRatio = 0; 035 /** 036 * Coupled gear ratio between the CANcoder and the drive motor. 037 * <p> 038 * For a typical swerve module, the azimuth turn motor also drives the wheel a nontrivial 039 * amount, which affects the accuracy of odometry and control. This ratio represents the 040 * number of rotations of the drive motor caused by a rotation of the azimuth. 041 */ 042 public double CouplingGearRatio = 0; 043 044 /** Radius of the driving wheel in inches. */ 045 public double WheelRadius = 0; 046 047 /** 048 * The steer motor closed-loop gains. 049 * <p> 050 * The steer motor uses the control ouput type specified by 051 * {@link #SteerMotorClosedLoopOutput} and any {@link SwerveModule.SteerRequestType}. 052 */ 053 public Slot0Configs SteerMotorGains = new Slot0Configs(); 054 /** 055 * The drive motor closed-loop gains. 056 * <p> 057 * When using closed-loop control, the drive motor uses the control output 058 * type specified by {@link #DriveMotorClosedLoopOutput} and any closed-loop 059 * {@link SwerveModule.DriveRequestType}. 060 */ 061 public Slot0Configs DriveMotorGains = new Slot0Configs(); 062 063 /** The closed-loop output type to use for the steer motors. */ 064 public ClosedLoopOutputType SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; 065 /** The closed-loop output type to use for the drive motors. */ 066 public ClosedLoopOutputType DriveMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; 067 068 /** The maximum amount of stator current the drive motors can apply without slippage. */ 069 public double SlipCurrent = 400; 070 071 /** True if the steering motor is reversed from the CANcoder. */ 072 public boolean SteerMotorInverted = false; 073 074 /** 075 * When using open-loop drive control, this specifies the speed at which the robot travels 076 * when driven with 12 volts, in meters per second. This is used to approximate the output 077 * for a desired velocity. If using closed loop control, this value is ignored. 078 */ 079 public double SpeedAt12VoltsMps = 0; 080 081 /** Sim-specific constants **/ 082 /** Simulated azimuthal inertia in kilogram meters squared. */ 083 public double SteerInertia = 0.00001; 084 /** Simulated drive inertia in kilogram meters squared. */ 085 public double DriveInertia = 0.001; 086 /** Simulated steer voltage required to overcome friction. */ 087 public double SteerFrictionVoltage = 0.25; 088 /** Simulated drive voltage required to overcome friction. */ 089 public double DriveFrictionVoltage = 0.25; 090 091 /** 092 * Choose how the feedback sensors should be configured. 093 * <p> 094 * If the robot does not support Pro, then this should remain as RemoteCANcoder. 095 * Otherwise, users have the option to use either FusedCANcoder or SyncCANcoder depending 096 * on if there is a risk that the CANcoder can fail in a way to provide "good" data. 097 */ 098 public SteerFeedbackType FeedbackSource = SteerFeedbackType.RemoteCANcoder; 099 100 /** 101 * The initial configs used to configure the drive motor of the swerve module. 102 * The default value is the factory-default. 103 * <p> 104 * Users may change the initial configuration as they need. 105 * Any config that's not referenced in the {@link SwerveModuleConstants} 106 * class is available to be changed. 107 * <p> 108 * The list of configs that will be overwritten is as follows: 109 * <ul> 110 * <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode, overwritten with {@link SwerveModule#configNeutralMode(NeutralModeValue)}) 111 * <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#DriveMotorInverted}) 112 * <li> {@link Slot0Configs} ({@link SwerveModuleConstants#DriveMotorGains}) 113 * <li> {@link CurrentLimitsConfigs#StatorCurrentLimit} / {@link TorqueCurrentConfigs#PeakForwardTorqueCurrent} 114 * / {@link TorqueCurrentConfigs#PeakReverseTorqueCurrent} ({@link SwerveModuleConstants#SlipCurrent}) 115 * <li> {@link CurrentLimitsConfigs#StatorCurrentLimitEnable} (Enabled) 116 * </ul> 117 */ 118 public TalonFXConfiguration DriveMotorInitialConfigs = new TalonFXConfiguration(); 119 /** 120 * The initial configs used to configure the steer motor of the swerve module. 121 * The default value is the factory-default. 122 * <p> 123 * Users may change the initial configuration as they need. 124 * Any config that's not referenced in the {@link SwerveModuleConstants} 125 * class is available to be changed. 126 * <p> 127 * The list of configs that will be overwritten is as follows: 128 * <ul> 129 * <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode) 130 * <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#SteerMotorInverted}) 131 * <li> {@link Slot0Configs} ({@link SwerveModuleConstants#SteerMotorGains}) 132 * <li> {@link FeedbackConfigs#FeedbackRemoteSensorID} ({@link SwerveModuleConstants#CANcoderId}) 133 * <li> {@link FeedbackConfigs#FeedbackSensorSource} ({@link SwerveModuleConstants#FeedbackSource}) 134 * <li> {@link FeedbackConfigs#RotorToSensorRatio} ({@link SwerveModuleConstants#SteerMotorGearRatio}) 135 * <li> {@link MotionMagicConfigs} (Calculated from gear ratios) 136 * <li> {@link ClosedLoopGeneralConfigs#ContinuousWrap} (true) 137 * </ul> 138 */ 139 public TalonFXConfiguration SteerMotorInitialConfigs = new TalonFXConfiguration(); 140 /** 141 * The initial configs used to configure the CANcoder of the swerve module. 142 * The default value is the factory-default. 143 * <p> 144 * Users may change the initial configuration as they need. 145 * Any config that's not referenced in the {@link SwerveModuleConstants} 146 * class is available to be changed. 147 * <p> 148 * The list of configs that will be overwritten is as follows: 149 * <ul> 150 * <li> {@link MagnetSensorConfigs#MagnetOffset} ({@link SwerveModuleConstants#CANcoderOffset}) 151 * </ul> 152 */ 153 public CANcoderConfiguration CANcoderInitialConfigs = new CANcoderConfiguration(); 154 155 /** 156 * Sets the gear ratio between the drive motor and the wheel. 157 * 158 * @param ratio Gear ratio between the drive motor and the wheel 159 * @return this object 160 */ 161 public SwerveModuleConstantsFactory withDriveMotorGearRatio(double ratio) { 162 this.DriveMotorGearRatio = ratio; 163 return this; 164 } 165 166 /** 167 * Sets the gear ratio between the steer motor and the CANcoder. 168 * For example, the SDS Mk4 has a steering ratio of 12.8. 169 * 170 * @param ratio Gear ratio between the steer motor and the CANcoder 171 * @return this object 172 */ 173 public SwerveModuleConstantsFactory withSteerMotorGearRatio(double ratio) { 174 this.SteerMotorGearRatio = ratio; 175 return this; 176 } 177 178 /** 179 * Sets the coupled gear ratio between the CANcoder and the drive motor. 180 * <p> 181 * For a typical swerve module, the azimuth turn motor also drives the wheel a nontrivial 182 * amount, which affects the accuracy of odometry and control. This ratio represents the 183 * number of rotations of the drive motor caused by a rotation of the azimuth. 184 * 185 * @param ratio Coupled gear ratio between the CANcoder and the drive motor 186 * @return this object 187 */ 188 public SwerveModuleConstantsFactory withCouplingGearRatio(double ratio) { 189 this.CouplingGearRatio = ratio; 190 return this; 191 } 192 193 /** 194 * Sets the radius of the driving wheel in inches. 195 * 196 * @param radius Radius of the driving wheel in inches 197 * @return this object 198 */ 199 public SwerveModuleConstantsFactory withWheelRadius(double radius) { 200 this.WheelRadius = radius; 201 return this; 202 } 203 204 /** 205 * Sets the steer motor closed-loop gains. 206 * <p> 207 * The steer motor uses the control ouput type specified by 208 * {@link #SteerMotorClosedLoopOutput} and any {@link SwerveModule.SteerRequestType}. 209 * 210 * @param gains Steer motor closed-loop gains 211 * @return this object 212 */ 213 public SwerveModuleConstantsFactory withSteerMotorGains(Slot0Configs gains) { 214 this.SteerMotorGains = gains; 215 return this; 216 } 217 218 /** 219 * Sets the drive motor closed-loop gains. 220 * <p> 221 * When using closed-loop control, the drive motor uses the control output 222 * type specified by {@link #DriveMotorClosedLoopOutput} and any closed-loop 223 * {@link SwerveModule.DriveRequestType}. 224 * 225 * @param gains Drive motor closed-loop gains 226 * @return this object 227 */ 228 public SwerveModuleConstantsFactory withDriveMotorGains(Slot0Configs gains) { 229 this.DriveMotorGains = gains; 230 return this; 231 } 232 233 /** 234 * Sets closed-loop output type to use for the steer motors. 235 * 236 * @param outputType Closed-loop output type to use for the steer motors 237 * @return this object 238 */ 239 public SwerveModuleConstantsFactory withSteerMotorClosedLoopOutput(ClosedLoopOutputType outputType) { 240 this.SteerMotorClosedLoopOutput = outputType; 241 return this; 242 } 243 244 /** 245 * Sets closed-loop output type to use for the drive motors. 246 * 247 * @param outputType Closed-loop output type to use for the drive motors 248 * @return this object 249 */ 250 public SwerveModuleConstantsFactory withDriveMotorClosedLoopOutput(ClosedLoopOutputType outputType) { 251 this.DriveMotorClosedLoopOutput = outputType; 252 return this; 253 } 254 255 /** 256 * Sets the maximum amount of stator current the drive motors can 257 * apply without slippage. 258 * 259 * @param slipCurrent Maximum amount of stator current 260 * @return this object 261 */ 262 public SwerveModuleConstantsFactory withSlipCurrent(double slipCurrent) { 263 this.SlipCurrent = slipCurrent; 264 return this; 265 } 266 267 /** 268 * Sets whether the steering motor is reversed from the CANcoder. 269 * 270 * @param steerMotorInverted True if the steering motor is reversed from the CANcoder 271 * @return this object 272 */ 273 public SwerveModuleConstantsFactory withSteerMotorInverted(boolean steerMotorInverted) { 274 this.SteerMotorInverted = steerMotorInverted; 275 return this; 276 } 277 278 /** 279 * When using open-loop drive control, this specifies the speed at which the robot travels 280 * when driven with 12 volts, in meters per second. This is used to approximate the output 281 * for a desired velocity. If using closed loop control, this value is ignored. 282 * 283 * @param speedAt12VoltsMps Speed at which the robot travels when driven with 284 * 12 volts, in meters per second 285 * @return this object 286 */ 287 public SwerveModuleConstantsFactory withSpeedAt12VoltsMps(double speedAt12VoltsMps) { 288 this.SpeedAt12VoltsMps = speedAt12VoltsMps; 289 return this; 290 } 291 292 /** 293 * Sets the simulated azimuthal inertia in kilogram meters squared. 294 * 295 * @param steerInertia Azimuthal inertia in kilogram meters squared 296 * @return this object 297 */ 298 public SwerveModuleConstantsFactory withSteerInertia(double steerInertia) { 299 this.SteerInertia = steerInertia; 300 return this; 301 } 302 303 /** 304 * Sets the simulated drive inertia in kilogram meters squared. 305 * 306 * @param driveInertia Drive inertia in kilogram meters squared 307 * @return this object 308 */ 309 public SwerveModuleConstantsFactory withDriveInertia(double driveInertia) { 310 this.DriveInertia = driveInertia; 311 return this; 312 } 313 314 /** 315 * Sets the simulated steer voltage required to overcome friction. 316 * 317 * @param voltage Steer voltage required to overcome friction 318 * @return this object 319 */ 320 public SwerveModuleConstantsFactory withSteerFrictionVoltage(double voltage) { 321 this.SteerFrictionVoltage = voltage; 322 return this; 323 } 324 325 /** 326 * Sets the simulated drive voltage required to overcome friction. 327 * 328 * @param voltage Drive voltage required to overcome friction 329 * @return this object 330 */ 331 public SwerveModuleConstantsFactory withDriveFrictionVoltage(double voltage) { 332 this.DriveFrictionVoltage = voltage; 333 return this; 334 } 335 336 /** 337 * Chooses how the feedback sensors should be configured. 338 * <p> 339 * If the robot does not support Pro, then this should remain as RemoteCANcoder. 340 * Otherwise, users have the option to use either FusedCANcoder or SyncCANcoder depending 341 * on if there is a risk that the CANcoder can fail in a way to provide "good" data. 342 * 343 * @param source The feedback sensor source 344 * @return this object 345 */ 346 public SwerveModuleConstantsFactory withFeedbackSource(SteerFeedbackType source) { 347 this.FeedbackSource = source; 348 return this; 349 } 350 /** 351 * The initial configs used to configure the drive motor of the swerve module. 352 * The default value is the factory-default. 353 * <p> 354 * Users may change the initial configuration as they need. 355 * Any config that's not referenced in the {@link SwerveModuleConstants} 356 * class is available to be changed. 357 * <p> 358 * The list of configs that will be overwritten is as follows: 359 * <ul> 360 * <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode, overwritten with {@link SwerveModule#configNeutralMode(NeutralModeValue)}) 361 * <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#DriveMotorInverted}) 362 * <li> {@link Slot0Configs} ({@link SwerveModuleConstants#DriveMotorGains}) 363 * <li> {@link CurrentLimitsConfigs#StatorCurrentLimit} / {@link TorqueCurrentConfigs#PeakForwardTorqueCurrent} 364 * / {@link TorqueCurrentConfigs#PeakReverseTorqueCurrent} ({@link SwerveModuleConstants#SlipCurrent}) 365 * <li> {@link CurrentLimitsConfigs#StatorCurrentLimitEnable} (Enabled) 366 * </ul> 367 * 368 * @param configs Configs to set as an initial set of configs 369 * @return this object 370 */ 371 public SwerveModuleConstantsFactory withDriveMotorInitialConfigs(TalonFXConfiguration configs) { 372 this.DriveMotorInitialConfigs = configs; 373 return this; 374 } 375 /** 376 * The initial configs used to configure the steer motor of the swerve module. 377 * The default value is the factory-default. 378 * <p> 379 * Users may change the initial configuration as they need. 380 * Any config that's not referenced in the {@link SwerveModuleConstants} 381 * class is available to be changed. 382 * <p> 383 * The list of configs that will be overwritten is as follows: 384 * <ul> 385 * <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode) 386 * <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#SteerMotorInverted}) 387 * <li> {@link Slot0Configs} ({@link SwerveModuleConstants#SteerMotorGains}) 388 * <li> {@link FeedbackConfigs#FeedbackRemoteSensorID} ({@link SwerveModuleConstants#CANcoderId}) 389 * <li> {@link FeedbackConfigs#FeedbackSensorSource} ({@link SwerveModuleConstants#FeedbackSource}) 390 * <li> {@link FeedbackConfigs#RotorToSensorRatio} ({@link SwerveModuleConstants#SteerMotorGearRatio}) 391 * <li> {@link MotionMagicConfigs} (Calculated from gear ratios) 392 * <li> {@link ClosedLoopGeneralConfigs#ContinuousWrap} (true) 393 * </ul> 394 * 395 * @param configs Configs to set as an initial set of configs 396 * @return this object 397 */ 398 public SwerveModuleConstantsFactory withSteerMotorInitialConfigs(TalonFXConfiguration configs) { 399 this.SteerMotorInitialConfigs = configs; 400 return this; 401 } 402 /** 403 * The initial configs used to configure the CANcoder of the swerve module. 404 * The default value is the factory-default. 405 * <p> 406 * Users may change the initial configuration as they need. 407 * Any config that's not referenced in the {@link SwerveModuleConstants} 408 * class is available to be changed. 409 * <p> 410 * The list of configs that will be overwritten is as follows: 411 * <ul> 412 * <li> {@link MagnetSensorConfigs#MagnetOffset} ({@link SwerveModuleConstants#CANcoderOffset}) 413 * </ul> 414 * 415 * @param configs Configs to set as an initial set of configs 416 * @return this object 417 */ 418 public SwerveModuleConstantsFactory withCANcoderInitialConfigs(CANcoderConfiguration configs) { 419 this.CANcoderInitialConfigs = configs; 420 return this; 421 } 422 423 /** 424 * Creates the constants for a swerve module with the given properties. 425 * 426 * @param steerId CAN ID of the steer motor 427 * @param driveId CAN ID of the drive motor 428 * @param cancoderId CAN ID of the CANcoder used for azimuth 429 * @param cancoderOffset Offset of the CANcoder in rotations 430 * @param locationX The location of this module's wheels relative to the physical 431 * center of the robot in meters along the X axis of the robot 432 * @param locationY The location of this module's wheels relative to the physical 433 * center of the robot in meters along the Y axis of the robot 434 * @param driveMotorReversed True if the driving motor is reversed 435 * @return Constants for the swerve module 436 */ 437 public SwerveModuleConstants createModuleConstants( 438 int steerId, 439 int driveId, 440 int cancoderId, 441 double cancoderOffset, 442 double locationX, 443 double locationY, 444 boolean driveMotorReversed) { 445 return new SwerveModuleConstants() 446 .withSteerMotorId(steerId) 447 .withDriveMotorId(driveId) 448 .withCANcoderId(cancoderId) 449 .withCANcoderOffset(cancoderOffset) 450 .withLocationX(locationX) 451 .withLocationY(locationY) 452 .withDriveMotorGearRatio(DriveMotorGearRatio) 453 .withSteerMotorGearRatio(SteerMotorGearRatio) 454 .withCouplingGearRatio(CouplingGearRatio) 455 .withWheelRadius(WheelRadius) 456 .withSlipCurrent(SlipCurrent) 457 .withSteerMotorGains(SteerMotorGains) 458 .withDriveMotorGains(DriveMotorGains) 459 .withSteerMotorClosedLoopOutput(SteerMotorClosedLoopOutput) 460 .withDriveMotorClosedLoopOutput(DriveMotorClosedLoopOutput) 461 .withSteerMotorInverted(SteerMotorInverted) 462 .withDriveMotorInverted(driveMotorReversed) 463 .withSpeedAt12VoltsMps(SpeedAt12VoltsMps) 464 .withSteerInertia(SteerInertia) 465 .withDriveInertia(DriveInertia) 466 .withSteerFrictionVoltage(SteerFrictionVoltage) 467 .withDriveFrictionVoltage(DriveFrictionVoltage) 468 .withFeedbackSource(FeedbackSource) 469 .withDriveMotorInitialConfigs(DriveMotorInitialConfigs) 470 .withSteerMotorInitialConfigs(SteerMotorInitialConfigs) 471 .withCANcoderInitialConfigs(CANcoderInitialConfigs); 472 } 473}