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.BaseStatusSignal; 010import com.ctre.phoenix6.StatusCode; 011import com.ctre.phoenix6.StatusSignal; 012import com.ctre.phoenix6.configs.CANcoderConfiguration; 013import com.ctre.phoenix6.configs.CurrentLimitsConfigs; 014import com.ctre.phoenix6.configs.MotorOutputConfigs; 015import com.ctre.phoenix6.configs.TalonFXConfiguration; 016import com.ctre.phoenix6.configs.TorqueCurrentConfigs; 017import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC; 018import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; 019import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; 020import com.ctre.phoenix6.controls.MotionMagicVoltage; 021import com.ctre.phoenix6.controls.TorqueCurrentFOC; 022import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; 023import com.ctre.phoenix6.controls.VelocityVoltage; 024import com.ctre.phoenix6.controls.VoltageOut; 025import com.ctre.phoenix6.hardware.CANcoder; 026import com.ctre.phoenix6.hardware.TalonFX; 027import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; 028import com.ctre.phoenix6.signals.InvertedValue; 029import com.ctre.phoenix6.signals.NeutralModeValue; 030 031import edu.wpi.first.math.geometry.Rotation2d; 032import edu.wpi.first.math.kinematics.SwerveModulePosition; 033import edu.wpi.first.math.kinematics.SwerveModuleState; 034import edu.wpi.first.math.util.Units; 035 036/** 037 * Swerve Module class that encapsulates a swerve module powered by CTR 038 * Electronics devices. 039 * <p> 040 * This class handles the hardware devices and configures them for 041 * swerve module operation using the Phoenix 6 API. 042 * <p> 043 * This class constructs hardware devices internally, so the user 044 * only specifies the constants (IDs, PID gains, gear ratios, etc). 045 * Getters for these hardware devices are available. 046 */ 047public class SwerveModule { 048 /** 049 * Supported closed-loop output types. 050 */ 051 public enum ClosedLoopOutputType { 052 Voltage, 053 /** Requires Pro */ 054 TorqueCurrentFOC, 055 } 056 057 /** 058 * All possible control requests for the module steer motor. 059 */ 060 public enum SteerRequestType { 061 /** 062 * Control the drive motor using a Motion Magic® request. 063 * The control output type is determined by {@link SwerveModuleConstants#SteerMotorClosedLoopOutput} 064 */ 065 MotionMagic, 066 /** 067 * Control the drive motor using a Motion Magic® Expo request. 068 * The control output type is determined by {@link SwerveModuleConstants#SteerMotorClosedLoopOutput} 069 */ 070 MotionMagicExpo, 071 } 072 073 /** 074 * All possible control requests for the module drive motor. 075 */ 076 public enum DriveRequestType { 077 /** 078 * Control the drive motor using an open-loop voltage request. 079 */ 080 OpenLoopVoltage, 081 /** 082 * Control the drive motor using a velocity closed-loop request. 083 * The control output type is determined by {@link SwerveModuleConstants#DriveMotorClosedLoopOutput} 084 */ 085 Velocity, 086 } 087 088 private final TalonFX m_driveMotor; 089 private final TalonFX m_steerMotor; 090 private final CANcoder m_cancoder; 091 092 private final StatusSignal<Double> m_drivePosition; 093 private final StatusSignal<Double> m_driveVelocity; 094 private final StatusSignal<Double> m_steerPosition; 095 private final StatusSignal<Double> m_steerVelocity; 096 private final BaseStatusSignal[] m_signals; 097 private final double m_driveRotationsPerMeter; 098 private final double m_couplingRatioDriveRotorToCANcoder; 099 100 private final double m_speedAt12VoltsMps; 101 102 /* steer motor controls */ 103 private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); 104 private final MotionMagicTorqueCurrentFOC m_angleTorqueSetter = new MotionMagicTorqueCurrentFOC(0); 105 private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); 106 private final MotionMagicExpoTorqueCurrentFOC m_angleTorqueExpoSetter = new MotionMagicExpoTorqueCurrentFOC(0); 107 /* drive motor controls */ 108 private final VoltageOut m_voltageOpenLoopSetter = new VoltageOut(0); 109 private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); 110 /* Velocity Torque current neutral should always be coast, as neutral corresponds to 0-current or maintain velocity, not 0-velocity */ 111 private final VelocityTorqueCurrentFOC m_velocityTorqueSetter = new VelocityTorqueCurrentFOC(0).withOverrideCoastDurNeutral(true); 112 113 private final ClosedLoopOutputType m_steerClosedLoopOutput; 114 private final ClosedLoopOutputType m_driveClosedLoopOutput; 115 116 private final SwerveModulePosition m_internalState = new SwerveModulePosition(); 117 private SwerveModuleState m_targetState = new SwerveModuleState(); 118 119 /** 120 * Construct a SwerveModule with the specified constants. 121 * 122 * @param constants Constants used to construct the module 123 * @param canbusName The name of the CAN bus this module is on 124 */ 125 public SwerveModule(SwerveModuleConstants constants, String canbusName) { 126 m_driveMotor = new TalonFX(constants.DriveMotorId, canbusName); 127 m_steerMotor = new TalonFX(constants.SteerMotorId, canbusName); 128 m_cancoder = new CANcoder(constants.CANcoderId, canbusName); 129 130 TalonFXConfiguration driveConfigs = constants.DriveMotorInitialConfigs; 131 driveConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; 132 133 driveConfigs.Slot0 = constants.DriveMotorGains; 134 driveConfigs.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; 135 driveConfigs.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; 136 driveConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; 137 driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true; 138 139 driveConfigs.MotorOutput.Inverted = constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive 140 : InvertedValue.CounterClockwise_Positive; 141 StatusCode response = m_driveMotor.getConfigurator().apply(driveConfigs); 142 if (!response.isOK()) { 143 System.out.println( 144 "TalonFX ID " + m_driveMotor.getDeviceID() + " failed config with error " + response.toString()); 145 } 146 147 TalonFXConfiguration steerConfigs = constants.SteerMotorInitialConfigs; 148 steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; 149 150 steerConfigs.Slot0 = constants.SteerMotorGains; 151 // Modify configuration to use remote CANcoder fused 152 steerConfigs.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; 153 switch (constants.FeedbackSource) { 154 case RemoteCANcoder: 155 steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; 156 break; 157 case FusedCANcoder: 158 steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; 159 break; 160 case SyncCANcoder: 161 steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder; 162 break; 163 } 164 steerConfigs.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; 165 166 steerConfigs.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; 167 steerConfigs.MotionMagic.MotionMagicAcceleration = steerConfigs.MotionMagic.MotionMagicCruiseVelocity / 0.100; 168 steerConfigs.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; 169 steerConfigs.MotionMagic.MotionMagicExpo_kA = 0.1; 170 171 steerConfigs.ClosedLoopGeneral.ContinuousWrap = true; // Enable continuous wrap for swerve modules 172 173 steerConfigs.MotorOutput.Inverted = constants.SteerMotorInverted 174 ? InvertedValue.Clockwise_Positive 175 : InvertedValue.CounterClockwise_Positive; 176 response = m_steerMotor.getConfigurator().apply(steerConfigs); 177 if (!response.isOK()) { 178 System.out.println( 179 "TalonFX ID " + m_steerMotor.getDeviceID() + " failed config with error: " + response.toString()); 180 } 181 182 CANcoderConfiguration cancoderConfigs = constants.CANcoderInitialConfigs; 183 cancoderConfigs.MagnetSensor.MagnetOffset = constants.CANcoderOffset; 184 response = m_cancoder.getConfigurator().apply(cancoderConfigs); 185 if (!response.isOK()) { 186 System.out.println( 187 "CANcoder ID " + m_cancoder.getDeviceID() + " failed config with error: " + response.toString()); 188 } 189 190 m_drivePosition = m_driveMotor.getPosition().clone(); 191 m_driveVelocity = m_driveMotor.getVelocity().clone(); 192 m_steerPosition = m_steerMotor.getPosition().clone(); 193 m_steerVelocity = m_steerMotor.getVelocity().clone(); 194 195 m_signals = new BaseStatusSignal[4]; 196 m_signals[0] = m_drivePosition; 197 m_signals[1] = m_driveVelocity; 198 m_signals[2] = m_steerPosition; 199 m_signals[3] = m_steerVelocity; 200 201 /* Calculate the ratio of drive motor rotation to meter on ground */ 202 double rotationsPerWheelRotation = constants.DriveMotorGearRatio; 203 double metersPerWheelRotation = 2 * Math.PI * Units.inchesToMeters(constants.WheelRadius); 204 m_driveRotationsPerMeter = rotationsPerWheelRotation / metersPerWheelRotation; 205 m_couplingRatioDriveRotorToCANcoder = constants.CouplingGearRatio; 206 207 /* Make control requests synchronous */ 208 m_angleVoltageSetter.UpdateFreqHz = 0; 209 m_angleTorqueSetter.UpdateFreqHz = 0; 210 m_angleVoltageExpoSetter.UpdateFreqHz = 0; 211 m_angleTorqueExpoSetter.UpdateFreqHz = 0; 212 213 m_velocityTorqueSetter.UpdateFreqHz = 0; 214 m_velocityVoltageSetter.UpdateFreqHz = 0; 215 m_voltageOpenLoopSetter.UpdateFreqHz = 0; 216 217 /* Set the drive motor closed-loop output type */ 218 m_steerClosedLoopOutput = constants.SteerMotorClosedLoopOutput; 219 m_driveClosedLoopOutput = constants.DriveMotorClosedLoopOutput; 220 221 /* Get the expected speed when applying 12 volts */ 222 m_speedAt12VoltsMps = constants.SpeedAt12VoltsMps; 223 } 224 225 /** 226 * Gets the state of this module and passes it back as a 227 * SwerveModulePosition object with latency compensated values. 228 * 229 * @param refresh True if the signals should be refreshed 230 * @return SwerveModulePosition containing this module's state. 231 */ 232 public SwerveModulePosition getPosition(boolean refresh) { 233 if (refresh) { 234 /* Refresh all signals */ 235 m_drivePosition.refresh(); 236 m_driveVelocity.refresh(); 237 m_steerPosition.refresh(); 238 m_steerVelocity.refresh(); 239 } 240 241 /* Now latency-compensate our signals */ 242 double drive_rot = BaseStatusSignal.getLatencyCompensatedValue(m_drivePosition, m_driveVelocity); 243 double angle_rot = BaseStatusSignal.getLatencyCompensatedValue(m_steerPosition, m_steerVelocity); 244 245 /* 246 * Back out the drive rotations based on angle rotations due to coupling between 247 * azimuth and steer 248 */ 249 drive_rot -= angle_rot * m_couplingRatioDriveRotorToCANcoder; 250 251 /* And push them into a SwerveModulePosition object to return */ 252 m_internalState.distanceMeters = drive_rot / m_driveRotationsPerMeter; 253 /* Angle is already in terms of steer rotations */ 254 m_internalState.angle = Rotation2d.fromRotations(angle_rot); 255 256 return m_internalState; 257 } 258 259 /** 260 * Applies the desired SwerveModuleState to this module. 261 * 262 * @param state Speed and direction the module should target 263 * @param driveRequestType The {@link DriveRequestType} to apply 264 */ 265 public void apply(SwerveModuleState state, DriveRequestType driveRequestType) { 266 apply(state, driveRequestType, SteerRequestType.MotionMagic); 267 } 268 269 /** 270 * Applies the desired SwerveModuleState to this module. 271 * 272 * @param state Speed and direction the module should target 273 * @param driveRequestType The {@link DriveRequestType} to apply 274 * @param steerRequestType The {@link SteerRequestType} to apply; defaults to {@link SteerRequestType#MotionMagic} 275 */ 276 public void apply(SwerveModuleState state, DriveRequestType driveRequestType, SteerRequestType steerRequestType) { 277 var optimized = SwerveModuleState.optimize(state, m_internalState.angle); 278 m_targetState = optimized; 279 280 double angleToSetDeg = optimized.angle.getRotations(); 281 switch (steerRequestType) { 282 case MotionMagic: 283 switch (m_steerClosedLoopOutput) { 284 case Voltage: 285 m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg)); 286 break; 287 288 case TorqueCurrentFOC: 289 m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg)); 290 break; 291 } 292 break; 293 294 case MotionMagicExpo: 295 switch (m_steerClosedLoopOutput) { 296 case Voltage: 297 m_steerMotor.setControl(m_angleVoltageExpoSetter.withPosition(angleToSetDeg)); 298 break; 299 300 case TorqueCurrentFOC: 301 m_steerMotor.setControl(m_angleTorqueExpoSetter.withPosition(angleToSetDeg)); 302 break; 303 } 304 break; 305 } 306 307 double velocityToSet = optimized.speedMetersPerSecond * m_driveRotationsPerMeter; 308 309 /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ 310 /* To reduce the "skew" that occurs when changing direction */ 311 double steerMotorError = angleToSetDeg - m_steerPosition.getValue(); 312 /* If error is close to 0 rotations, we're already there, so apply full power */ 313 /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ 314 double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError)); 315 /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ 316 if (cosineScalar < 0.0) { 317 cosineScalar = 0.0; 318 } 319 velocityToSet *= cosineScalar; 320 321 /* Back out the expected shimmy the drive motor will see */ 322 /* Find the angular rate to determine what to back out */ 323 double azimuthTurnRps = m_steerVelocity.getValue(); 324 /* Azimuth turn rate multiplied by coupling ratio provides back-out rps */ 325 double driveRateBackOut = azimuthTurnRps * m_couplingRatioDriveRotorToCANcoder; 326 velocityToSet += driveRateBackOut; 327 328 switch (driveRequestType) { 329 case OpenLoopVoltage: 330 /* Open loop ignores the driveRotationsPerMeter since it only cares about the open loop at the mechanism */ 331 /* But we do care about the backout due to coupling, so we keep it in */ 332 velocityToSet /= m_driveRotationsPerMeter; 333 m_driveMotor.setControl(m_voltageOpenLoopSetter.withOutput(velocityToSet / m_speedAt12VoltsMps * 12.0)); 334 break; 335 336 case Velocity: 337 switch (m_driveClosedLoopOutput) { 338 case Voltage: 339 m_driveMotor.setControl(m_velocityVoltageSetter.withVelocity(velocityToSet)); 340 break; 341 342 case TorqueCurrentFOC: 343 m_driveMotor.setControl(m_velocityTorqueSetter.withVelocity(velocityToSet)); 344 break; 345 } 346 break; 347 } 348 } 349 350 /** 351 * Controls this module to the specified steer target, and applies the specific drive request. 352 * <p> 353 * This is intended only to be used for characterization of the robot, do not use this for normal use. 354 * 355 * @param steerTarget The angle the wheels should face for characterization 356 * @param driveRequest The direct voltage to apply to the motor for use during characterization 357 */ 358 public void applyCharacterization(Rotation2d steerTarget, VoltageOut driveRequest) 359 { 360 double angleToSetDeg = steerTarget.getRotations(); 361 /* Use the configured closed loop output mode */ 362 switch (m_steerClosedLoopOutput) { 363 case Voltage: 364 m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg)); 365 break; 366 367 case TorqueCurrentFOC: 368 m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg)); 369 break; 370 } 371 372 /* And apply the high-level drive request */ 373 m_driveMotor.setControl(driveRequest); 374 } 375 376 /** 377 * Controls this module to the specified steer target, and applies the specific drive request. 378 * <p> 379 * This is intended only to be used for characterization of the robot, do not use this for normal use. 380 * 381 * @param steerTarget The angle the wheels should face for characterization 382 * @param driveRequest The direct Torque Current to apply to the motor for use during characterization 383 */ 384 public void applyCharacterization(Rotation2d steerTarget, TorqueCurrentFOC driveRequest) 385 { 386 double angleToSetDeg = steerTarget.getRotations(); 387 /* Use the configured closed loop output mode */ 388 switch (m_steerClosedLoopOutput) { 389 case Voltage: 390 m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg)); 391 break; 392 393 case TorqueCurrentFOC: 394 m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg)); 395 break; 396 } 397 398 /* And apply the high-level drive request */ 399 m_driveMotor.setControl(driveRequest); 400 } 401 402 /** 403 * Configures the neutral mode to use for the module's drive motor. 404 * 405 * @param neutralMode The drive motor neutral mode 406 * @return Status code response of the request 407 */ 408 public StatusCode configNeutralMode(NeutralModeValue neutralMode) { 409 var configs = new MotorOutputConfigs(); 410 411 /* First read the configs so they're up-to-date */ 412 StatusCode status = m_driveMotor.getConfigurator().refresh(configs); 413 if (status.isOK()) { 414 /* Then set the neutral mode config to the appropriate value */ 415 configs.NeutralMode = neutralMode; 416 status = m_driveMotor.getConfigurator().apply(configs); 417 } 418 if (!status.isOK()) { 419 System.out.println( 420 "TalonFX ID " + m_driveMotor.getDeviceID() + " failed config neutral mode with error " + status.toString()); 421 } 422 return status; 423 } 424 425 /** 426 * Gets the last cached swerve module position. 427 * This differs from {@link getPosition} in that it will not 428 * perform any latency compensation or refresh the signals. 429 * 430 * @return Last cached SwerveModulePosition 431 */ 432 public SwerveModulePosition getCachedPosition() { 433 return m_internalState; 434 } 435 436 /** 437 * Get the current state of the module. 438 * <p> 439 * This is typically used for telemetry, as the SwerveModulePosition 440 * is used for odometry. 441 * 442 * @return Current state of the module 443 */ 444 public SwerveModuleState getCurrentState() { 445 return new SwerveModuleState(m_driveVelocity.getValue() / m_driveRotationsPerMeter, Rotation2d.fromRotations(m_steerPosition.getValue())); 446 } 447 448 /** 449 * Get the target state of the module. 450 * <p> 451 * This is typically used for telemetry. 452 * 453 * @return Target state of the module 454 */ 455 public SwerveModuleState getTargetState() { 456 return m_targetState; 457 } 458 459 /** 460 * Gets the position/velocity signals of the drive and steer 461 * 462 * @return Array of BaseStatusSignals for this module in the following order: 463 * 0 - Drive Position 464 * 1 - Drive Velocity 465 * 2 - Steer Position 466 * 3 - Steer Velocity 467 */ 468 BaseStatusSignal[] getSignals() { 469 return m_signals; 470 } 471 472 /** 473 * Resets this module's drive motor position to 0 rotations. 474 */ 475 public void resetPosition() { 476 /* Only touch drive pos, not steer */ 477 m_driveMotor.setPosition(0); 478 } 479 480 /** 481 * Gets this module's Drive Motor TalonFX reference. 482 * <p> 483 * This should be used only to access signals and change configurations that the 484 * swerve drivetrain does not configure itself. 485 * 486 * @return This module's Drive Motor reference 487 */ 488 public TalonFX getDriveMotor() { 489 return m_driveMotor; 490 } 491 492 /** 493 * Gets this module's Steer Motor TalonFX reference. 494 * <p> 495 * This should be used only to access signals and change configurations that the 496 * swerve drivetrain does not configure itself. 497 * 498 * @return This module's Steer Motor reference 499 */ 500 public TalonFX getSteerMotor() { 501 return m_steerMotor; 502 } 503 504 /** 505 * Gets this module's CANcoder reference. 506 * <p> 507 * This should be used only to access signals and change configurations that the 508 * swerve drivetrain does not configure itself. 509 * 510 * @return This module's CANcoder reference 511 */ 512 public CANcoder getCANcoder() { 513 return m_cancoder; 514 } 515}