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