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