WPILibC++ 2024.3.2
|
Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and return the robot's heading relative to a starting position and instant measurements. More...
#include <frc/ADIS16470_IMU.h>
Public Types | |
enum class | CalibrationTime { _32ms = 0 , _64ms = 1 , _128ms = 2 , _256ms = 3 , _512ms = 4 , _1s = 5 , _2s = 6 , _4s = 7 , _8s = 8 , _16s = 9 , _32s = 10 , _64s = 11 } |
ADIS16470 calibration times. More... | |
enum | IMUAxis { kX , kY , kZ , kYaw , kPitch , kRoll } |
IMU axes. More... | |
Public Member Functions | |
ADIS16470_IMU () | |
Creates a new ADIS16740 IMU object. More... | |
ADIS16470_IMU (IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) | |
Creates a new ADIS16740 IMU object. More... | |
ADIS16470_IMU (IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis, frc::SPI::Port port, CalibrationTime cal_time) | |
Creates a new ADIS16740 IMU object. More... | |
~ADIS16470_IMU () override | |
ADIS16470_IMU (ADIS16470_IMU &&other) | |
ADIS16470_IMU & | operator= (ADIS16470_IMU &&other) |
int | ConfigDecRate (uint16_t decimationRate) |
Configures the decimation rate of the IMU. More... | |
void | Calibrate () |
Switches the active SPI port to standard SPI mode, writes the command to activate the new null configuration, and re-enables auto SPI. More... | |
int | ConfigCalTime (CalibrationTime new_cal_time) |
Switches the active SPI port to standard SPI mode, writes a new value to the NULL_CNFG register in the IMU, and re-enables auto SPI. More... | |
void | Reset () |
Reset the gyro. More... | |
void | SetGyroAngle (IMUAxis axis, units::degree_t angle) |
Allow the designated gyro angle to be set to a given value. More... | |
void | SetGyroAngleX (units::degree_t angle) |
Allow the gyro angle X to be set to a given value. More... | |
void | SetGyroAngleY (units::degree_t angle) |
Allow the gyro angle Y to be set to a given value. More... | |
void | SetGyroAngleZ (units::degree_t angle) |
Allow the gyro angle Z to be set to a given value. More... | |
units::degree_t | GetAngle (IMUAxis axis=IMUAxis::kYaw) const |
Returns the axis angle (CCW positive). More... | |
units::degrees_per_second_t | GetRate (IMUAxis axis=IMUAxis::kYaw) const |
Returns the axis angular rate (CCW positive). More... | |
units::meters_per_second_squared_t | GetAccelX () const |
Returns the acceleration in the X axis. More... | |
units::meters_per_second_squared_t | GetAccelY () const |
Returns the acceleration in the Y axis. More... | |
units::meters_per_second_squared_t | GetAccelZ () const |
Returns the acceleration in the Z axis. More... | |
units::degree_t | GetXComplementaryAngle () const |
Returns the X-axis complementary angle. More... | |
units::degree_t | GetYComplementaryAngle () const |
Returns the Y-axis complementary angle. More... | |
units::degree_t | GetXFilteredAccelAngle () const |
Returns the X-axis filtered acceleration angle. More... | |
units::degree_t | GetYFilteredAccelAngle () const |
Returns the Y-axis filtered acceleration angle. More... | |
IMUAxis | GetYawAxis () const |
Returns which axis, kX, kY, or kZ, is set to the yaw axis. More... | |
IMUAxis | GetPitchAxis () const |
Returns which axis, kX, kY, or kZ, is set to the pitch axis. More... | |
IMUAxis | GetRollAxis () const |
Returns which axis, kX, kY, or kZ, is set to the roll axis. More... | |
bool | IsConnected () const |
Checks the connection status of the IMU. More... | |
int | GetPort () const |
Gets the SPI port number. More... | |
void | InitSendable (wpi::SendableBuilder &builder) override |
Initializes this Sendable object. More... | |
Public Member Functions inherited from wpi::Sendable | |
virtual | ~Sendable ()=default |
virtual void | InitSendable (SendableBuilder &builder)=0 |
Initializes this Sendable object. More... | |
Public Member Functions inherited from wpi::SendableHelper< ADIS16470_IMU > | |
SendableHelper (const SendableHelper &rhs)=default | |
SendableHelper (SendableHelper &&rhs) | |
SendableHelper & | operator= (const SendableHelper &rhs)=default |
SendableHelper & | operator= (SendableHelper &&rhs) |
Public Attributes | |
IMUAxis | m_yaw_axis |
IMUAxis | m_pitch_axis |
IMUAxis | m_roll_axis |
Additional Inherited Members | |
Protected Member Functions inherited from wpi::SendableHelper< ADIS16470_IMU > | |
SendableHelper ()=default | |
~SendableHelper () | |
Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and return the robot's heading relative to a starting position and instant measurements.
The ADIS16470 gyro angle outputs track the robot's heading based on the starting position. As the robot rotates the new heading is computed by integrating the rate of rotation returned by the IMU. When the class is instantiated, a short calibration routine is performed where the IMU samples the gyros while at rest to determine the initial offset. This is subtracted from each sample to determine the heading.
This class is for the ADIS16470 IMU connected via the primary SPI port available on the RoboRIO.
|
strong |
ADIS16470 calibration times.
IMU axes.
kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw, kPitch, and kRoll are configured by the user to refer to an X, Y, or Z axis.
Enumerator | |
---|---|
kX | The IMU's X axis. |
kY | The IMU's Y axis. |
kZ | The IMU's Z axis. |
kYaw | The user-configured yaw axis. |
kPitch | The user-configured pitch axis. |
kRoll | The user-configured roll axis. |
frc::ADIS16470_IMU::ADIS16470_IMU | ( | ) |
Creates a new ADIS16740 IMU object.
The default setup is the onboard SPI port with a calibration time of 4 seconds. Yaw, pitch, and roll are kZ, kX, and kY respectively.
Creates a new ADIS16740 IMU object.
The default setup is the onboard SPI port with a calibration time of 4 seconds.
Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in an error.
yaw_axis | The axis that measures the yaw |
pitch_axis | The axis that measures the pitch |
roll_axis | The axis that measures the roll |
|
explicit |
Creates a new ADIS16740 IMU object.
Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch, or kRoll will result in an error.
yaw_axis | The axis that measures the yaw |
pitch_axis | The axis that measures the pitch |
roll_axis | The axis that measures the roll |
port | The SPI Port the gyro is plugged into |
cal_time | Calibration time |
|
override |
frc::ADIS16470_IMU::ADIS16470_IMU | ( | ADIS16470_IMU && | other | ) |
void frc::ADIS16470_IMU::Calibrate | ( | ) |
int frc::ADIS16470_IMU::ConfigCalTime | ( | CalibrationTime | new_cal_time | ) |
int frc::ADIS16470_IMU::ConfigDecRate | ( | uint16_t | decimationRate | ) |
Configures the decimation rate of the IMU.
decimationRate | The new decimation value. |
units::meters_per_second_squared_t frc::ADIS16470_IMU::GetAccelX | ( | ) | const |
Returns the acceleration in the X axis.
units::meters_per_second_squared_t frc::ADIS16470_IMU::GetAccelY | ( | ) | const |
Returns the acceleration in the Y axis.
units::meters_per_second_squared_t frc::ADIS16470_IMU::GetAccelZ | ( | ) | const |
Returns the acceleration in the Z axis.
units::degree_t frc::ADIS16470_IMU::GetAngle | ( | IMUAxis | axis = IMUAxis::kYaw | ) | const |
Returns the axis angle (CCW positive).
axis | The IMUAxis whose angle to return. Defaults to user configured Yaw. |
IMUAxis frc::ADIS16470_IMU::GetPitchAxis | ( | ) | const |
Returns which axis, kX, kY, or kZ, is set to the pitch axis.
units::degrees_per_second_t frc::ADIS16470_IMU::GetRate | ( | IMUAxis | axis = IMUAxis::kYaw | ) | const |
Returns the axis angular rate (CCW positive).
axis | The IMUAxis whose rate to return. Defaults to user configured Yaw. |
IMUAxis frc::ADIS16470_IMU::GetRollAxis | ( | ) | const |
Returns which axis, kX, kY, or kZ, is set to the roll axis.
units::degree_t frc::ADIS16470_IMU::GetXComplementaryAngle | ( | ) | const |
Returns the X-axis complementary angle.
units::degree_t frc::ADIS16470_IMU::GetXFilteredAccelAngle | ( | ) | const |
Returns the X-axis filtered acceleration angle.
IMUAxis frc::ADIS16470_IMU::GetYawAxis | ( | ) | const |
Returns which axis, kX, kY, or kZ, is set to the yaw axis.
units::degree_t frc::ADIS16470_IMU::GetYComplementaryAngle | ( | ) | const |
Returns the Y-axis complementary angle.
units::degree_t frc::ADIS16470_IMU::GetYFilteredAccelAngle | ( | ) | const |
Returns the Y-axis filtered acceleration angle.
|
overridevirtual |
bool frc::ADIS16470_IMU::IsConnected | ( | ) | const |
Checks the connection status of the IMU.
ADIS16470_IMU & frc::ADIS16470_IMU::operator= | ( | ADIS16470_IMU && | other | ) |
void frc::ADIS16470_IMU::Reset | ( | ) |
Reset the gyro.
Resets the gyro accumulations to a heading of zero. This can be used if there is significant drift in the gyro and it needs to be recalibrated after running.
void frc::ADIS16470_IMU::SetGyroAngle | ( | IMUAxis | axis, |
units::degree_t | angle | ||
) |
Allow the designated gyro angle to be set to a given value.
This may happen with unread values in the buffer, it is suggested that the IMU is not moving when this method is run.
axis | IMUAxis that will be changed |
angle | The new angle (CCW positive) |
void frc::ADIS16470_IMU::SetGyroAngleX | ( | units::degree_t | angle | ) |
Allow the gyro angle X to be set to a given value.
This may happen with unread values in the buffer, it is suggested that the IMU is not moving when this method is run.
angle | The new angle (CCW positive) |
void frc::ADIS16470_IMU::SetGyroAngleY | ( | units::degree_t | angle | ) |
Allow the gyro angle Y to be set to a given value.
This may happen with unread values in the buffer, it is suggested that the IMU is not moving when this method is run.
angle | The new angle (CCW positive) |
void frc::ADIS16470_IMU::SetGyroAngleZ | ( | units::degree_t | angle | ) |
Allow the gyro angle Z to be set to a given value.
This may happen with unread values in the buffer, it is suggested that the IMU is not moving when this method is run.
angle | The new angle (CCW positive) |
IMUAxis frc::ADIS16470_IMU::m_pitch_axis |
IMUAxis frc::ADIS16470_IMU::m_roll_axis |
IMUAxis frc::ADIS16470_IMU::m_yaw_axis |