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;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.hal.AnalogGyroJNI;
010import edu.wpi.first.hal.FRCNetComm.tResourceType;
011import edu.wpi.first.hal.HAL;
012import edu.wpi.first.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
015import edu.wpi.first.wpilibj.interfaces.Gyro;
016
017/**
018 * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
019 * tracks the robots heading based on the starting position. As the robot rotates the new heading is
020 * computed by integrating the rate of rotation returned by the sensor. When the class is
021 * instantiated, it does a short calibration routine where it samples the gyro while at rest to
022 * determine the default offset. This is subtracted from each sample to determine the heading.
023 *
024 * <p>This class is for gyro sensors that connect to an analog input.
025 */
026public class AnalogGyro implements Gyro, Sendable {
027  private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
028  protected AnalogInput m_analog;
029  private boolean m_channelAllocated;
030
031  private int m_gyroHandle;
032
033  /** Initialize the gyro. Calibration is handled by calibrate(). */
034  public void initGyro() {
035    if (m_gyroHandle == 0) {
036      m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
037    }
038
039    AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);
040
041    HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel() + 1);
042    SendableRegistry.addLW(this, "AnalogGyro", m_analog.getChannel());
043  }
044
045  @Override
046  public void calibrate() {
047    AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle);
048  }
049
050  /**
051   * Gyro constructor using the channel number.
052   *
053   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
054   *     channels 0-1.
055   */
056  public AnalogGyro(int channel) {
057    this(new AnalogInput(channel));
058    m_channelAllocated = true;
059    SendableRegistry.addChild(this, m_analog);
060  }
061
062  /**
063   * Gyro constructor with a precreated analog channel object. Use this constructor when the analog
064   * channel needs to be shared.
065   *
066   * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
067   *     on-board channels 0-1.
068   */
069  public AnalogGyro(AnalogInput channel) {
070    requireNonNullParam(channel, "channel", "AnalogGyro");
071
072    m_analog = channel;
073    initGyro();
074    calibrate();
075  }
076
077  /**
078   * Gyro constructor using the channel number along with parameters for presetting the center and
079   * offset values. Bypasses calibration.
080   *
081   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
082   *     channels 0-1.
083   * @param center Preset uncalibrated value to use as the accumulator center value.
084   * @param offset Preset uncalibrated value to use as the gyro offset.
085   */
086  public AnalogGyro(int channel, int center, double offset) {
087    this(new AnalogInput(channel), center, offset);
088    m_channelAllocated = true;
089    SendableRegistry.addChild(this, m_analog);
090  }
091
092  /**
093   * Gyro constructor with a precreated analog channel object along with parameters for presetting
094   * the center and offset values. Bypasses calibration.
095   *
096   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
097   *     channels 0-1.
098   * @param center Preset uncalibrated value to use as the accumulator center value.
099   * @param offset Preset uncalibrated value to use as the gyro offset.
100   */
101  public AnalogGyro(AnalogInput channel, int center, double offset) {
102    requireNonNullParam(channel, "channel", "AnalogGyro");
103
104    m_analog = channel;
105    initGyro();
106    AnalogGyroJNI.setAnalogGyroParameters(
107        m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
108        offset, center);
109    reset();
110  }
111
112  @Override
113  public void reset() {
114    AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
115  }
116
117  /** Delete (free) the accumulator and the analog components used for the gyro. */
118  @Override
119  public void close() {
120    SendableRegistry.remove(this);
121    if (m_analog != null && m_channelAllocated) {
122      m_analog.close();
123    }
124    m_analog = null;
125    AnalogGyroJNI.freeAnalogGyro(m_gyroHandle);
126  }
127
128  @Override
129  public double getAngle() {
130    if (m_analog == null) {
131      return 0.0;
132    } else {
133      return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle);
134    }
135  }
136
137  @Override
138  public double getRate() {
139    if (m_analog == null) {
140      return 0.0;
141    } else {
142      return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle);
143    }
144  }
145
146  /**
147   * Return the gyro offset value set during calibration to use as a future preset.
148   *
149   * @return the current offset value
150   */
151  public double getOffset() {
152    return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle);
153  }
154
155  /**
156   * Return the gyro center value set during calibration to use as a future preset.
157   *
158   * @return the current center value
159   */
160  public int getCenter() {
161    return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle);
162  }
163
164  /**
165   * Set the gyro sensitivity. This takes the number of volts/degree/second sensitivity of the gyro
166   * and uses it in subsequent calculations to allow the code to work with multiple gyros. This
167   * value is typically found in the gyro datasheet.
168   *
169   * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
170   */
171  public void setSensitivity(double voltsPerDegreePerSecond) {
172    AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, voltsPerDegreePerSecond);
173  }
174
175  /**
176   * Set the size of the neutral zone. Any voltage from the gyro less than this amount from the
177   * center is considered stationary. Setting a deadband will decrease the amount of drift when the
178   * gyro isn't rotating, but will make it less accurate.
179   *
180   * @param volts The size of the deadband in volts
181   */
182  public void setDeadband(double volts) {
183    AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, volts);
184  }
185
186  /**
187   * Gets the analog input for the gyro.
188   *
189   * @return AnalogInput
190   */
191  public AnalogInput getAnalogInput() {
192    return m_analog;
193  }
194
195  @Override
196  public void initSendable(SendableBuilder builder) {
197    builder.setSmartDashboardType("Gyro");
198    builder.addDoubleProperty("Value", this::getAngle, null);
199  }
200}