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 com.ctre.phoenix6.BaseStatusSignal; 010import com.ctre.phoenix6.StatusCode; 011import com.ctre.phoenix6.StatusSignal; 012import com.ctre.phoenix6.Utils; 013import com.ctre.phoenix6.controls.ControlRequest; 014import com.ctre.phoenix6.controls.EmptyControl; 015import com.ctre.phoenix6.jni.CtreJniWrapper; 016import com.ctre.phoenix6.jni.ErrorReportingJNI; 017import com.ctre.phoenix6.jni.StatusSignalJNI; 018import com.ctre.phoenix6.spns.SpnValue; 019import com.ctre.phoenix6.unmanaged.Unmanaged; 020 021import java.util.Map; 022import java.util.concurrent.ConcurrentHashMap; 023import java.util.concurrent.locks.Lock; 024import java.util.concurrent.locks.ReentrantLock; 025import java.util.function.BooleanSupplier; 026import java.util.function.DoubleFunction; 027 028public abstract class ParentDevice extends CtreJniWrapper { 029 protected static final EmptyControl _emptyControl = new EmptyControl(); 030 031 protected final DeviceIdentifier deviceIdentifier; 032 033 private final Map<Integer, BaseStatusSignal> _signalValues = new ConcurrentHashMap<Integer, BaseStatusSignal>(); 034 private ControlRequest _controlReq = _emptyControl; 035 private final Lock _controlReqLck = new ReentrantLock(); 036 037 private final double _creationTime = Utils.getCurrentTimeSeconds(); 038 039 private StatusCode _versionStatus = StatusCode.CouldNotRetrieveV6Firmware; 040 private double _timeToRefreshVersion = Utils.getCurrentTimeSeconds(); 041 042 private final StatusSignal<Integer> _compliancy; 043 private final StatusSignal<Integer> _resetSignal; 044 045 private void reportIfTooOld() { 046 /* if version is correct, no more checking */ 047 if (_versionStatus.isOK()) { 048 return; 049 } 050 /* Check if we have our versions initialized */ 051 if (_compliancy == null) { 052 return; 053 } 054 055 /* get fresh data if enough time has passed */ 056 final double currentTime = Utils.getCurrentTimeSeconds(); 057 if (currentTime >= _timeToRefreshVersion) { 058 /* Try to refresh again after 250ms */ 059 _timeToRefreshVersion = currentTime + 0.25; 060 /* Refresh versions, don't print out if there's an error with refreshing */ 061 _compliancy.refresh(false); 062 063 /* process the fetched version */ 064 StatusCode code = StatusCode.OK; 065 066 /* First check if we have good versions or not */ 067 if (_compliancy.getStatus().isOK()) { 068 final int firmwareCompliancy = _compliancy.getValue(); 069 final int apiCompliancy = Unmanaged.getApiCompliancy(); 070 071 if (apiCompliancy > firmwareCompliancy) { 072 code = StatusCode.FirmwareTooOld; 073 } else if (apiCompliancy < firmwareCompliancy) { 074 code = StatusCode.ApiTooOld; 075 } 076 } else { 077 /* don't care why we couldn't get message, just report we didn't get it */ 078 code = StatusCode.CouldNotRetrieveV6Firmware; 079 } 080 081 /* how much time has passed */ 082 final double deltaTimeSec = currentTime - _creationTime; 083 084 if (code.isOK()) { 085 /* version is retrieved and healthy */ 086 } else if (code == StatusCode.FirmwareTooOld || code == StatusCode.ApiTooOld || deltaTimeSec >= 3.0) { 087 /* report error */ 088 ErrorReportingJNI.reportStatusCode(code.value, deviceIdentifier.toString()); 089 } else { 090 /* don't start reporting CouldNotRetrieveV6Firmware yet, device was likely recently constructed */ 091 } 092 /* save the status code */ 093 _versionStatus = code; 094 } 095 } 096 097 public interface MapGenerator<T> { 098 Map<Integer, String> run(); 099 } 100 101 public ParentDevice(int deviceID, String model, String canbus) { 102 this.deviceIdentifier = new DeviceIdentifier(deviceID, model, canbus); 103 104 _compliancy = lookupStatusSignal( 105 SpnValue.Compliancy_Version.value, 106 Integer.class, 107 val -> (int)val, 108 "Compliancy", 109 false, 110 true); 111 _resetSignal = lookupStatusSignal( 112 SpnValue.Startup_ResetFlags.value, 113 Integer.class, 114 val -> (int)val, 115 "ResetFlags", 116 false, 117 true); 118 } 119 120 /** 121 * @return The device ID of this device [0,62]. 122 */ 123 public int getDeviceID() { 124 return deviceIdentifier.deviceID; 125 } 126 127 /** 128 * @return Name of the network this device is on. 129 */ 130 public String getNetwork() { 131 return deviceIdentifier.network; 132 } 133 134 /** 135 * Gets a number unique for this device's hardware type and ID. 136 * This number is not unique across networks. 137 * <p> 138 * This can be used to easily reference hardware devices on the 139 * same network in collections such as maps. 140 * 141 * @return Hash of this device. 142 */ 143 public long getDeviceHash() { 144 return deviceIdentifier.deviceHash; 145 } 146 147 /** 148 * Get the latest applied control. 149 * Caller can cast this to the derived class if they know its type. Otherwise, 150 * use {@link ControlRequest#getControlInfo} to get info out of it. 151 * 152 * @return Latest applied control 153 */ 154 public ControlRequest getAppliedControl() { 155 return _controlReq; 156 } 157 158 /** 159 * @return true if device has reset since the previous call of this routine. 160 */ 161 public boolean hasResetOccurred() { 162 return _resetSignal.refresh(false).hasUpdated(); 163 } 164 165 /** 166 * @return a {@link BooleanSupplier} that checks for device resets. 167 */ 168 public BooleanSupplier getResetOccurredChecker() { 169 final var resetSignal = _resetSignal.clone(); 170 return () -> resetSignal.refresh(false).hasUpdated(); 171 } 172 173 /** 174 * Returns whether the device is still connected to the robot. 175 * This is equivalent to refreshing and checking the latency of the 176 * Version status signal. 177 * <p> 178 * This defaults to reporting a device as disconnected if it has 179 * not been seen for over 0.5 seconds. 180 * 181 * @return true if the device is connected 182 */ 183 public boolean isConnected() { 184 return isConnected(0.500); 185 } 186 187 /** 188 * Returns whether the device is still connected to the robot. 189 * This is equivalent to refreshing and checking the latency of the 190 * Version status signal. 191 * 192 * @param maxLatencySeconds The maximum latency of the Version status signal 193 * before the device is reported as disconnected 194 * @return true if the device is connected 195 */ 196 public boolean isConnected(double maxLatencySeconds) { 197 return _compliancy.refresh(false).getTimestamp().getLatency() <= maxLatencySeconds; 198 } 199 200 /** 201 * Optimizes the device's bus utilization by reducing the update frequencies of its status signals. 202 * <p> 203 * All status signals that have not been explicitly given an update frequency using 204 * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other 205 * status signals in the same status frame have been given an update frequency, the 206 * update frequency will be honored for the entire frame. 207 * <p> 208 * This function only needs to be called once in the robot program. Additionally, this method does 209 * not necessarily need to be called after setting the update frequencies of other signals. 210 * <p> 211 * To restore the default status update frequencies, call {@link #resetSignalFrequencies}. 212 * Alternatively, remove this method call, redeploy the robot application, and power-cycle 213 * the device on the bus. The user can also override individual status update frequencies 214 * using {@link BaseStatusSignal#setUpdateFrequency}. 215 * <p> 216 * This will wait up to 0.100 seconds (100ms) for each signal by default. 217 * 218 * @return Status code of the first failed update frequency set call, or OK if all succeeded 219 */ 220 public StatusCode optimizeBusUtilization() { 221 return optimizeBusUtilization(0, 0.100); 222 } 223 /** 224 * Optimizes the device's bus utilization by reducing the update frequencies of its status signals. 225 * <p> 226 * All status signals that have not been explicitly given an update frequency using 227 * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other 228 * status signals in the same status frame have been given an update frequency, the 229 * update frequency will be honored for the entire frame. 230 * <p> 231 * This function only needs to be called once on this device in the robot program. Additionally, this 232 * method does not necessarily need to be called after setting the update frequencies of other signals. 233 * <p> 234 * To restore the default status update frequencies, call {@link #resetSignalFrequencies}. 235 * Alternatively, remove this method call, redeploy the robot application, and power-cycle 236 * the device on the bus. The user can also override individual status update frequencies 237 * using {@link BaseStatusSignal#setUpdateFrequency}. 238 * 239 * @deprecated This overload is deprecated for removal in 2025. Use 240 * {@link #optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds)} 241 * with an optimizedFreqHz of 0 Hz instead. 242 * 243 * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action 244 * @return Status code of the first failed update frequency set call, or OK if all succeeded 245 */ 246 @Deprecated(forRemoval = true) 247 public StatusCode optimizeBusUtilization(double timeoutSeconds) { 248 return optimizeBusUtilization(0, timeoutSeconds); 249 } 250 /** 251 * Optimizes the device's bus utilization by reducing the update frequencies of its status signals. 252 * <p> 253 * All status signals that have not been explicitly given an update frequency using 254 * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other 255 * status signals in the same status frame have been given an update frequency, the 256 * update frequency will be honored for the entire frame. 257 * <p> 258 * This function only needs to be called once on this device in the robot program. Additionally, this 259 * method does not necessarily need to be called after setting the update frequencies of other signals. 260 * <p> 261 * To restore the default status update frequencies, call {@link #resetSignalFrequencies}. 262 * Alternatively, remove this method call, redeploy the robot application, and power-cycle 263 * the device on the bus. The user can also override individual status update frequencies 264 * using {@link BaseStatusSignal#setUpdateFrequency}. 265 * 266 * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of 267 * 0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal 268 * frequency is 4 Hz. 269 * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action 270 * @return Status code of the first failed update frequency set call, or OK if all succeeded 271 */ 272 public StatusCode optimizeBusUtilization(double optimizedFreqHz, double timeoutSeconds) { 273 return StatusCode.valueOf(StatusSignalJNI.JNI_OptimizeUpdateFrequencies(deviceIdentifier.getNetwork(), deviceIdentifier.getDeviceHash(), optimizedFreqHz, timeoutSeconds)); 274 } 275 276 /** 277 * Optimizes the bus utilization of the provided devices by reducing the update frequencies 278 * of their status signals. 279 * <p> 280 * All status signals that have not been explicitly given an update frequency using 281 * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other 282 * status signals in the same status frame have been given an update frequency, the 283 * update frequency will be honored for the entire frame. 284 * <p> 285 * This function only needs to be called once in the robot program for the provided devices. 286 * Additionally, this method does not necessarily need to be called after setting the update 287 * frequencies of other signals. 288 * <p> 289 * To restore the default status update frequencies, call {@link #resetSignalFrequenciesForAll}. 290 * Alternatively, remove this method call, redeploy the robot application, and power-cycle 291 * the devices on the bus. The user can also override individual status update frequencies 292 * using {@link BaseStatusSignal#setUpdateFrequency}. 293 * <p> 294 * This will wait up to 0.100 seconds (100ms) for each status frame. 295 * 296 * @param devices Devices for which to opimize bus utilization 297 * @return Status code of the first failed optimize call, or OK if all succeeded 298 */ 299 public static StatusCode optimizeBusUtilizationForAll(ParentDevice... devices) { 300 return optimizeBusUtilizationForAll(0, devices); 301 } 302 /** 303 * Optimizes the bus utilization of the provided devices by reducing the update frequencies 304 * of their status signals. 305 * <p> 306 * All status signals that have not been explicitly given an update frequency using 307 * {@link BaseStatusSignal#setUpdateFrequency} will be disabled. Note that if other 308 * status signals in the same status frame have been given an update frequency, the 309 * update frequency will be honored for the entire frame. 310 * <p> 311 * This function only needs to be called once in the robot program for the provided devices. 312 * Additionally, this method does not necessarily need to be called after setting the update 313 * frequencies of other signals. 314 * <p> 315 * To restore the default status update frequencies, call {@link #resetSignalFrequenciesForAll}. 316 * Alternatively, remove this method call, redeploy the robot application, and power-cycle 317 * the devices on the bus. The user can also override individual status update frequencies 318 * using {@link BaseStatusSignal#setUpdateFrequency}. 319 * <p> 320 * This will wait up to 0.100 seconds (100ms) for each status frame. 321 * 322 * @param optimizedFreqHz The update frequency to apply to the optimized status signals. A frequency of 323 * 0 Hz (default) will turn off the signals. Otherwise, the minimum supported signal 324 * frequency is 4 Hz. 325 * @param devices Devices for which to opimize bus utilization 326 * @return Status code of the first failed optimize call, or OK if all succeeded 327 */ 328 public static StatusCode optimizeBusUtilizationForAll(double optimizedFreqHz, ParentDevice... devices) { 329 StatusCode retval = StatusCode.OK; 330 for (var device : devices) { 331 StatusCode err = device.optimizeBusUtilization(optimizedFreqHz, 0.100); 332 if (retval.isOK()) { 333 retval = err; 334 } 335 } 336 return retval; 337 } 338 339 /** 340 * Resets the update frequencies of all the device's status signals to the defaults. 341 * <p> 342 * This restores the default update frequency of all status signals, including status signals 343 * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status 344 * signals optimized out using {@link #optimizeBusUtilization}. 345 * <p> 346 * This will wait up to 0.100 seconds (100ms) for each signal by default. 347 * 348 * @return Status code of the first failed update frequency set call, or OK if all succeeded 349 */ 350 public StatusCode resetSignalFrequencies() { 351 return resetSignalFrequencies(0.100); 352 } 353 /** 354 * Resets the update frequencies of all the device's status signals to the defaults. 355 * <p> 356 * This restores the default update frequency of all status signals, including status signals 357 * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status 358 * signals optimized out using {@link #optimizeBusUtilization}. 359 * 360 * @param timeoutSeconds Maximum amount of time to wait for each status frame when performing the action 361 * @return Status code of the first failed update frequency set call, or OK if all succeeded 362 */ 363 public StatusCode resetSignalFrequencies(double timeoutSeconds) { 364 return StatusCode.valueOf(StatusSignalJNI.JNI_ResetUpdateFrequencies(deviceIdentifier.getNetwork(), deviceIdentifier.getDeviceHash(), timeoutSeconds)); 365 } 366 367 /** 368 * Resets the update frequencies of all the devices' status signals to the defaults. 369 * <p> 370 * This restores the default update frequency of all status signals, including status signals 371 * explicitly given an update frequency using {@link BaseStatusSignal#setUpdateFrequency} and status 372 * signals optimized out using {@link #optimizeBusUtilizationForAll}. 373 * <p> 374 * This will wait up to 0.100 seconds (100ms) for each status frame. 375 * 376 * @param devices Devices for which to restore default update frequencies 377 * @return Status code of the first failed restore call, or OK if all succeeded 378 */ 379 public static StatusCode resetSignalFrequenciesForAll(ParentDevice... devices) { 380 StatusCode retval = StatusCode.OK; 381 for (var device : devices) { 382 StatusCode err = device.resetSignalFrequencies(); 383 if (retval.isOK()) { 384 retval = err; 385 } 386 } 387 return retval; 388 } 389 390 protected StatusCode setControlPrivate(ControlRequest request) { 391 StatusCode status; 392 393 _controlReqLck.lock(); 394 /* make sure we always unlock */ 395 try { 396 reportIfTooOld(); 397 if (!_versionStatus.isOK() && _compliancy.getStatus().isOK()) { 398 /* version mismatch, cancel controls and report the error */ 399 _controlReq = _emptyControl; 400 _emptyControl.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash); 401 402 status = _versionStatus; 403 } else { 404 /* send the request */ 405 _controlReq = request; 406 status = _controlReq.sendRequest(deviceIdentifier.network, deviceIdentifier.deviceHash); 407 } 408 } finally { 409 _controlReqLck.unlock(); 410 } 411 412 if (!status.isOK()) { 413 ErrorReportingJNI.reportStatusCode(status.value, deviceIdentifier.toString() + " SetControl " 414 + request.getName()); 415 } 416 return status; 417 } 418 419 private <T> StatusSignal<T> commonLookup( 420 int spn, Class<T> classOfSignal, DoubleFunction<T> toValue, 421 MapGenerator<T> generator, String signalName, 422 boolean reportOnConstruction, boolean refresh 423 ) { 424 /* lookup and return if found */ 425 BaseStatusSignal toFind = _signalValues.get(spn); 426 if (toFind != null) { 427 /* Found it, toFind is now the found StatusSignal */ 428 /* since we didn't construct, report errors */ 429 reportOnConstruction = true; 430 } else { 431 /* insert into map */ 432 if (generator == null) { 433 _signalValues.put( 434 spn, new StatusSignal<T>( 435 deviceIdentifier, spn, this::reportIfTooOld, 436 classOfSignal, toValue, signalName 437 ) 438 ); 439 } else { 440 _signalValues.put( 441 spn, new StatusSignal<T>( 442 deviceIdentifier, spn, this::reportIfTooOld, 443 classOfSignal, toValue, generator, signalName 444 ) 445 ); 446 } 447 448 /* look up and return */ 449 toFind = _signalValues.get(spn); 450 } 451 /* Try to return the found value cast to the child class */ 452 453 /* Turn off unchecked warning, we check the type afterwards */ 454 @SuppressWarnings("unchecked") 455 StatusSignal<T> toReturn = StatusSignal.class.cast(toFind); 456 457 if (toReturn == null) { 458 /* Cast failed, so return with error */ 459 return new StatusSignal<T>(classOfSignal, toValue, StatusCode.InvalidParamValue); 460 } else if (!toReturn.getTypeClass().equals(classOfSignal)) { 461 /* Type does not match, return with error */ 462 return new StatusSignal<T>(classOfSignal, toValue, StatusCode.InvalidParamValue); 463 } else { 464 if (refresh) { 465 /* Refresh the signal before we pass it down */ 466 toReturn.refresh(reportOnConstruction); 467 } 468 /* We're good, go ahead and return */ 469 return toReturn; 470 } 471 } 472 473 protected <T> StatusSignal<T> lookupStatusSignal( 474 int spn, Class<T> classOfSignal, 475 DoubleFunction<T> toValue, String signalName, 476 boolean reportOnConstruction, boolean refresh 477 ) { 478 return commonLookup(spn, classOfSignal, toValue, null, signalName, reportOnConstruction, refresh); 479 } 480 481 protected <T> StatusSignal<T> lookupStatusSignal( 482 int spn, Class<T> classOfSignal, DoubleFunction<T> toValue, 483 MapGenerator<T> generator, String signalName, 484 boolean reportOnConstruction, boolean refresh 485 ) { 486 return commonLookup(spn, classOfSignal, toValue, generator, signalName, reportOnConstruction, refresh); 487 } 488}