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.hardware; 008 009import java.util.ArrayList; 010import java.util.concurrent.locks.Lock; 011import java.util.concurrent.locks.ReentrantLock; 012 013import com.ctre.phoenix6.StatusCode; 014import com.ctre.phoenix6.StatusSignal; 015import com.ctre.phoenix6.configs.MotorOutputConfigs; 016import com.ctre.phoenix6.controls.ControlRequest; 017import com.ctre.phoenix6.controls.DutyCycleOut; 018import com.ctre.phoenix6.controls.NeutralOut; 019import com.ctre.phoenix6.controls.VoltageOut; 020import com.ctre.phoenix6.hardware.core.CoreTalonFX; 021import com.ctre.phoenix6.jni.PlatformJNI; 022import com.ctre.phoenix6.signals.InvertedValue; 023import com.ctre.phoenix6.signals.NeutralModeValue; 024import com.ctre.phoenix6.sim.DeviceType; 025import com.ctre.phoenix6.wpiutils.AutoFeedEnable; 026import com.ctre.phoenix6.wpiutils.CallbackHelper; 027import com.ctre.phoenix6.wpiutils.MotorSafetyImplem; 028 029import edu.wpi.first.wpilibj.motorcontrol.MotorController; 030import edu.wpi.first.wpilibj.simulation.CallbackStore; 031import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 032import edu.wpi.first.hal.HAL; 033import edu.wpi.first.hal.HALValue; 034import edu.wpi.first.hal.SimBoolean; 035import edu.wpi.first.hal.SimDevice; 036import edu.wpi.first.hal.SimDouble; 037import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 038import edu.wpi.first.hal.SimDevice.Direction; 039import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 040import edu.wpi.first.hal.simulation.SimValueCallback; 041import edu.wpi.first.util.sendable.Sendable; 042import edu.wpi.first.util.sendable.SendableBuilder; 043import edu.wpi.first.util.sendable.SendableRegistry; 044 045/** 046 * WPILib-integrated version of {@link CoreTalonFX} 047 */ 048public class TalonFX extends CoreTalonFX implements MotorController, Sendable, AutoCloseable { 049 /** 050 * The StatusSignal getters are copies so that calls 051 * to the WPI interface do not update any references 052 */ 053 private final StatusSignal<Double> m_dutyCycle = getDutyCycle().clone(); 054 055 private final DutyCycleOut m_setterControl = new DutyCycleOut(0); 056 private final NeutralOut m_neutralControl = new NeutralOut(); 057 private final VoltageOut m_voltageControl = new VoltageOut(0); 058 059 private final MotorOutputConfigs m_configs = new MotorOutputConfigs(); 060 061 private String m_description; 062 063 private static final DeviceType kSimDeviceType = DeviceType.PRO_TalonFXType; 064 065 private SimDevice m_simMotor; 066 private SimDouble m_simSupplyVoltage; 067 private SimDouble m_simDutyCycle; 068 private SimDouble m_simMotorVoltage; 069 private SimDouble m_simTorqueCurrent; 070 private SimDouble m_simSupplyCurrent; 071 072 private SimDevice m_simForwardLimit; 073 private SimBoolean m_simForwardLimitValue; 074 075 private SimDevice m_simReverseLimit; 076 private SimBoolean m_simReverseLimitValue; 077 078 private SimDevice m_simRotor; 079 private SimDouble m_simRotorPos; 080 private SimDouble m_simRotorRawPos; 081 private SimDouble m_simRotorVel; 082 private SimDouble m_simRotorAccel; 083 084 // callbacks to register 085 private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback(); 086 private final Runnable m_onPeriodicCallback = new OnPeriodicCallback(); 087 088 // returned registered callbacks 089 private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>(); 090 private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null; 091 092 /** 093 * The default motor safety timeout IF calling application 094 * enables the feature. 095 */ 096 public static final double kDefaultSafetyExpiration = 0.1; 097 098 /** 099 * Late-constructed motor safety, which ensures feature is off unless calling 100 * applications explicitly enables it. 101 */ 102 private MotorSafetyImplem m_motorSafety = null; 103 private double m_motSafeExpiration = kDefaultSafetyExpiration; 104 private final Lock m_motorSafetyLock = new ReentrantLock(); 105 106 /** 107 * Constructs a new Talon FX motor controller object. 108 * <p> 109 * Constructs the device using the default CAN bus for the system: 110 * <ul> 111 * <li>"rio" on roboRIO 112 * <li>"can0" on Linux 113 * <li>"*" on Windows 114 * </ul> 115 * 116 * @param deviceId ID of the device, as configured in Phoenix Tuner. 117 */ 118 public TalonFX(int deviceId) { 119 this(deviceId, ""); 120 } 121 122 /** 123 * Constructs a new Talon FX motor controller object. 124 * 125 * @param deviceId ID of the device, as configured in Phoenix Tuner. 126 * @param canbus Name of the CAN bus this device is on. Possible CAN bus 127 * strings are: 128 * <ul> 129 * <li>"rio" for the native roboRIO CAN bus 130 * <li>CANivore name or serial number 131 * <li>SocketCAN interface (non-FRC Linux only) 132 * <li>"*" for any CANivore seen by the program 133 * <li>empty string (default) to select the default for the 134 * system: 135 * <ul> 136 * <li>"rio" on roboRIO 137 * <li>"can0" on Linux 138 * <li>"*" on Windows 139 * </ul> 140 * </ul> 141 */ 142 public TalonFX(int deviceId, String canbus) { 143 super(deviceId, canbus); 144 145 m_description = "Talon FX (v6) " + deviceId; 146 SendableRegistry.addLW(this, "Talon FX (v6) ", deviceId); 147 148 m_simMotor = SimDevice.create("CANMotor:Talon FX (v6)", deviceId); 149 150 final String base = "Talon FX (v6)[" + deviceId + "]/"; 151 m_simRotor = SimDevice.create("CANEncoder:" + base + "Rotor Sensor"); 152 m_simForwardLimit = SimDevice.create("CANDIO:" + base + "Fwd Limit"); 153 m_simReverseLimit = SimDevice.create("CANDIO:" + base + "Rev Limit"); 154 155 if (m_simMotor != null) { 156 /* First make sure our simulated device gets enabled */ 157 AutoFeedEnable.getInstance().start(); 158 m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback); 159 160 m_simSupplyVoltage = m_simMotor.createDouble("supplyVoltage", Direction.kInput, 12.0); 161 162 m_simDutyCycle = m_simMotor.createDouble("dutyCycle", Direction.kOutput, 0); 163 m_simMotorVoltage = m_simMotor.createDouble("motorVoltage", Direction.kOutput, 0); 164 m_simTorqueCurrent = m_simMotor.createDouble("torqueCurrent", Direction.kOutput, 0); 165 m_simSupplyCurrent = m_simMotor.createDouble("supplyCurrent", Direction.kOutput, 0); 166 167 final SimDeviceSim sim = new SimDeviceSim("CANMotor:Talon FX (v6)"); 168 m_simValueChangedCallbacks 169 .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true)); 170 } 171 if (m_simRotor != null) { 172 m_simRotorPos = m_simRotor.createDouble("position", Direction.kOutput, 0); 173 174 m_simRotorRawPos = m_simRotor.createDouble("rawPositionInput", Direction.kInput, 0); 175 m_simRotorVel = m_simRotor.createDouble("velocity", Direction.kInput, 0); 176 m_simRotorAccel = m_simRotor.createDouble("acceleration", Direction.kInput, 0); 177 178 final SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Rotor Sensor"); 179 m_simValueChangedCallbacks 180 .add(sim.registerValueChangedCallback(m_simRotorRawPos, m_onValueChangedCallback, true)); 181 m_simValueChangedCallbacks 182 .add(sim.registerValueChangedCallback(m_simRotorVel, m_onValueChangedCallback, true)); 183 m_simValueChangedCallbacks 184 .add(sim.registerValueChangedCallback(m_simRotorAccel, m_onValueChangedCallback, true)); 185 } 186 if (m_simForwardLimit != null) { 187 m_simForwardLimit.createBoolean("init", Direction.kOutput, true); 188 m_simForwardLimit.createBoolean("input", Direction.kOutput, true); 189 190 m_simForwardLimitValue = m_simForwardLimit.createBoolean("value", Direction.kBidir, false); 191 192 final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Fwd Limit"); 193 m_simValueChangedCallbacks 194 .add(sim.registerValueChangedCallback(m_simForwardLimitValue, m_onValueChangedCallback, true)); 195 } 196 if (m_simReverseLimit != null) { 197 m_simReverseLimit.createBoolean("init", Direction.kOutput, true); 198 m_simReverseLimit.createBoolean("input", Direction.kOutput, true); 199 200 m_simReverseLimitValue = m_simReverseLimit.createBoolean("value", Direction.kBidir, false); 201 202 final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Rev Limit"); 203 m_simValueChangedCallbacks 204 .add(sim.registerValueChangedCallback(m_simReverseLimitValue, m_onValueChangedCallback, true)); 205 } 206 } 207 208 // ----- Callbacks for Sim ----- // 209 private class OnValueChangedCallback implements SimValueCallback { 210 @Override 211 public void callback(String name, int handle, int direction, HALValue value) { 212 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 213 String physType = deviceName + ":" + name; 214 PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(), 215 physType, CallbackHelper.getRawValue(value)); 216 } 217 } 218 219 private class OnPeriodicCallback implements Runnable { 220 @Override 221 public void run() { 222 double value = 0; 223 int err = 0; 224 225 final int deviceID = getDeviceID(); 226 227 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage"); 228 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 229 if (err == 0) { 230 m_simSupplyVoltage.set(value); 231 } 232 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "DutyCycle"); 233 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 234 if (err == 0) { 235 m_simDutyCycle.set(value); 236 } 237 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "MotorVoltage"); 238 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 239 if (err == 0) { 240 m_simMotorVoltage.set(value); 241 } 242 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "TorqueCurrent"); 243 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 244 if (err == 0) { 245 m_simTorqueCurrent.set(value); 246 } 247 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyCurrent"); 248 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 249 if (err == 0) { 250 m_simSupplyCurrent.set(value); 251 } 252 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorPosition"); 253 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 254 if (err == 0) { 255 m_simRotorPos.set(value); 256 } 257 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawRotorPosition"); 258 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 259 if (err == 0) { 260 m_simRotorRawPos.set(value); 261 } 262 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorVelocity"); 263 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 264 if (err == 0) { 265 m_simRotorVel.set(value); 266 } 267 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RotorAcceleration"); 268 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 269 if (err == 0) { 270 m_simRotorAccel.set(value); 271 } 272 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ForwardLimit"); 273 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 274 if (err == 0) { 275 m_simForwardLimitValue.set((int) value != 0); 276 } 277 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "ReverseLimit"); 278 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 279 if (err == 0) { 280 m_simReverseLimitValue.set((int) value != 0); 281 } 282 } 283 } 284 285 // ------- AutoCloseable ----- // 286 @Override 287 public void close() { 288 SendableRegistry.remove(this); 289 if (m_simPeriodicBeforeCallback != null) { 290 m_simPeriodicBeforeCallback.close(); 291 m_simPeriodicBeforeCallback = null; 292 } 293 if (m_simMotor != null) { 294 m_simMotor.close(); 295 m_simMotor = null; 296 } 297 if (m_simRotor != null) { 298 m_simRotor.close(); 299 m_simRotor = null; 300 } 301 if (m_simForwardLimit != null) { 302 m_simForwardLimit.close(); 303 m_simForwardLimit = null; 304 } 305 if (m_simReverseLimit != null) { 306 m_simReverseLimit.close(); 307 m_simReverseLimit = null; 308 } 309 310 for (var callback : m_simValueChangedCallbacks) { 311 callback.close(); 312 } 313 m_simValueChangedCallbacks.clear(); 314 315 AutoFeedEnable.getInstance().stop(); 316 } 317 318 // ------ set/get routines for WPILIB interfaces ------// 319 /** 320 * Common interface for setting the speed of a motor controller. 321 * 322 * @param speed The speed to set. Value should be between -1.0 and 1.0. 323 */ 324 @Override 325 public void set(double speed) { 326 feed(); 327 setControl(m_setterControl.withOutput(speed)); 328 } 329 330 /** 331 * Common interface for seting the direct voltage output of a motor controller. 332 * 333 * @param volts The voltage to output. 334 */ 335 @Override 336 public void setVoltage(double volts) { 337 feed(); 338 setControl(m_voltageControl.withOutput(volts)); 339 } 340 341 /** 342 * Common interface for getting the current set speed of a motor controller. 343 * 344 * @return The current set speed. Value is between -1.0 and 1.0. 345 */ 346 @Override 347 public double get() { 348 return m_dutyCycle.refresh().getValue(); 349 } 350 351 // ---------Intercept CTRE calls for motor safety ---------// 352 @Override 353 protected StatusCode setControlPrivate(ControlRequest request) { 354 /* intercept the control setter and feed motor-safety */ 355 feed(); 356 return super.setControlPrivate(request); 357 } 358 359 // ----------------------- turn-motor-off routines-------------------// 360 /** 361 * Common interface for disabling a motor controller. 362 */ 363 @Override 364 public void disable() { 365 setControl(m_neutralControl); 366 } 367 368 /** 369 * Common interface to stop motor movement until set is called again. 370 */ 371 @Override 372 public void stopMotor() { 373 disable(); 374 } 375 376 // ----------------------- Invert routines -------------------// 377 /** 378 * Common interface for inverting direction of a motor controller. 379 * <p> 380 * Since invert is a config, this API is blocking. We recommend that 381 * users avoid calling this API periodically. 382 * 383 * @param isInverted The state of inversion, true is inverted. 384 */ 385 @Override 386 public void setInverted(boolean isInverted) { 387 /* First read the configs so they're up-to-date */ 388 getConfigurator().refresh(m_configs); 389 /* Then set the invert config to the appropriate value */ 390 m_configs.Inverted = isInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; 391 getConfigurator().apply(m_configs); 392 } 393 394 /** 395 * Common interface for returning the inversion state of a motor controller. 396 * <p> 397 * Since invert is a config, this API is blocking. We recommend that 398 * users avoid calling this API periodically. 399 * 400 * @return The state of the inversion, true is inverted. 401 */ 402 @Override 403 public boolean getInverted() { 404 /* Read the configs so they're up-to-date */ 405 getConfigurator().refresh(m_configs); 406 /* Return true if we're Clockwise_Positive */ 407 return m_configs.Inverted == InvertedValue.Clockwise_Positive; 408 } 409 410 // -------------------- Neutral mode routines ----------------// 411 /** 412 * Sets the mode of operation when output is neutral or disabled. 413 * <p> 414 * Since neutral mode is a config, this API is blocking. We recommend 415 * that users avoid calling this API periodically. 416 * 417 * @param neutralMode The state of the motor controller bridge when output is neutral or disabled 418 */ 419 public void setNeutralMode(NeutralModeValue neutralMode) { 420 /* First read the configs so they're up-to-date */ 421 getConfigurator().refresh(m_configs); 422 /* Then set the neutral mode config to the appropriate value */ 423 m_configs.NeutralMode = neutralMode; 424 getConfigurator().apply(m_configs); 425 } 426 427 // ---- Sendable -------// 428 @Override 429 public void initSendable(SendableBuilder builder) { 430 builder.setSmartDashboardType("Motor Controller"); 431 builder.setActuator(true); 432 builder.setSafeState(this::stopMotor); 433 builder.addDoubleProperty("Value", this::get, this::set); 434 } 435 436 /** 437 * @return Description of motor controller 438 */ 439 public String getDescription() { 440 return m_description; 441 } 442 443 /* ----- Motor Safety ----- */ 444 /** caller must lock appropriately */ 445 private MotorSafetyImplem GetMotorSafety() { 446 if (m_motorSafety == null) { 447 /* newly created MS object */ 448 m_motorSafety = new MotorSafetyImplem(this, getDescription()); 449 /* apply the expiration timeout */ 450 m_motorSafety.setExpiration(m_motSafeExpiration); 451 } 452 return m_motorSafety; 453 } 454 455 /** 456 * Feed the motor safety object. 457 * <p> 458 * Resets the timer on this object that is used to do the timeouts. 459 */ 460 public void feed() { 461 m_motorSafetyLock.lock(); 462 try { 463 if (m_motorSafety == null) { 464 /* do nothing, MS features were never enabled */ 465 } else { 466 GetMotorSafety().feed(); 467 } 468 } finally { 469 m_motorSafetyLock.unlock(); 470 } 471 } 472 473 /** 474 * Set the expiration time for the corresponding motor safety object. 475 * 476 * @param expirationTime The timeout value in seconds. 477 */ 478 public void setExpiration(double expirationTime) { 479 m_motorSafetyLock.lock(); 480 try { 481 /* save the value for if/when we do create the MS object */ 482 m_motSafeExpiration = expirationTime; 483 /* apply it only if MS object exists */ 484 if (m_motorSafety == null) { 485 /* do nothing, MS features were never enabled */ 486 } else { 487 /* this call will trigger creating a registered MS object */ 488 GetMotorSafety().setExpiration(m_motSafeExpiration); 489 } 490 } finally { 491 m_motorSafetyLock.unlock(); 492 } 493 } 494 495 /** 496 * Retrieve the timeout value for the corresponding motor safety object. 497 * 498 * @return the timeout value in seconds. 499 */ 500 public double getExpiration() { 501 m_motorSafetyLock.lock(); 502 try { 503 /* return the intended expiration time */ 504 return m_motSafeExpiration; 505 } finally { 506 m_motorSafetyLock.unlock(); 507 } 508 } 509 510 /** 511 * Determine of the motor is still operating or has timed out. 512 * 513 * @return a true value if the motor is still operating normally and hasn't 514 * timed out. 515 */ 516 public boolean isAlive() { 517 m_motorSafetyLock.lock(); 518 try { 519 if (m_motorSafety == null) { 520 /* MC is alive - MS features were never enabled to neutral the MC. */ 521 return true; 522 } else { 523 return GetMotorSafety().isAlive(); 524 } 525 } finally { 526 m_motorSafetyLock.unlock(); 527 } 528 } 529 530 /** 531 * Enable/disable motor safety for this device. 532 * <p> 533 * Turn on and off the motor safety option for this object. 534 * 535 * @param enabled True if motor safety is enforced for this object. 536 */ 537 public void setSafetyEnabled(boolean enabled) { 538 m_motorSafetyLock.lock(); 539 try { 540 if (m_motorSafety == null && !enabled) { 541 /* 542 * Caller wants to disable MS, 543 * but MS features were nevere enabled, 544 * so it doesn't need to be disabled. 545 */ 546 } else { 547 /* MS will be created if it does not exist */ 548 GetMotorSafety().setSafetyEnabled(enabled); 549 } 550 } finally { 551 m_motorSafetyLock.unlock(); 552 } 553 } 554 555 /** 556 * Return the state of the motor safety enabled flag. 557 * <p> 558 * Return if the motor safety is currently enabled for this device. 559 * 560 * @return True if motor safety is enforced for this device 561 */ 562 public boolean isSafetyEnabled() { 563 m_motorSafetyLock.lock(); 564 try { 565 if (m_motorSafety == null) { 566 /* MS features were never enabled. */ 567 return false; 568 } else { 569 return GetMotorSafety().isSafetyEnabled(); 570 } 571 } finally { 572 m_motorSafetyLock.unlock(); 573 } 574 } 575}