001package com.ctre.phoenix.sensors; 002 003import java.util.ArrayList; 004 005import com.ctre.phoenix.WPI_CallbackHelper; 006import com.ctre.phoenix.platform.DeviceType; 007import com.ctre.phoenix.platform.PlatformJNI; 008 009import edu.wpi.first.hal.HALValue; 010import edu.wpi.first.hal.SimDevice; 011import edu.wpi.first.hal.SimDouble; 012import edu.wpi.first.hal.SimDevice.Direction; 013import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 014import edu.wpi.first.hal.simulation.SimValueCallback; 015import edu.wpi.first.hal.simulation.SimulatorJNI; 016import edu.wpi.first.hal.HAL; 017import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 018import edu.wpi.first.wpilibj.simulation.CallbackStore; 019import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 020import edu.wpi.first.util.sendable.Sendable; 021import edu.wpi.first.util.sendable.SendableBuilder; 022import edu.wpi.first.util.sendable.SendableRegistry; 023 024/** 025 * CTRE CANCoder. 026 * 027 * @deprecated This device's Phoenix 5 API is deprecated for removal in the 028 * 2025 season. Users should update to Phoenix 6 firmware and migrate to the 029 * Phoenix 6 API. A migration guide is available at 030 * https://v6.docs.ctr-electronics.com/en/stable/docs/migration/migration-guide/index.html. 031 * <p> 032 * If the Phoenix 5 API must be used for this device, the device must have 22.X 033 * firmware. This firmware is available in Tuner X after selecting Phoenix 5 in 034 * the firmware year dropdown. 035 */ 036@Deprecated(since = "2024", forRemoval = true) 037public class WPI_CANCoder extends CANCoder implements Sendable, AutoCloseable { 038 039 private SimDevice m_simCANCoder; 040 private SimDouble m_simVbat; 041 private SimDouble m_simPosition; 042 private SimDouble m_simAbsPosition; 043 private SimDouble m_simRawPosition; 044 private SimDouble m_simVelocity; 045 046 // callbacks to register 047 private SimValueCallback onValueChangedCallback = new OnValueChangedCallback(); 048 private Runnable onPeriodicCallback = new OnPeriodicCallback(); 049 050 // returned registered callbacks 051 private ArrayList<CallbackStore> simValueChangedCallbacks = new ArrayList<CallbackStore>(); 052 private SimPeriodicBeforeCallback simPeriodicCallback; 053 054 /** 055 * Constructor for CANCoder 056 * @param deviceNumber device ID of CANCoder 057 * @param canbus Name of the CANbus; can be a CANivore device name or serial number. 058 * Pass in nothing or "rio" to use the roboRIO. 059 */ 060 public WPI_CANCoder(int deviceNumber, String canbus) { 061 super(deviceNumber, canbus); 062 SendableRegistry.addLW(this, "CANCoder ", deviceNumber); 063 064 m_simCANCoder = SimDevice.create("CANEncoder:CANCoder", deviceNumber); 065 if(m_simCANCoder != null){ 066 simPeriodicCallback = HAL.registerSimPeriodicBeforeCallback(onPeriodicCallback); 067 068 m_simVbat = m_simCANCoder.createDouble("busVoltage", Direction.kInput, 12.0); 069 070 m_simPosition = m_simCANCoder.createDouble("position", Direction.kOutput, 0); 071 m_simAbsPosition = m_simCANCoder.createDouble("absolutePosition", Direction.kOutput, 0); 072 073 m_simRawPosition = m_simCANCoder.createDouble("rawPositionInput", Direction.kInput, 0); 074 m_simVelocity = m_simCANCoder.createDouble("velocity", Direction.kInput, 0); 075 076 SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANCoder"); 077 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVbat, onValueChangedCallback, true)); 078 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawPosition, onValueChangedCallback, true)); 079 simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, onValueChangedCallback, true)); 080 } 081 } 082 083 /** 084 * Constructor for CANCoder 085 * @param deviceNumber device ID of CANCoder 086 */ 087 public WPI_CANCoder(int deviceNumber) { 088 this(deviceNumber, ""); 089 } 090 091 // ----- Auto-Closable ----- // 092 @Override 093 public void close(){ 094 SendableRegistry.remove(this); 095 if(m_simCANCoder != null) { 096 m_simCANCoder.close(); 097 m_simCANCoder = null; 098 } 099 super.DestroyObject(); //Destroy the device 100 } 101 102 // ----- Callbacks for Sim ----- // 103 private class OnValueChangedCallback implements SimValueCallback { 104 @Override 105 public void callback(String name, int handle, int direction, HALValue value) { 106 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 107 String physType = deviceName + ":" + name; 108 PlatformJNI.JNI_SimSetPhysicsInput(DeviceType.CANCoder.value, getDeviceID(), 109 physType, WPI_CallbackHelper.getRawValue(value)); 110 } 111 } 112 113 private class OnPeriodicCallback implements Runnable { 114 @Override 115 public void run() { 116 double value = 0; 117 int err = 0; 118 119 int deviceID = getDeviceID(); 120 121 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "BusVoltage"); 122 err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID); 123 if (err == 0) { 124 m_simVbat.set(value); 125 } 126 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensPos"); 127 err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID); 128 if (err == 0) { 129 m_simPosition.set(value); 130 } 131 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensAbsPos"); 132 err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID); 133 if (err == 0) { 134 m_simAbsPosition.set(value); 135 } 136 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensRawPos"); 137 err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID); 138 if (err == 0) { 139 m_simRawPosition.set(value); 140 } 141 value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensVel"); 142 err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID); 143 if (err == 0) { 144 m_simVelocity.set(value); 145 } 146 } 147 } 148 149 // ----- Sendable ----- // 150 @Override 151 public void initSendable(SendableBuilder builder) { 152 builder.setSmartDashboardType("CANCoder"); 153 builder.addDoubleProperty("Position", this::getPosition, null); 154 } 155}