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.math.geometry.Rotation2d;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
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 Sendable, AutoCloseable {
027  private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
028  private 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  private 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  /**
046   * Calibrate the gyro by running for a number of samples and computing the center value. Then use
047   * the center value as the Accumulator center value for subsequent measurements.
048   *
049   * <p>It's important to make sure that the robot is not moving while the centering calculations
050   * are in progress, this is typically done when the robot is first turned on while it's sitting at
051   * rest before the competition starts.
052   */
053  public void calibrate() {
054    AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle);
055  }
056
057  /**
058   * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
059   *
060   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
061   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
062   * 360 to 0 on the second time around.
063   *
064   * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
065   * top. It needs to follow the NWU axis convention.
066   *
067   * <p>This heading is based on integration of the returned rate from the gyro.
068   *
069   * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
070   */
071  public Rotation2d getRotation2d() {
072    return Rotation2d.fromDegrees(-getAngle());
073  }
074
075  /**
076   * Gyro constructor using the channel number.
077   *
078   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
079   *     channels 0-1.
080   */
081  @SuppressWarnings("this-escape")
082  public AnalogGyro(int channel) {
083    this(new AnalogInput(channel));
084    m_channelAllocated = true;
085    SendableRegistry.addChild(this, m_analog);
086  }
087
088  /**
089   * Gyro constructor with a precreated analog channel object. Use this constructor when the analog
090   * channel needs to be shared.
091   *
092   * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
093   *     on-board channels 0-1.
094   */
095  @SuppressWarnings("this-escape")
096  public AnalogGyro(AnalogInput channel) {
097    requireNonNullParam(channel, "channel", "AnalogGyro");
098
099    m_analog = channel;
100    initGyro();
101    calibrate();
102  }
103
104  /**
105   * Gyro constructor using the channel number along with parameters for presetting the center and
106   * offset values. Bypasses calibration.
107   *
108   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
109   *     channels 0-1.
110   * @param center Preset uncalibrated value to use as the accumulator center value.
111   * @param offset Preset uncalibrated value to use as the gyro offset.
112   */
113  @SuppressWarnings("this-escape")
114  public AnalogGyro(int channel, int center, double offset) {
115    this(new AnalogInput(channel), center, offset);
116    m_channelAllocated = true;
117    SendableRegistry.addChild(this, m_analog);
118  }
119
120  /**
121   * Gyro constructor with a precreated analog channel object along with parameters for presetting
122   * the center and offset values. Bypasses calibration.
123   *
124   * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
125   *     channels 0-1.
126   * @param center Preset uncalibrated value to use as the accumulator center value.
127   * @param offset Preset uncalibrated value to use as the gyro offset.
128   */
129  @SuppressWarnings("this-escape")
130  public AnalogGyro(AnalogInput channel, int center, double offset) {
131    requireNonNullParam(channel, "channel", "AnalogGyro");
132
133    m_analog = channel;
134    initGyro();
135    AnalogGyroJNI.setAnalogGyroParameters(
136        m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
137        offset, center);
138    reset();
139  }
140
141  /**
142   * Reset the gyro.
143   *
144   * <p>Resets the gyro to a heading of zero. This can be used if there is significant drift in the
145   * gyro, and it needs to be recalibrated after it has been running.
146   */
147  public void reset() {
148    AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
149  }
150
151  /** Delete (free) the accumulator and the analog components used for the gyro. */
152  @Override
153  public void close() {
154    SendableRegistry.remove(this);
155    if (m_analog != null && m_channelAllocated) {
156      m_analog.close();
157    }
158    m_analog = null;
159    AnalogGyroJNI.freeAnalogGyro(m_gyroHandle);
160  }
161
162  /**
163   * Return the heading of the robot in degrees.
164   *
165   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
166   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
167   * 360 to 0 on the second time around.
168   *
169   * <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
170   * It needs to follow the NED axis convention.
171   *
172   * <p>This heading is based on integration of the returned rate from the gyro.
173   *
174   * @return the current heading of the robot in degrees.
175   */
176  public double getAngle() {
177    if (m_analog == null) {
178      return 0.0;
179    } else {
180      return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle);
181    }
182  }
183
184  /**
185   * Return the rate of rotation of the gyro.
186   *
187   * <p>The rate is based on the most recent reading of the gyro analog value
188   *
189   * <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
190   * It needs to follow the NED axis convention.
191   *
192   * @return the current rate in degrees per second
193   */
194  public double getRate() {
195    if (m_analog == null) {
196      return 0.0;
197    } else {
198      return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle);
199    }
200  }
201
202  /**
203   * Return the gyro offset value set during calibration to use as a future preset.
204   *
205   * @return the current offset value
206   */
207  public double getOffset() {
208    return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle);
209  }
210
211  /**
212   * Return the gyro center value set during calibration to use as a future preset.
213   *
214   * @return the current center value
215   */
216  public int getCenter() {
217    return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle);
218  }
219
220  /**
221   * Set the gyro sensitivity. This takes the number of volts/degree/second sensitivity of the gyro
222   * and uses it in subsequent calculations to allow the code to work with multiple gyros. This
223   * value is typically found in the gyro datasheet.
224   *
225   * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
226   */
227  public void setSensitivity(double voltsPerDegreePerSecond) {
228    AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, voltsPerDegreePerSecond);
229  }
230
231  /**
232   * Set the size of the neutral zone. Any voltage from the gyro less than this amount from the
233   * center is considered stationary. Setting a deadband will decrease the amount of drift when the
234   * gyro isn't rotating, but will make it less accurate.
235   *
236   * @param volts The size of the deadband in volts
237   */
238  public void setDeadband(double volts) {
239    AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, volts);
240  }
241
242  /**
243   * Gets the analog input for the gyro.
244   *
245   * @return AnalogInput
246   */
247  public AnalogInput getAnalogInput() {
248    return m_analog;
249  }
250
251  @Override
252  public void initSendable(SendableBuilder builder) {
253    builder.setSmartDashboardType("Gyro");
254    builder.addDoubleProperty("Value", this::getAngle, null);
255  }
256}