001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.simulation;
006
007import edu.wpi.first.hal.simulation.AnalogGyroDataJNI;
008import edu.wpi.first.hal.simulation.NotifyCallback;
009import edu.wpi.first.wpilibj.AnalogGyro;
010
011/** Class to control a simulated analog gyro. */
012public class AnalogGyroSim {
013  private final int m_index;
014
015  /**
016   * Constructs from an AnalogGyro object.
017   *
018   * @param gyro AnalogGyro to simulate
019   */
020  public AnalogGyroSim(AnalogGyro gyro) {
021    m_index = gyro.getAnalogInput().getChannel();
022  }
023
024  /**
025   * Constructs from an analog input channel number.
026   *
027   * @param channel Channel number
028   */
029  public AnalogGyroSim(int channel) {
030    m_index = channel;
031  }
032
033  /**
034   * Register a callback on the angle.
035   *
036   * @param callback the callback that will be called whenever the angle changes
037   * @param initialNotify if true, the callback will be run on the initial value
038   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
039   *     this object so GC doesn't cancel the callback.
040   */
041  public CallbackStore registerAngleCallback(NotifyCallback callback, boolean initialNotify) {
042    int uid = AnalogGyroDataJNI.registerAngleCallback(m_index, callback, initialNotify);
043    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelAngleCallback);
044  }
045
046  /**
047   * Get the current angle of the gyro.
048   *
049   * @return the angle measured by the gyro
050   */
051  public double getAngle() {
052    return AnalogGyroDataJNI.getAngle(m_index);
053  }
054
055  /**
056   * Change the angle measured by the gyro.
057   *
058   * @param angle the new value
059   */
060  public void setAngle(double angle) {
061    AnalogGyroDataJNI.setAngle(m_index, angle);
062  }
063
064  /**
065   * Register a callback on the rate.
066   *
067   * @param callback the callback that will be called whenever the rate changes
068   * @param initialNotify if true, the callback will be run on the initial value
069   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
070   *     this object so GC doesn't cancel the callback.
071   */
072  public CallbackStore registerRateCallback(NotifyCallback callback, boolean initialNotify) {
073    int uid = AnalogGyroDataJNI.registerRateCallback(m_index, callback, initialNotify);
074    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelRateCallback);
075  }
076
077  /**
078   * Get the rate of angle change on this gyro.
079   *
080   * @return the rate
081   */
082  public double getRate() {
083    return AnalogGyroDataJNI.getRate(m_index);
084  }
085
086  /**
087   * Change the rate of the gyro.
088   *
089   * @param rate the new rate
090   */
091  public void setRate(double rate) {
092    AnalogGyroDataJNI.setRate(m_index, rate);
093  }
094
095  /**
096   * Register a callback on whether the gyro is initialized.
097   *
098   * @param callback the callback that will be called whenever the gyro is initialized
099   * @param initialNotify if true, the callback will be run on the initial value
100   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
101   *     this object so GC doesn't cancel the callback.
102   */
103  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
104    int uid = AnalogGyroDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
105    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelInitializedCallback);
106  }
107
108  /**
109   * Check if the gyro is initialized.
110   *
111   * @return true if initialized
112   */
113  public boolean getInitialized() {
114    return AnalogGyroDataJNI.getInitialized(m_index);
115  }
116
117  /**
118   * Set whether this gyro is initialized.
119   *
120   * @param initialized the new value
121   */
122  public void setInitialized(boolean initialized) {
123    AnalogGyroDataJNI.setInitialized(m_index, initialized);
124  }
125
126  /** Reset all simulation data for this object. */
127  public void resetData() {
128    AnalogGyroDataJNI.resetData(m_index);
129  }
130}