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.SimDouble;
008import edu.wpi.first.wpilibj.ADIS16470_IMU;
009
010/** Class to control a simulated ADIS16470 gyroscope. */
011@SuppressWarnings("TypeName")
012public class ADIS16470_IMUSim {
013  private final SimDouble m_simGyroAngleX;
014  private final SimDouble m_simGyroAngleY;
015  private final SimDouble m_simGyroAngleZ;
016  private final SimDouble m_simGyroRateX;
017  private final SimDouble m_simGyroRateY;
018  private final SimDouble m_simGyroRateZ;
019  private final SimDouble m_simAccelX;
020  private final SimDouble m_simAccelY;
021  private final SimDouble m_simAccelZ;
022
023  /**
024   * Constructs from an ADIS16470_IMU object.
025   *
026   * @param gyro ADIS16470_IMU to simulate
027   */
028  public ADIS16470_IMUSim(ADIS16470_IMU gyro) {
029    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]");
030    m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
031    m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
032    m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
033    m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
034    m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
035    m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
036    m_simAccelX = wrappedSimDevice.getDouble("accel_x");
037    m_simAccelY = wrappedSimDevice.getDouble("accel_y");
038    m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
039  }
040
041  /**
042   * Sets the X axis angle in degrees (CCW positive).
043   *
044   * @param angleDegrees The angle.
045   */
046  public void setGyroAngleX(double angleDegrees) {
047    m_simGyroAngleX.set(angleDegrees);
048  }
049
050  /**
051   * Sets the Y axis angle in degrees (CCW positive).
052   *
053   * @param angleDegrees The angle.
054   */
055  public void setGyroAngleY(double angleDegrees) {
056    m_simGyroAngleY.set(angleDegrees);
057  }
058
059  /**
060   * Sets the Z axis angle in degrees (CCW positive).
061   *
062   * @param angleDegrees The angle.
063   */
064  public void setGyroAngleZ(double angleDegrees) {
065    m_simGyroAngleZ.set(angleDegrees);
066  }
067
068  /**
069   * Sets the X axis angle in degrees per second (CCW positive).
070   *
071   * @param angularRateDegreesPerSecond The angular rate.
072   */
073  public void setGyroRateX(double angularRateDegreesPerSecond) {
074    m_simGyroRateX.set(angularRateDegreesPerSecond);
075  }
076
077  /**
078   * Sets the Y axis angle in degrees per second (CCW positive).
079   *
080   * @param angularRateDegreesPerSecond The angular rate.
081   */
082  public void setGyroRateY(double angularRateDegreesPerSecond) {
083    m_simGyroRateY.set(angularRateDegreesPerSecond);
084  }
085
086  /**
087   * Sets the Z axis angle in degrees per second (CCW positive).
088   *
089   * @param angularRateDegreesPerSecond The angular rate.
090   */
091  public void setGyroRateZ(double angularRateDegreesPerSecond) {
092    m_simGyroRateZ.set(angularRateDegreesPerSecond);
093  }
094
095  /**
096   * Sets the X axis acceleration in meters per second squared.
097   *
098   * @param accelMetersPerSecondSquared The acceleration.
099   */
100  public void setAccelX(double accelMetersPerSecondSquared) {
101    m_simAccelX.set(accelMetersPerSecondSquared);
102  }
103
104  /**
105   * Sets the Y axis acceleration in meters per second squared.
106   *
107   * @param accelMetersPerSecondSquared The acceleration.
108   */
109  public void setAccelY(double accelMetersPerSecondSquared) {
110    m_simAccelY.set(accelMetersPerSecondSquared);
111  }
112
113  /**
114   * Sets the Z axis acceleration in meters per second squared.
115   *
116   * @param accelMetersPerSecondSquared The acceleration.
117   */
118  public void setAccelZ(double accelMetersPerSecondSquared) {
119    m_simAccelZ.set(accelMetersPerSecondSquared);
120  }
121}