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 java.util.ArrayList; 010 011import com.ctre.phoenix6.CANBus; 012import com.ctre.phoenix6.StatusSignal; 013import com.ctre.phoenix6.Utils; 014import com.ctre.phoenix6.hardware.core.CoreCANdi; 015import com.ctre.phoenix6.jni.PlatformJNI; 016import com.ctre.phoenix6.sim.DeviceType; 017import com.ctre.phoenix6.wpiutils.AutoFeedEnable; 018import com.ctre.phoenix6.wpiutils.CallbackHelper; 019import com.ctre.phoenix6.wpiutils.ReplayAutoEnable; 020 021import edu.wpi.first.hal.HAL; 022import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 023import edu.wpi.first.hal.HALValue; 024import edu.wpi.first.hal.SimDevice; 025import edu.wpi.first.hal.SimDevice.Direction; 026import edu.wpi.first.hal.SimBoolean; 027import edu.wpi.first.hal.SimDouble; 028import edu.wpi.first.hal.SimEnum; 029import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 030import edu.wpi.first.util.sendable.Sendable; 031import edu.wpi.first.util.sendable.SendableBuilder; 032import edu.wpi.first.util.sendable.SendableRegistry; 033import edu.wpi.first.wpilibj.RobotBase; 034import edu.wpi.first.wpilibj.simulation.CallbackStore; 035import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 036 037/** 038 * WPILib-integrated version of {@link CoreCANdi} 039 */ 040public class CANdi extends CoreCANdi implements Sendable, AutoCloseable { 041 /** 042 * The StatusSignal getters are copies so that calls 043 * to the WPI interface do not update any references 044 */ 045 private static final DeviceType kSimDeviceType = DeviceType.P6_CANdiType; 046 047 private SimDevice m_simCANdi; 048 private SimDouble m_simSupplyVoltage; 049 private SimDouble m_simRequestedOutputCurrent; 050 private SimDouble m_simOutputCurrent; 051 052 private SimDevice m_simPwm1; 053 private SimDouble m_simPwm1Position; 054 private SimBoolean m_simPwm1Connected; 055 private SimDouble m_simPwm1Velocity; 056 private SimDouble m_simPwm1RiseRise; 057 private SimDouble m_simPwm1RiseFall; 058 059 private SimDevice m_simPwm2; 060 private SimDouble m_simPwm2Position; 061 private SimBoolean m_simPwm2Connected; 062 private SimDouble m_simPwm2Velocity; 063 private SimDouble m_simPwm2RiseRise; 064 private SimDouble m_simPwm2RiseFall; 065 066 private SimDevice m_simQuadrature; 067 private SimDouble m_simQuadPos; 068 private SimDouble m_simQuadRawPos; 069 private SimDouble m_simQuadVel; 070 071 private SimDevice m_simS1DIO; 072 private SimBoolean m_simS1Closed; 073 private SimEnum m_simS1State; 074 075 private SimDevice m_simS2DIO; 076 private SimBoolean m_simS2Closed; 077 private SimEnum m_simS2State; 078 079 // returned registered callbacks 080 private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>(); 081 private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null; 082 083 /** 084 * Constructs a new CANdi object. 085 * <p> 086 * Constructs the device using the default CAN bus for the system: 087 * <ul> 088 * <li>"rio" on roboRIO 089 * <li>"can0" on Linux 090 * <li>"*" on Windows 091 * </ul> 092 * 093 * @param deviceId ID of the device, as configured in Phoenix Tuner. 094 */ 095 public CANdi(int deviceId) { 096 this(deviceId, ""); 097 } 098 099 /** 100 * Constructs a new CANdi object. 101 * 102 * @param deviceId ID of the device, as configured in Phoenix Tuner. 103 * @param canbus The CAN bus this device is on. 104 */ 105 public CANdi(int deviceId, CANBus canbus) { 106 this(deviceId, canbus.getName()); 107 } 108 109 /** 110 * Constructs a new CANdi object. 111 * 112 * @param deviceId ID of the device, as configured in Phoenix Tuner. 113 * @param canbus Name of the CAN bus this device is on. Possible CAN bus 114 * strings are: 115 * <ul> 116 * <li>"rio" for the native roboRIO CAN bus 117 * <li>CANivore name or serial number 118 * <li>SocketCAN interface (non-FRC Linux only) 119 * <li>"*" for any CANivore seen by the program 120 * <li>empty string (default) to select the default for the 121 * system: 122 * <ul> 123 * <li>"rio" on roboRIO 124 * <li>"can0" on Linux 125 * <li>"*" on Windows 126 * </ul> 127 * </ul> 128 */ 129 public CANdi(int deviceId, String canbus) { 130 super(deviceId, canbus); 131 SendableRegistry.addLW(this, "CANdi (v6) ", deviceId); 132 133 if (RobotBase.isSimulation()) { 134 /* run in both swsim and hwsim */ 135 AutoFeedEnable.getInstance().start(); 136 } 137 if (Utils.isReplay()) { 138 ReplayAutoEnable.getInstance().start(); 139 } 140 141 m_simCANdi = SimDevice.create("CAN:CANdi (v6)", deviceId); 142 143 final String base = "CANdi (v6)[" + deviceId + "]/"; 144 m_simPwm1 = SimDevice.create("CANDutyCycle:" + base + "PWM1"); 145 m_simPwm2 = SimDevice.create("CANDutyCycle:" + base + "PWM2"); 146 m_simQuadrature = SimDevice.create("CANEncoder:" + base + "Quadrature"); 147 m_simS1DIO = SimDevice.create("CANDIO:" + base + "S1"); 148 m_simS2DIO = SimDevice.create("CANDIO:" + base + "S2"); 149 150 final String[] StateEnums = new String[]{"Floating", "Low", "High"}; 151 152 if (m_simCANdi != null) { 153 m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(this::onPeriodic); 154 155 m_simSupplyVoltage = m_simCANdi.createDouble("supplyVoltage", Direction.kInput, 12.0); 156 m_simRequestedOutputCurrent = m_simCANdi.createDouble("requestedOutputCurrent", Direction.kInput, 0); 157 m_simOutputCurrent = m_simCANdi.createDouble("outputCurrent", Direction.kOutput, 0); 158 159 final SimDeviceSim sim = new SimDeviceSim("CAN:CANdi (v6)"); 160 m_simValueChangedCallbacks 161 .add(sim.registerValueChangedCallback(m_simSupplyVoltage, this::onValueChanged, true)); 162 m_simValueChangedCallbacks 163 .add(sim.registerValueChangedCallback(m_simRequestedOutputCurrent, this::onValueChanged, true)); 164 } 165 if (m_simPwm1 != null) { 166 m_simPwm1Position = m_simPwm1.createDouble("position", Direction.kInput, 0); 167 m_simPwm1Connected = m_simPwm1.createBoolean("connected", Direction.kInput, true); 168 m_simPwm1Velocity = m_simPwm1.createDouble("velocity", Direction.kInput, 0); 169 m_simPwm1RiseRise = m_simPwm1.createDouble("riseToRise", Direction.kInput, 0.004); 170 m_simPwm1RiseFall = m_simPwm1.createDouble("riseToFall", Direction.kInput, 0); 171 172 final SimDeviceSim sim = new SimDeviceSim("CANDutyCycle:" + base + "PWM1"); 173 m_simValueChangedCallbacks 174 .add(sim.registerValueChangedCallback(m_simPwm1Position, this::onValueChanged, true)); 175 m_simValueChangedCallbacks 176 .add(sim.registerValueChangedCallback(m_simPwm1Connected, this::onValueChanged, true)); 177 m_simValueChangedCallbacks 178 .add(sim.registerValueChangedCallback(m_simPwm1Velocity, this::onValueChanged, true)); 179 m_simValueChangedCallbacks 180 .add(sim.registerValueChangedCallback(m_simPwm1RiseRise, this::onValueChanged, true)); 181 m_simValueChangedCallbacks 182 .add(sim.registerValueChangedCallback(m_simPwm1RiseFall, this::onValueChanged, true)); 183 } 184 if (m_simPwm2 != null) { 185 m_simPwm2Position = m_simPwm2.createDouble("position", Direction.kInput, 0); 186 m_simPwm2Connected = m_simPwm2.createBoolean("connected", Direction.kInput, true); 187 m_simPwm2Velocity = m_simPwm2.createDouble("velocity", Direction.kInput, 0); 188 m_simPwm2RiseRise = m_simPwm2.createDouble("riseToRise", Direction.kInput, 0.004); 189 m_simPwm2RiseFall = m_simPwm2.createDouble("riseToFall", Direction.kInput, 0); 190 191 final SimDeviceSim sim = new SimDeviceSim("CANDutyCycle:" + base + "PWM2"); 192 m_simValueChangedCallbacks 193 .add(sim.registerValueChangedCallback(m_simPwm2Position, this::onValueChanged, true)); 194 m_simValueChangedCallbacks 195 .add(sim.registerValueChangedCallback(m_simPwm2Connected, this::onValueChanged, true)); 196 m_simValueChangedCallbacks 197 .add(sim.registerValueChangedCallback(m_simPwm2Velocity, this::onValueChanged, true)); 198 m_simValueChangedCallbacks 199 .add(sim.registerValueChangedCallback(m_simPwm2RiseRise, this::onValueChanged, true)); 200 m_simValueChangedCallbacks 201 .add(sim.registerValueChangedCallback(m_simPwm2RiseFall, this::onValueChanged, true)); 202 } 203 if (m_simQuadrature != null) { 204 m_simQuadrature.createBoolean("init", Direction.kOutput, true); 205 206 m_simQuadPos = m_simQuadrature.createDouble("position", Direction.kOutput, 0); 207 208 m_simQuadRawPos = m_simQuadrature.createDouble("rawPositionInput", Direction.kInput, 0); 209 m_simQuadVel = m_simQuadrature.createDouble("velocity", Direction.kInput, 0); 210 211 final SimDeviceSim sim = new SimDeviceSim("CANEncoder:" + base + "Quadrature"); 212 m_simValueChangedCallbacks 213 .add(sim.registerValueChangedCallback(m_simQuadRawPos, this::onValueChanged, true)); 214 m_simValueChangedCallbacks 215 .add(sim.registerValueChangedCallback(m_simQuadVel, this::onValueChanged, true)); 216 } 217 if (m_simS1DIO != null) { 218 m_simS1DIO.createBoolean("init", Direction.kOutput, true); 219 m_simS1DIO.createBoolean("input", Direction.kOutput, true); 220 m_simS1Closed = m_simS1DIO.createBoolean("value", Direction.kOutput, false); 221 m_simS1State = m_simS1DIO.createEnum("state", Direction.kInput, StateEnums, 0); 222 223 final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "S1"); 224 m_simValueChangedCallbacks 225 .add(sim.registerValueChangedCallback(m_simS1State, this::onValueChanged, true)); 226 } 227 if (m_simS2DIO != null) { 228 m_simS2DIO.createBoolean("init", Direction.kOutput, true); 229 m_simS2DIO.createBoolean("input", Direction.kOutput, true); 230 m_simS2Closed = m_simS2DIO.createBoolean("value", Direction.kOutput, false); 231 m_simS2State = m_simS2DIO.createEnum("state", Direction.kInput, StateEnums, 0); 232 233 final SimDeviceSim sim = new SimDeviceSim("CANDIO:" + base + "S2"); 234 m_simValueChangedCallbacks 235 .add(sim.registerValueChangedCallback(m_simS2State, this::onValueChanged, true)); 236 } 237 } 238 239 // ----- Auto-Closable ----- // 240 @Override 241 public void close() { 242 SendableRegistry.remove(this); 243 if (m_simPeriodicBeforeCallback != null) { 244 m_simPeriodicBeforeCallback.close(); 245 m_simPeriodicBeforeCallback = null; 246 } 247 if (m_simCANdi != null) { 248 m_simCANdi.close(); 249 m_simCANdi = null; 250 } 251 if (m_simPwm1 != null) { 252 m_simPwm1.close(); 253 m_simPwm1 = null; 254 } 255 if (m_simPwm2 != null) { 256 m_simPwm2.close(); 257 m_simPwm2 = null; 258 } 259 if (m_simQuadrature != null) { 260 m_simQuadrature.close(); 261 m_simQuadrature = null; 262 } 263 if (m_simS1DIO != null) { 264 m_simS1DIO.close(); 265 m_simS1DIO = null; 266 } 267 if (m_simS2DIO != null) { 268 m_simS2DIO.close(); 269 m_simS2DIO = null; 270 } 271 272 for (var callback : m_simValueChangedCallbacks) { 273 callback.close(); 274 } 275 m_simValueChangedCallbacks.clear(); 276 277 AutoFeedEnable.getInstance().stop(); 278 } 279 280 // ----- Callbacks for Sim ----- // 281 private void onValueChanged(String name, int handle, int direction, HALValue value) { 282 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 283 String physType = deviceName + ":" + name; 284 PlatformJNI.JNI_SimSetPhysicsInput( 285 kSimDeviceType.value, getDeviceID(), 286 physType, CallbackHelper.getRawValue(value) 287 ); 288 } 289 290 private void onPeriodic() { 291 double value = 0; 292 int err = 0; 293 294 final int deviceID = getDeviceID(); 295 296 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage"); 297 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 298 if (err == 0) { 299 m_simSupplyVoltage.set(value); 300 } 301 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RequestedOutputCurrent"); 302 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 303 if (err == 0) { 304 m_simRequestedOutputCurrent.set(value); 305 } 306 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "OutputCurrent"); 307 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 308 if (err == 0) { 309 m_simOutputCurrent.set(value); 310 } 311 312 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Position"); 313 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 314 if (err == 0) { 315 m_simPwm1Position.set(value); 316 } 317 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Connected"); 318 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 319 if (err == 0) { 320 m_simPwm1Connected.set((int)value != 0); 321 } 322 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1Velocity"); 323 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 324 if (err == 0) { 325 m_simPwm1Velocity.set(value); 326 } 327 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1RiseToRise"); 328 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 329 if (err == 0) { 330 m_simPwm1RiseRise.set(value); 331 } 332 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM1RiseToFall"); 333 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 334 if (err == 0) { 335 m_simPwm1RiseFall.set(value); 336 } 337 338 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Position"); 339 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 340 if (err == 0) { 341 m_simPwm2Position.set(value); 342 } 343 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Connected"); 344 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 345 if (err == 0) { 346 m_simPwm2Connected.set((int)value != 0); 347 } 348 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2Velocity"); 349 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 350 if (err == 0) { 351 m_simPwm2Velocity.set(value); 352 } 353 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2RiseToRise"); 354 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 355 if (err == 0) { 356 m_simPwm2RiseRise.set(value); 357 } 358 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "PWM2RiseToFall"); 359 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 360 if (err == 0) { 361 m_simPwm2RiseFall.set(value); 362 } 363 364 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "QuadraturePosition"); 365 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 366 if (err == 0) { 367 m_simQuadPos.set(value); 368 } 369 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawQuadraturePosition"); 370 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 371 if (err == 0) { 372 m_simQuadRawPos.set(value); 373 } 374 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "QuadratureVelocity"); 375 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 376 if (err == 0) { 377 m_simQuadVel.set(value); 378 } 379 380 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S1State"); 381 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 382 if (err == 0) { 383 m_simS1State.set((int)value); 384 } 385 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S1Closed"); 386 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 387 if (err == 0) { 388 m_simS1Closed.set((int)value != 0); 389 } 390 391 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S2State"); 392 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 393 if (err == 0) { 394 m_simS2State.set((int)value); 395 } 396 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "S2Closed"); 397 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 398 if (err == 0) { 399 m_simS2Closed.set((int)value != 0); 400 } 401 } 402 403 // ----- Sendable ----- // 404 @Override 405 public void initSendable(SendableBuilder builder) { 406 builder.setSmartDashboardType("CANdi"); 407 } 408}