001/** 002 * WPI Compliant motor controller class. 003 * WPILIB's object model requires many interfaces to be implemented to use 004 * the various features. 005 * This includes... 006 * - Software PID loops running in the robot controller 007 * - LiveWindow/Test mode features 008 * - Motor Safety (auto-turn off of motor if Set stops getting called) 009 * - Single Parameter set that assumes a simple motor controller. 010 */ 011package com.ctre.phoenix.motorcontrol.can; 012 013import com.ctre.phoenix.motorcontrol.ControlMode; 014import com.ctre.phoenix.motorcontrol.DemandType; 015import com.ctre.phoenix.motorcontrol.WPI_AutoFeedEnable; 016import com.ctre.phoenix.motorcontrol.WPI_MotorSafetyImplem; 017import com.ctre.phoenix.platform.DeviceType; 018import com.ctre.phoenix.platform.PlatformJNI; 019import com.ctre.phoenix.Logger; 020import com.ctre.phoenix.WPI_CallbackHelper; 021import com.ctre.phoenix.ErrorCode; 022 023import java.util.ArrayList; 024 025import edu.wpi.first.wpilibj.RobotController; 026import edu.wpi.first.wpilibj.motorcontrol.MotorController; 027import edu.wpi.first.wpilibj.simulation.CallbackStore; 028import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 029import edu.wpi.first.util.sendable.SendableRegistry; 030import edu.wpi.first.wpilibj.DriverStation; 031import edu.wpi.first.util.sendable.Sendable; 032import edu.wpi.first.util.sendable.SendableBuilder; 033import edu.wpi.first.hal.FRCNetComm.tResourceType; 034import edu.wpi.first.hal.SimDevice.Direction; 035import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 036import edu.wpi.first.hal.simulation.SimValueCallback; 037import edu.wpi.first.hal.simulation.SimulatorJNI; 038import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 039import edu.wpi.first.hal.HAL; 040import edu.wpi.first.hal.HALValue; 041import edu.wpi.first.hal.SimDevice; 042import edu.wpi.first.hal.SimDouble; 043import edu.wpi.first.hal.SimBoolean; 044 045/** 046 * CTRE Talon FX Motor Controller when used on CAN Bus. 047 * 048 * @deprecated This device's Phoenix 5 API is deprecated for removal in the 049 * 2025 season. Users should update to Phoenix 6 firmware and migrate to the 050 * Phoenix 6 API. A migration guide is available at 051 * https://v6.docs.ctr-electronics.com/en/stable/docs/migration/migration-guide/index.html. 052 * <p> 053 * If the Phoenix 5 API must be used for this device, the device must have 22.X 054 * firmware. This firmware is available in Tuner X after selecting Phoenix 5 in 055 * the firmware year dropdown. 056 */ 057@Deprecated(since = "2024", forRemoval = true) 058public class WPI_TalonFX extends TalonFX implements MotorController, Sendable, AutoCloseable { 059 060 private String _description; 061 private double _speed; 062 063 private SimDevice m_simMotor; 064 private SimDouble m_simPercOut; 065 private SimDouble m_simMotorOutputLeadVoltage; 066 private SimDouble m_simSupplyCurrent; 067 private SimDouble m_simMotorCurrent; 068 private SimDouble m_simVbat; 069 070 private SimDevice m_simIntegSens; 071 private SimDouble m_simIntegSensPos; 072 private SimDouble m_simIntegSensAbsPos; 073 private SimDouble m_simIntegSensRawPos; 074 private SimDouble m_simIntegSensVel; 075 076 private SimDevice m_simFwdLim; 077 private SimBoolean m_simFwdLimInit; 078 private SimBoolean m_simFwdLimInput; 079 private SimBoolean m_simFwdLimValue; 080 081 private SimDevice m_simRevLim; 082 private SimBoolean m_simRevLimInit; 083 private SimBoolean m_simRevLimInput; 084 private SimBoolean m_simRevLimValue; 085 086 // callbacks to register 087 private SimValueCallback onValueChangedCallback = new OnValueChangedCallback(); 088 private Runnable onPeriodicCallback = new OnPeriodicCallback(); 089 090 // returned registered callbacks 091 private ArrayList<CallbackStore> simValueChangedCallbacks = new ArrayList<CallbackStore>(); 092 private SimPeriodicBeforeCallback simPeriodicCallback; 093 094 /** 095 * The default motor safety timeout IF calling application 096 * enables the feature. 097 */ 098 public static final double kDefaultSafetyExpiration = 0.1; 099 100 /** 101 * Late-constructed motor safety, which ensures feature is off unless calling 102 * applications explicitly enables it. 103 */ 104 private WPI_MotorSafetyImplem _motorSafety = null; 105 private final Object _lockMotorSaf = new Object(); 106 private double _motSafeExpiration = kDefaultSafetyExpiration; 107 108 /** 109 * Constructor for motor controller 110 * @param deviceNumber device ID of motor controller 111 * @param canbus Name of the CANbus; can be a CANivore device name or serial number. 112 * Pass in nothing or "rio" to use the roboRIO. 113 */ 114 public WPI_TalonFX(int deviceNumber, String canbus) { 115 super(deviceNumber, canbus); 116 _description = "Talon FX " + deviceNumber; 117 118 SendableRegistry.addLW(this, "Talon FX ", deviceNumber); 119 120 m_simMotor = SimDevice.create("CANMotor:Talon FX", deviceNumber); 121 if(m_simMotor != null){ 122 WPI_AutoFeedEnable.getInstance(); 123 simPeriodicCallback = HAL.registerSimPeriodicBeforeCallback(onPeriodicCallback); 124 125 m_simPercOut = m_simMotor.createDouble("percentOutput", Direction.kOutput, 0); 126 m_simMotorOutputLeadVoltage = m_simMotor.createDouble("motorOutputLeadVoltage", Direction.kOutput, 0); 127 128 m_simSupplyCurrent = m_simMotor.createDouble("supplyCurrent", Direction.kInput, 0); 129 m_simMotorCurrent = m_simMotor.createDouble("motorCurrent", Direction.kInput, 0); 130 m_simVbat = m_simMotor.createDouble("busVoltage", Direction.kInput, 12.0); 131 132 SimDeviceSim sim = new SimDeviceSim("CANMotor:Talon FX"); 133 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simSupplyCurrent, onValueChangedCallback, true)); 134 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simMotorCurrent, onValueChangedCallback, true)); 135 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVbat, onValueChangedCallback, true)); 136 } 137 138 String base = "Talon FX[" + deviceNumber + "]/"; 139 m_simIntegSens = SimDevice.create("CANEncoder:" + base + "Integrated Sensor"); 140 if(m_simIntegSens != null){ 141 m_simIntegSensPos = m_simIntegSens.createDouble("position", Direction.kOutput, 0); 142 m_simIntegSensAbsPos = m_simIntegSens.createDouble("absolutePosition", Direction.kOutput, 0); 143 144 m_simIntegSensRawPos = m_simIntegSens.createDouble("rawPositionInput", Direction.kInput, 0); 145 m_simIntegSensVel = m_simIntegSens.createDouble("velocity", Direction.kInput, 0); 146 147 SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Integrated Sensor"); 148 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simIntegSensRawPos, onValueChangedCallback, true)); 149 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simIntegSensVel, onValueChangedCallback, true)); 150 } 151 152 m_simFwdLim = SimDevice.create("CANDIO:" + base + "Fwd Limit"); 153 if(m_simFwdLim != null){ 154 m_simFwdLimInit = m_simFwdLim.createBoolean("init", Direction.kOutput, true); 155 m_simFwdLimInput = m_simFwdLim.createBoolean("input", Direction.kOutput, true); 156 157 m_simFwdLimValue = m_simFwdLim.createBoolean("value", Direction.kBidir, false); 158 159 SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Fwd Limit"); 160 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simFwdLimValue, onValueChangedCallback, true)); 161 } 162 163 m_simRevLim = SimDevice.create("CANDIO:" + base + "Rev Limit"); 164 if(m_simRevLim != null){ 165 m_simRevLimInit = m_simRevLim.createBoolean("init", Direction.kOutput, true); 166 m_simRevLimInput = m_simRevLim.createBoolean("input", Direction.kOutput, true); 167 168 m_simRevLimValue = m_simRevLim.createBoolean("value", Direction.kBidir, false); 169 170 SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "Rev Limit"); 171 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRevLimValue, onValueChangedCallback, true)); 172 } 173 } 174 175 /** 176 * Constructor for motor controller 177 * @param deviceNumber device ID of motor controller 178 */ 179 public WPI_TalonFX(int deviceNumber) { 180 this(deviceNumber, ""); 181 } 182 183 // ----- Auto-Closable ----- // 184 @Override 185 public void close(){ 186 SendableRegistry.remove(this); 187 if(m_simMotor != null) { 188 m_simMotor.close(); 189 m_simMotor = null; 190 } 191 if(m_simIntegSens != null) { 192 m_simIntegSens.close(); 193 m_simIntegSens = null; 194 } 195 if(m_simFwdLim != null) { 196 m_simFwdLim.close(); 197 m_simFwdLim = null; 198 } 199 if(m_simRevLim != null) { 200 m_simRevLim.close(); 201 m_simRevLim = null; 202 } 203 super.DestroyObject(); //Destroy the device 204 } 205 206 // ----- Callbacks for Sim ----- // 207 private class OnValueChangedCallback implements SimValueCallback { 208 @Override 209 public void callback(String name, int handle, int direction, HALValue value) { 210 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 211 String physType = deviceName + ":" + name; 212 PlatformJNI.JNI_SimSetPhysicsInput(DeviceType.TalonFX.value, getDeviceID(), 213 physType, WPI_CallbackHelper.getRawValue(value)); 214 } 215 } 216 217 private class OnPeriodicCallback implements Runnable { 218 @Override 219 public void run() { 220 double value = 0; 221 int err = 0; 222 223 int deviceID = getDeviceID(); 224 225 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "PercentOutput"); 226 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 227 if (err == 0) { 228 m_simPercOut.set(value); 229 } 230 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "MotorOutputLeadVoltage"); 231 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 232 if (err == 0) { 233 m_simMotorOutputLeadVoltage.set(value); 234 } 235 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "BusVoltage"); 236 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 237 if (err == 0) { 238 m_simVbat.set(value); 239 } 240 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "CurrentSupply"); 241 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 242 if (err == 0) { 243 m_simSupplyCurrent.set(value); 244 } 245 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "CurrentStator"); 246 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 247 if (err == 0) { 248 m_simMotorCurrent.set(value); 249 } 250 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensPos"); 251 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 252 if (err == 0) { 253 m_simIntegSensPos.set(value); 254 } 255 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensAbsPos"); 256 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 257 if (err == 0) { 258 m_simIntegSensAbsPos.set(value); 259 } 260 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensRawPos"); 261 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 262 if (err == 0) { 263 m_simIntegSensRawPos.set(value); 264 } 265 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "IntegSensVel"); 266 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 267 if (err == 0) { 268 m_simIntegSensVel.set(value); 269 } 270 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "LimitFwd"); 271 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 272 if (err == 0) { 273 m_simFwdLimValue.set((int)value != 0); 274 } 275 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.TalonFX.value, deviceID, "LimitRev"); 276 err = PlatformJNI.JNI_SimGetLastError(DeviceType.TalonFX.value, deviceID); 277 if (err == 0) { 278 m_simRevLimValue.set((int)value != 0); 279 } 280 } 281 } 282 283 // ------ set/get routines for WPILIB interfaces ------// 284 /** 285 * Common interface for setting the speed of a simple speed controller. 286 * 287 * @param speed The speed to set. Value should be between -1.0 and 1.0. 288 * Value is also saved for Get(). 289 */ 290 @Override 291 public void set(double speed) { 292 _speed = speed; 293 set(ControlMode.PercentOutput, _speed); 294 feed(); 295 } 296 297 /** 298 * Common interface for getting the current set speed of a speed controller. 299 * 300 * @return The current set speed. Value is between -1.0 and 1.0. 301 */ 302 @Override 303 public double get() { 304 return _speed; 305 } 306 307 // ---------Intercept CTRE calls for motor safety ---------// 308 /** 309 * Sets the appropriate output on the talon, depending on the mode. 310 * @param mode The output mode to apply. 311 * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. 312 * In Current mode, output value is in amperes. 313 * In Velocity mode, output value is in position change / 100ms. 314 * In Position mode, output value is in encoder ticks or an analog value, 315 * depending on the sensor. 316 * In Follower mode, the output value is the integer device ID of the talon to 317 * duplicate. 318 * 319 * @param value The setpoint value, as described above. 320 * 321 * 322 * Standard Driving Example: 323 * _talonLeft.set(ControlMode.PercentOutput, leftJoy); 324 * _talonRght.set(ControlMode.PercentOutput, rghtJoy); 325 */ 326 public void set(ControlMode mode, double value) { 327 /* intercept the advanced Set and feed motor-safety */ 328 super.set(mode, value); 329 feed(); 330 } 331 332 /** 333 * @param mode Sets the appropriate output on the talon, depending on the mode. 334 * @param demand0 The output value to apply. 335 * such as advanced feed forward and/or auxiliary close-looping in firmware. 336 * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. 337 * In Current mode, output value is in amperes. 338 * In Velocity mode, output value is in position change / 100ms. 339 * In Position mode, output value is in encoder ticks or an analog value, 340 * depending on the sensor. See 341 * In Follower mode, the output value is the integer device ID of the talon to 342 * duplicate. 343 * 344 * @param demand1Type The demand type for demand1. 345 * Neutral: Ignore demand1 and apply no change to the demand0 output. 346 * AuxPID: Use demand1 to set the target for the auxiliary PID 1. 347 * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the 348 * demand0 output. In PercentOutput the demand0 output is the motor output, 349 * and in closed-loop modes the demand0 output is the output of PID0. 350 * @param demand1 Supplmental output value. Units match the set mode. 351 * 352 * 353 * Arcade Drive Example: 354 * _talonLeft.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, +joyTurn); 355 * _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, -joyTurn); 356 * 357 * Drive Straight Example: 358 * Note: Selected Sensor Configuration is necessary for both PID0 and PID1. 359 * _talonLeft.follow(_talonRght, FollwerType.AuxOutput1); 360 * _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.AuxPID, desiredRobotHeading); 361 * 362 * Drive Straight to a Distance Example: 363 * Note: Other configurations (sensor selection, PID gains, etc.) need to be set. 364 * _talonLeft.follow(_talonRght, FollwerType.AuxOutput1); 365 * _talonRght.set(ControlMode.MotionMagic, targetDistance, DemandType.AuxPID, desiredRobotHeading); 366 */ 367 public void set(ControlMode mode, double demand0, DemandType demand1Type, double demand1){ 368 /* intercept the advanced Set and feed motor-safety */ 369 super.set(mode, demand0, demand1Type, demand1); 370 feed(); 371 } 372 373 /** 374 * Sets the voltage output of the SpeedController. Compensates for the current bus 375 * voltage to ensure that the desired voltage is output even if the battery voltage is below 376 * 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a 377 * feedforward calculation). 378 * 379 * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work 380 * properly - unlike the ordinary set function, it is not "set it and forget it." 381 * 382 * @param outputVolts The voltage to output. 383 */ 384 @Override 385 public void setVoltage(double outputVolts) { 386 if(super.isVoltageCompensationEnabled()) 387 { 388 com.ctre.phoenix.Logger.log(ErrorCode.DoubleVoltageCompensatingWPI, _description + ": setVoltage "); 389 } 390 set(outputVolts / RobotController.getBatteryVoltage()); 391 } 392 393 // ----------------------- Invert routines -------------------// 394 /** 395 * Common interface for inverting direction of a speed controller. 396 * 397 * @param isInverted The state of inversion, true is inverted. 398 */ 399 @Override 400 public void setInverted(boolean isInverted) { 401 super.setInverted(isInverted); 402 } 403 404 /** 405 * Common interface for returning the inversion state of a speed controller. 406 * 407 * @return The state of inversion, true is inverted. 408 */ 409 @Override 410 public boolean getInverted() { 411 return super.getInverted(); 412 } 413 414 // ----------------------- turn-motor-off routines-------------------// 415 /** 416 * Common interface for disabling a motor. 417 */ 418 @Override 419 public void disable() { 420 neutralOutput(); 421 } 422 423 /** 424 * Common interface to stop the motor until Set is called again. 425 */ 426 @Override 427 public void stopMotor() { 428 neutralOutput(); 429 } 430 431 // ---- Sendable -------// 432 433 /** 434 * Initialize sendable 435 * @param builder Base sendable to build on 436 */ 437 @Override 438 public void initSendable(SendableBuilder builder) { 439 builder.setSmartDashboardType("Motor Controller"); 440 builder.setActuator(true); 441 builder.setSafeState(this::stopMotor); 442 builder.addDoubleProperty("Value", this::get, this::set); 443 } 444 445 /** 446 * @return description of controller 447 */ 448 public String getDescription() { 449 return _description; 450 } 451 452 /* ----- Motor Safety ----- */ 453 /** caller must lock appropriately */ 454 private WPI_MotorSafetyImplem GetMotorSafety() { 455 if (_motorSafety == null) { 456 /* newly created MS object */ 457 _motorSafety = new WPI_MotorSafetyImplem(this, getDescription()); 458 /* apply the expiration timeout */ 459 _motorSafety.setExpiration(_motSafeExpiration); 460 } 461 return _motorSafety; 462 } 463 /** 464 * Feed the motor safety object. 465 * 466 * <p>Resets the timer on this object that is used to do the timeouts. 467 */ 468 public void feed() { 469 synchronized (_lockMotorSaf) { 470 if (_motorSafety == null) { 471 /* do nothing, MS features were never enabled */ 472 } else { 473 GetMotorSafety().feed(); 474 } 475 } 476 } 477 478 /** 479 * Set the expiration time for the corresponding motor safety object. 480 * 481 * @param expirationTime The timeout value in seconds. 482 */ 483 public void setExpiration(double expirationTime) { 484 synchronized (_lockMotorSaf) { 485 /* save the value for if/when we do create the MS object */ 486 _motSafeExpiration = expirationTime; 487 /* apply it only if MS object exists */ 488 if (_motorSafety == null) { 489 /* do nothing, MS features were never enabled */ 490 } else { 491 /* this call will trigger creating a registered MS object */ 492 GetMotorSafety().setExpiration(_motSafeExpiration); 493 } 494 } 495 } 496 497 /** 498 * Retrieve the timeout value for the corresponding motor safety object. 499 * 500 * @return the timeout value in seconds. 501 */ 502 public double getExpiration() { 503 synchronized (_lockMotorSaf) { 504 /* return the intended expiration time */ 505 return _motSafeExpiration; 506 } 507 } 508 509 /** 510 * Determine of the motor is still operating or has timed out. 511 * 512 * @return a true value if the motor is still operating normally and hasn't timed out. 513 */ 514 public boolean isAlive() { 515 synchronized (_lockMotorSaf) { 516 if (_motorSafety == null) { 517 /* MC is alive - MS features were never enabled to neutral the MC. */ 518 return true; 519 } else { 520 return GetMotorSafety().isAlive(); 521 } 522 } 523 } 524 525 /** 526 * Enable/disable motor safety for this device. 527 * 528 * <p>Turn on and off the motor safety option for this PWM object. 529 * 530 * @param enabled True if motor safety is enforced for this object 531 */ 532 public void setSafetyEnabled(boolean enabled) { 533 synchronized (_lockMotorSaf) { 534 if (_motorSafety == null && !enabled) { 535 /* Caller wants to disable MS, 536 but MS features were nevere enabled, 537 so it doesn't need to be disabled. */ 538 } else { 539 /* MS will be created if it does not exist */ 540 GetMotorSafety().setSafetyEnabled(enabled); 541 } 542 } 543 } 544 545 /** 546 * Return the state of the motor safety enabled flag. 547 * 548 * <p>Return if the motor safety is currently enabled for this device. 549 * 550 * @return True if motor safety is enforced for this device 551 */ 552 public boolean isSafetyEnabled() { 553 synchronized (_lockMotorSaf) { 554 if (_motorSafety == null) { 555 /* MS features were never enabled. */ 556 return false; 557 } else { 558 return GetMotorSafety().isSafetyEnabled(); 559 } 560 } 561 } 562 563 private void timeoutFunc() { 564 if (DriverStation.isDisabled() || DriverStation.isTest()) { 565 return; 566 } 567 568 DriverStation.reportError(getDescription() + "... Output not updated often enough.", false); 569 570 stopMotor(); 571 } 572}