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.phoenixpro.hardware; 008 009import java.util.ArrayList; 010 011import com.ctre.phoenixpro.StatusSignalValue; 012import com.ctre.phoenixpro.hardware.core.CoreCANcoder; 013import com.ctre.phoenixpro.jni.PlatformJNI; 014import com.ctre.phoenixpro.sim.DeviceType; 015import com.ctre.phoenixpro.wpiutils.AutoFeedEnable; 016import com.ctre.phoenixpro.wpiutils.CallbackHelper; 017 018import edu.wpi.first.hal.HAL; 019import edu.wpi.first.hal.HALValue; 020import edu.wpi.first.hal.SimDevice; 021import edu.wpi.first.hal.SimDouble; 022import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback; 023import edu.wpi.first.hal.SimDevice.Direction; 024import edu.wpi.first.hal.simulation.SimDeviceDataJNI; 025import edu.wpi.first.hal.simulation.SimValueCallback; 026import edu.wpi.first.util.sendable.Sendable; 027import edu.wpi.first.util.sendable.SendableBuilder; 028import edu.wpi.first.util.sendable.SendableRegistry; 029import edu.wpi.first.wpilibj.simulation.CallbackStore; 030import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 031 032public class CANcoder extends CoreCANcoder implements Sendable, AutoCloseable { 033 /** 034 * The StatusSignalValue getters are copies so that calls 035 * to the WPI interface do not update any references 036 */ 037 private final StatusSignalValue<Double> m_positionGetter = getPosition().clone(); 038 039 private static final DeviceType kSimDeviceType = DeviceType.PRO_CANcoderType; 040 041 private SimDevice m_simCANCoder; 042 private SimDouble m_simSupplyVoltage; 043 private SimDouble m_simPosition; 044 private SimDouble m_simRawPosition; 045 private SimDouble m_simVelocity; 046 047 // callbacks to register 048 private final SimValueCallback m_onValueChangedCallback = new OnValueChangedCallback(); 049 private final Runnable m_onPeriodicCallback = new OnPeriodicCallback(); 050 051 // returned registered callbacks 052 private final ArrayList<CallbackStore> m_simValueChangedCallbacks = new ArrayList<CallbackStore>(); 053 private SimPeriodicBeforeCallback m_simPeriodicBeforeCallback = null; 054 055 /** 056 * Constructs a new CANcoder object. 057 * <p> 058 * Constructs the device using the default CAN bus for the system: 059 * <ul> 060 * <li>"rio" on roboRIO 061 * <li>"can0" on Linux 062 * <li>"*" on Windows 063 * </ul> 064 * 065 * @param deviceId ID of the device, as configured in Phoenix Tuner. 066 */ 067 public CANcoder(int deviceId) { 068 this(deviceId, ""); 069 } 070 071 /** 072 * Constructs a new CANcoder object. 073 * 074 * @param deviceId ID of the device, as configured in Phoenix Tuner. 075 * @param canbus Name of the CAN bus this device is on. Possible CAN bus 076 * strings are: 077 * <ul> 078 * <li>"rio" for the native roboRIO CAN bus 079 * <li>CANivore name or serial number 080 * <li>SocketCAN interface (non-FRC Linux only) 081 * <li>"*" for any CANivore seen by the program 082 * <li>empty string (default) to select the default for the 083 * system: 084 * <ul> 085 * <li>"rio" on roboRIO 086 * <li>"can0" on Linux 087 * <li>"*" on Windows 088 * </ul> 089 * </ul> 090 */ 091 public CANcoder(int deviceId, String canbus) { 092 super(deviceId, canbus); 093 SendableRegistry.addLW(this, "CANCoder (Pro) ", deviceId); 094 095 m_simCANCoder = SimDevice.create("CANEncoder:CANCoder (Pro)", deviceId); 096 if (m_simCANCoder != null) { 097 AutoFeedEnable.getInstance(); 098 m_simPeriodicBeforeCallback = HAL.registerSimPeriodicBeforeCallback(m_onPeriodicCallback); 099 100 m_simSupplyVoltage = m_simCANCoder.createDouble("supplyVoltage", Direction.kInput, 12.0); 101 102 m_simPosition = m_simCANCoder.createDouble("position", Direction.kOutput, 0); 103 104 m_simRawPosition = m_simCANCoder.createDouble("rawPositionInput", Direction.kInput, 0); 105 m_simVelocity = m_simCANCoder.createDouble("velocity", Direction.kInput, 0); 106 107 final SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANCoder (Pro)"); 108 m_simValueChangedCallbacks 109 .add(sim.registerValueChangedCallback(m_simSupplyVoltage, m_onValueChangedCallback, true)); 110 m_simValueChangedCallbacks 111 .add(sim.registerValueChangedCallback(m_simRawPosition, m_onValueChangedCallback, true)); 112 m_simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, m_onValueChangedCallback, true)); 113 } 114 } 115 116 // ----- Auto-Closable ----- // 117 @Override 118 public void close() { 119 SendableRegistry.remove(this); 120 if (m_simPeriodicBeforeCallback != null) { 121 m_simPeriodicBeforeCallback.close(); 122 m_simPeriodicBeforeCallback = null; 123 } 124 if (m_simCANCoder != null) { 125 m_simCANCoder.close(); 126 m_simCANCoder = null; 127 } 128 129 for (var callback : m_simValueChangedCallbacks) { 130 callback.close(); 131 } 132 m_simValueChangedCallbacks.clear(); 133 } 134 135 // ----- Callbacks for Sim ----- // 136 private class OnValueChangedCallback implements SimValueCallback { 137 @Override 138 public void callback(String name, int handle, int direction, HALValue value) { 139 String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle)); 140 String physType = deviceName + ":" + name; 141 PlatformJNI.JNI_SimSetPhysicsInput(kSimDeviceType.value, getDeviceID(), 142 physType, CallbackHelper.getRawValue(value)); 143 } 144 } 145 146 private class OnPeriodicCallback implements Runnable { 147 @Override 148 public void run() { 149 double value = 0; 150 int err = 0; 151 152 final int deviceID = getDeviceID(); 153 154 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "SupplyVoltage"); 155 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 156 if (err == 0) { 157 m_simSupplyVoltage.set(value); 158 } 159 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Position"); 160 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 161 if (err == 0) { 162 m_simPosition.set(value); 163 } 164 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "RawPosition"); 165 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 166 if (err == 0) { 167 m_simRawPosition.set(value); 168 } 169 value = PlatformJNI.JNI_SimGetPhysicsValue(kSimDeviceType.value, deviceID, "Velocity"); 170 err = PlatformJNI.JNI_SimGetLastError(kSimDeviceType.value, deviceID); 171 if (err == 0) { 172 m_simVelocity.set(value); 173 } 174 } 175 } 176 177 // ----- Sendable ----- // 178 @Override 179 public void initSendable(SendableBuilder builder) { 180 builder.setSmartDashboardType("CANCoder"); 181 builder.addDoubleProperty("Position", m_positionGetter.asSupplier()::get, this::setPosition); 182 } 183}