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