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