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.interfaces;
006
007import edu.wpi.first.math.geometry.Rotation2d;
008
009/** Interface for yaw rate gyros. */
010public interface Gyro extends AutoCloseable {
011  /**
012   * Calibrate the gyro. It's important to make sure that the robot is not moving while the
013   * calibration is in progress, this is typically done when the robot is first turned on while it's
014   * sitting at rest before the match starts.
015   */
016  void calibrate();
017
018  /**
019   * Reset the gyro. Resets the gyro to a heading of zero. This can be used if there is significant
020   * drift in the gyro and it needs to be recalibrated after it has been running.
021   */
022  void reset();
023
024  /**
025   * Return the heading of the robot in degrees.
026   *
027   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
028   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
029   * 360 to 0 on the second time around.
030   *
031   * <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
032   * It needs to follow the NED axis convention.
033   *
034   * <p>This heading is based on integration of the returned rate from the gyro.
035   *
036   * @return the current heading of the robot in degrees.
037   */
038  double getAngle();
039
040  /**
041   * Return the rate of rotation of the gyro.
042   *
043   * <p>The rate is based on the most recent reading of the gyro analog value
044   *
045   * <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
046   * It needs to follow the NED axis convention.
047   *
048   * @return the current rate in degrees per second
049   */
050  double getRate();
051
052  /**
053   * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
054   *
055   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
056   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
057   * 360 to 0 on the second time around.
058   *
059   * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
060   * top. It needs to follow the NWU axis convention.
061   *
062   * <p>This heading is based on integration of the returned rate from the gyro.
063   *
064   * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
065   */
066  default Rotation2d getRotation2d() {
067    return Rotation2d.fromDegrees(-getAngle());
068  }
069}