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
024public class WPI_CANCoder extends CANCoder implements Sendable, AutoCloseable {
025
026        private SimDevice m_simCANCoder;
027        private SimDouble m_simVbat;
028        private SimDouble m_simPosition;
029        private SimDouble m_simAbsPosition;
030        private SimDouble m_simRawPosition;
031        private SimDouble m_simVelocity;
032
033        // callbacks to register
034        private SimValueCallback onValueChangedCallback = new OnValueChangedCallback();
035        private Runnable onPeriodicCallback = new OnPeriodicCallback();
036
037        // returned registered callbacks
038        private ArrayList<CallbackStore> simValueChangedCallbacks = new ArrayList<CallbackStore>();
039        private SimPeriodicBeforeCallback simPeriodicCallback;
040
041        /**
042         * Constructor for CANCoder
043         * @param deviceNumber device ID of CANCoder
044         * @param canbus Name of the CANbus; can be a CANivore device name or serial number.
045         *               Pass in nothing or "rio" to use the roboRIO.
046         */
047        public WPI_CANCoder(int deviceNumber, String canbus) {
048                super(deviceNumber, canbus);
049                SendableRegistry.addLW(this, "CANCoder ", deviceNumber);
050
051                m_simCANCoder = SimDevice.create("CANEncoder:CANCoder", deviceNumber);
052                if(m_simCANCoder != null){
053                        simPeriodicCallback = HAL.registerSimPeriodicBeforeCallback(onPeriodicCallback);
054
055                        m_simVbat = m_simCANCoder.createDouble("busVoltage", Direction.kInput, 12.0);
056
057                        m_simPosition = m_simCANCoder.createDouble("position", Direction.kOutput, 0);
058                        m_simAbsPosition = m_simCANCoder.createDouble("absolutePosition", Direction.kOutput, 0);
059
060                        m_simRawPosition = m_simCANCoder.createDouble("rawPositionInput", Direction.kInput, 0);
061                        m_simVelocity = m_simCANCoder.createDouble("velocity", Direction.kInput, 0);
062
063                        SimDeviceSim sim = new SimDeviceSim("CANEncoder:CANCoder");
064                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVbat, onValueChangedCallback, true));
065                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simRawPosition, onValueChangedCallback, true));
066                        simValueChangedCallbacks.add(sim.registerValueChangedCallback(m_simVelocity, onValueChangedCallback, true));
067                }
068        }
069
070        /**
071         * Constructor for CANCoder
072         * @param deviceNumber device ID of CANCoder
073         */
074        public WPI_CANCoder(int deviceNumber) {
075                this(deviceNumber, "");
076        }
077
078        // ----- Auto-Closable ----- //
079        @Override
080        public void close(){
081                SendableRegistry.remove(this);
082                if(m_simCANCoder != null) {
083                        m_simCANCoder.close();
084                        m_simCANCoder = null;
085                }
086                super.DestroyObject(); //Destroy the device
087        }
088
089        // ----- Callbacks for Sim ----- //
090        private class OnValueChangedCallback implements SimValueCallback {
091                @Override
092                public void callback(String name, int handle, int direction, HALValue value) {
093                        String deviceName = SimDeviceDataJNI.getSimDeviceName(SimDeviceDataJNI.getSimValueDeviceHandle(handle));
094                        String physType = deviceName + ":" + name;
095                        PlatformJNI.JNI_SimSetPhysicsInput(DeviceType.CANCoder.value, getDeviceID(),
096                                                                                           physType, WPI_CallbackHelper.getRawValue(value));
097                }
098        }
099
100        private class OnPeriodicCallback implements Runnable {
101                @Override
102                public void run() {
103                        double value = 0;
104                        int err = 0;
105
106                        int deviceID = getDeviceID();
107
108                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "BusVoltage");
109                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID);
110                        if (err == 0) {
111                                m_simVbat.set(value);
112                        }
113                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensPos");
114                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID);
115                        if (err == 0) {
116                                m_simPosition.set(value);
117                        }
118                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensAbsPos");
119                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID);
120                        if (err == 0) {
121                                m_simAbsPosition.set(value);
122                        }
123                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensRawPos");
124                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID);
125                        if (err == 0) {
126                                m_simRawPosition.set(value);
127                        }
128                        value = PlatformJNI.JNI_SimGetPhysicsValue(DeviceType.CANCoder.value, deviceID, "IntegSensVel");
129                        err = PlatformJNI.JNI_SimGetLastError(DeviceType.CANCoder.value, deviceID);
130                        if (err == 0) {
131                                m_simVelocity.set(value);
132                        }
133                }
134        }
135
136        // ----- Sendable ----- //
137        @Override
138        public void initSendable(SendableBuilder builder) {
139                builder.setSmartDashboardType("CANCoder");
140                builder.addDoubleProperty("Position", this::getPosition, null);
141        }
142}