diff --git a/.gitignore b/.gitignore index 2a577aed..3f18d73e 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,8 @@ template/ version cquery_log.txt compile_commands.json +.ccls-cache/ +.ccls temp.log temp.errors -.d/ \ No newline at end of file +.d/ diff --git a/firmware/libv5rts b/firmware/libv5rts index aca47b08..734552e0 160000 --- a/firmware/libv5rts +++ b/firmware/libv5rts @@ -1 +1 @@ -Subproject commit aca47b08fdeab48b318c1d183dd17804a01e2cd9 +Subproject commit 734552e0c3d38a436cacf4a66b95f13243f2006c diff --git a/include/api.h b/include/api.h index 6e57f579..10b8248e 100644 --- a/include/api.h +++ b/include/api.h @@ -41,14 +41,15 @@ #define PROS_VERSION_MAJOR 3 #define PROS_VERSION_MINOR 2 -#define PROS_VERSION_PATCH 0 -#define PROS_VERSION_STRING "3.2.0" +#define PROS_VERSION_PATCH 1 +#define PROS_VERSION_STRING "3.2.1" #define PROS_ERR (INT32_MAX) #define PROS_ERR_F (INFINITY) #include "pros/adi.h" #include "pros/colors.h" +#include "pros/imu.h" #include "pros/llemu.h" #include "pros/misc.h" #include "pros/motors.h" @@ -57,6 +58,7 @@ #ifdef __cplusplus #include "pros/adi.hpp" +#include "pros/imu.hpp" #include "pros/llemu.hpp" #include "pros/misc.hpp" #include "pros/motors.hpp" diff --git a/include/pros/apix.h b/include/pros/apix.h index ca278b4d..9ca9d3bb 100644 --- a/include/pros/apix.h +++ b/include/pros/apix.h @@ -369,6 +369,7 @@ void queue_reset(queue_t queue); typedef enum v5_device_e { E_DEVICE_NONE = 0, E_DEVICE_MOTOR = 2, + E_DEVICE_IMU = 6, E_DEVICE_RADIO = 8, E_DEVICE_VISION = 11, E_DEVICE_ADI = 12, diff --git a/include/pros/imu.h b/include/pros/imu.h new file mode 100644 index 00000000..48a2d024 --- /dev/null +++ b/include/pros/imu.h @@ -0,0 +1,256 @@ +/** + * \file pros/imu.h + * + * Contains prototypes for functions related to the VEX Inertial sensor. + * + * Visit https://pros.cs.purdue.edu/v5/tutorials/topical/imu.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2020, Purdue University ACM SIGBots. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#ifndef _PROS_IMU_H_ +#define _PROS_IMU_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +namespace pros { +namespace c { +#endif + +typedef enum imu_status_e { + E_IMU_STATUS_CALIBRATING = 0x01, + E_IMU_STATUS_ERROR = 0xFF, // NOTE: used for returning an error from the get_status function, not that the IMU is + // necessarily in an error state +} imu_status_e_t; + +typedef struct __attribute__((__packed__)) quaternion_s { + double x; + double y; + double z; + double w; +} quaternion_s_t; + +struct imu_raw_s { + double x; + double y; + double z; +}; + +typedef struct imu_raw_s imu_gyro_s_t; +typedef struct imu_raw_s imu_accel_s_t; + +typedef struct __attribute__((__packed__)) euler_s { + double pitch; + double roll; + double yaw; +} euler_s_t; + +/** + * Calibrate IMU + * + * This takes approximately 2 seconds, and is a non-blocking operation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is already calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ +int32_t imu_reset(uint8_t port); + +/** + * Get the total number of degrees the Inertial Sensor has spun about the z-axis + * + * This value is theoretically unbounded. Clockwise rotations are represented + * with positive degree values, while counterclockwise rotations are represented + * with negative ones. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The degree value or PROS_ERR_F if the operation failed, setting + * errno. + */ +double imu_get_rotation(uint8_t port); + +/** + * Get the Inertial Sensor's heading relative to the initial direction of its + * x-axis + * + * This value is bounded by (-360,360). Clockwise rotations are represented with + * positive degree values, while counterclockwise rotations are represented with + * negative ones. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The degree value or PROS_ERR_F if the operation failed, setting + * errno. + */ +double imu_get_heading(uint8_t port); + +/** + * Get a quaternion representing the Inertial Sensor's orientation + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The quaternion representing the sensor's orientation. If the + * operation failed, all the quaternion's members are filled with PROS_ERR_F and + * errno is set. + */ +quaternion_s_t imu_get_quaternion(uint8_t port); + +/** + * Get the Euler angles representing the Inertial Sensor's orientation + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The Euler angles representing the sensor's orientation. If the + * operation failed, all the structure's members are filled with PROS_ERR_F and + * errno is set. + */ +euler_s_t imu_get_euler(uint8_t port); + +/** + * Get the Inertial Sensor's pitch angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The pitch angle, or PROS_ERR_F if the operation failed, setting + * errno. + */ +double imu_get_pitch(uint8_t port); + +/** + * Get the Inertial Sensor's roll angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The roll angle, or PROS_ERR_F if the operation failed, setting errno. + */ +double imu_get_roll(uint8_t port); + +/** + * Get the Inertial Sensor's yaw angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The yaw angle, or PROS_ERR_F if the operation failed, setting errno. + */ +double imu_get_yaw(uint8_t port); + +/** + * Get the Inertial Sensor's raw gyroscope values + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The raw gyroscope values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ +imu_gyro_s_t imu_get_gyro_rate(uint8_t port); + +/** + * Get the Inertial Sensor's raw acceleroneter values + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The raw accelerometer values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ +imu_accel_s_t imu_get_accel(uint8_t port); + +/** + * Get the Inertial Sensor's status + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The Inertial Sensor's status code, or PROS_ERR if the operation + * failed, setting errno. + */ +imu_status_e_t imu_get_status(uint8_t port); + +// NOTE: not used +// void imu_set_mode(uint8_t port, uint32_t mode); +// uint32_t imu_get_mode(uint8_t port); + +#ifdef __cplusplus +} +} +} +#endif + +#endif diff --git a/include/pros/imu.hpp b/include/pros/imu.hpp new file mode 100644 index 00000000..105a8ab3 --- /dev/null +++ b/include/pros/imu.hpp @@ -0,0 +1,217 @@ +/** + * \file pros/imu.hpp + * + * Contains prototypes for functions related to the VEX Inertial sensor. + * + * Visit https://pros.cs.purdue.edu/v5/tutorials/topical/imu.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2020, Purdue University ACM SIGBots. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +#ifndef _PROS_IMU_HPP_ +#define _PROS_IMU_HPP_ + +#include +#include "pros/imu.h" + +namespace pros { +class Imu { + const std::uint8_t _port; + + public: + Imu(const std::uint8_t port) : _port(port){}; + + /** + * Calibrate IMU + * + * This takes approximately 2 seconds, and is a non-blocking operation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is already calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t reset() const; + /** + * Get the total number of degrees the Inertial Sensor has spun about the z-axis + * + * This value is theoretically unbounded. Clockwise rotations are represented + * with positive degree values, while counterclockwise rotations are represented + * with negative ones. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The degree value or PROS_ERR_F if the operation failed, setting + * errno. + */ + virtual double get_rotation() const; + /** + * Get the Inertial Sensor's heading relative to the initial direction of its + * x-axis + * + * This value is bounded by (-360,360). Clockwise rotations are represented with + * positive degree values, while counterclockwise rotations are represented with + * negative ones. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The degree value or PROS_ERR_F if the operation failed, setting + * errno. + */ + virtual double get_heading() const; + /** + * Get a quaternion representing the Inertial Sensor's orientation + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The quaternion representing the sensor's orientation. If the + * operation failed, all the quaternion's members are filled with PROS_ERR_F and + * errno is set. + */ + virtual pros::c::quaternion_s_t get_quaternion() const; + /** + * Get the Euler angles representing the Inertial Sensor's orientation + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The Euler angles representing the sensor's orientation. If the + * operation failed, all the structure's members are filled with PROS_ERR_F and + * errno is set. + */ + virtual pros::c::euler_s_t get_euler() const; + /** + * Get the Inertial Sensor's pitch angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The pitch angle, or PROS_ERR_F if the operation failed, setting + * errno. + */ + virtual double get_pitch() const; + /** + * Get the Inertial Sensor's roll angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The roll angle, or PROS_ERR_F if the operation failed, setting errno. + */ + virtual double get_roll() const; + /** + * Get the Inertial Sensor's yaw angle + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The yaw angle, or PROS_ERR_F if the operation failed, setting errno. + */ + virtual double get_yaw() const; + /** + * Get the Inertial Sensor's raw gyroscope values + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The raw gyroscope values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ + virtual pros::c::imu_gyro_s_t get_gyro_rate() const; + /** + * Get the Inertial Sensor's raw acceleroneter values + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The raw accelerometer values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ + virtual pros::c::imu_accel_s_t get_accel() const; + /** + * Get the Inertial Sensor's status + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \return The Inertial Sensor's status code, or PROS_ERR if the operation + * failed, setting errno. + */ + virtual pros::c::imu_status_e_t get_status() const; + /** + * Check whether the IMU is calibrating + * + * \return true if the V5 Inertial Sensor is calibrating or false + * false if it is not. + */ + virtual bool is_calibrating() const; +}; +} // namespace pros + +#endif diff --git a/src/devices/vdml_imu.c b/src/devices/vdml_imu.c new file mode 100644 index 00000000..6b11f36b --- /dev/null +++ b/src/devices/vdml_imu.c @@ -0,0 +1,144 @@ +/** + * \file devices/vdml_imu.c + * + * Contains functions for interacting with the VEX Inertial sensor. + * + * Copyright (c) 2017-2019, Purdue University ACM SIGBots. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include +#include "pros/imu.h" +#include "v5_api.h" +#include "vdml/registry.h" +#include "vdml/vdml.h" + +#define ERROR_IMU_STILL_CALIBRATING(port, device, err_return) \ + if (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING) { \ + errno = EAGAIN; \ + return_port(port - 1, err_return); \ + } + +int32_t imu_reset(uint8_t port) { + claim_port_i(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + vexDeviceImuReset(device->device_info); + return_port(port - 1, 1); +} + +double imu_get_rotation(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); + double rtn = vexDeviceImuHeadingGet(device->device_info); + return_port(port - 1, rtn); +} + +double imu_get_heading(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); + double rtn = vexDeviceImuDegreesGet(device->device_info); + return_port(port - 1, rtn); +} + +#define QUATERNION_ERR_INIT \ + { .x = PROS_ERR_F, .y = PROS_ERR_F, .z = PROS_ERR_F, .w = PROS_ERR_F } + +quaternion_s_t imu_get_quaternion(uint8_t port) { + quaternion_s_t rtn = QUATERNION_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + // HACK: vexos represents quaternions as {a,b,c,d} and we want them in + // {x,y,z,w}. we don't know how exactly the quaternion data is filled, so best + // just to manually shuffle stuff into the right places here + V5_DeviceImuQuaternion qt; + vexDeviceImuQuaternionGet(device->device_info, &qt); + rtn.x = qt.b; + rtn.y = qt.c; + rtn.z = qt.d; + rtn.w = qt.a; + return_port(port - 1, rtn); +} + +#define ATTITUDE_ERR_INIT \ + { .pitch = PROS_ERR_F, .roll = PROS_ERR_F, .yaw = PROS_ERR_F } + +euler_s_t imu_get_euler(uint8_t port) { + euler_s_t rtn = ATTITUDE_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&rtn); + return_port(port - 1, rtn); +} + +double imu_get_pitch(uint8_t port) { + return imu_get_euler(port).pitch; +} + +double imu_get_roll(uint8_t port) { + return imu_get_euler(port).roll; +} + +double imu_get_yaw(uint8_t port) { + return imu_get_euler(port).yaw; +} + +#define RAW_IMU_ERR_INIT {.x = PROS_ERR_F, .y = PROS_ERR_F, .z = PROS_ERR_F}; + +imu_gyro_s_t imu_get_gyro_rate(uint8_t port) { + imu_gyro_s_t rtn = RAW_IMU_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + // NOTE: `V5_DeviceImuRaw` has the same form as a quaternion, but this call + // never fills the `w` field, so we make a dummy quaternion container and copy + // the (x,y,z) part into the return struct + quaternion_s_t dummy; + vexDeviceImuRawGyroGet(device->device_info, (V5_DeviceImuRaw*)&dummy); + rtn.x = dummy.x; + rtn.y = dummy.y; + rtn.z = dummy.z; + return_port(port - 1, rtn); +} + +imu_accel_s_t imu_get_accel(uint8_t port) { + imu_accel_s_t rtn = RAW_IMU_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + // NOTE: this is the same as `imu_get_raw_gyro` + quaternion_s_t dummy; + vexDeviceImuRawAccelGet(device->device_info, (V5_DeviceImuRaw*)&dummy); + rtn.x = dummy.x; + rtn.y = dummy.y; + rtn.z = dummy.z; + return_port(port - 1, rtn); +} + +imu_status_e_t imu_get_status(uint8_t port) { + imu_status_e_t rtn = E_IMU_STATUS_ERROR; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, E_IMU_STATUS_ERROR); + rtn = vexDeviceImuStatusGet(device->device_info); + return_port(port - 1, rtn); +} diff --git a/src/devices/vdml_imu.cpp b/src/devices/vdml_imu.cpp new file mode 100644 index 00000000..dd5e2f98 --- /dev/null +++ b/src/devices/vdml_imu.cpp @@ -0,0 +1,63 @@ +/** + * \file devices/vdml_imu.cpp + * + * Contains functions for interacting with the VEX Inertial sensor. + * + * Copyright (c) 2017-2019, Purdue University ACM SIGBots. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include "pros/imu.hpp" + +namespace pros { +std::int32_t Imu::reset() const { + return pros::c::imu_reset(_port); +} + +double Imu::get_rotation() const { + return pros::c::imu_get_rotation(_port); +} + +double Imu::get_heading() const { + return pros::c::imu_get_heading(_port); +} + +pros::c::quaternion_s_t Imu::get_quaternion() const { + return pros::c::imu_get_quaternion(_port); +} + +pros::c::euler_s_t Imu::get_euler() const { + return pros::c::imu_get_euler(_port); +} + +double Imu::get_pitch() const { + return get_euler().pitch; +} + +double Imu::get_roll() const { + return get_euler().roll; +} + +double Imu::get_yaw() const { + return get_euler().yaw; +} + +pros::c::imu_gyro_s_t Imu::get_gyro_rate() const { + return pros::c::imu_get_gyro_rate(_port); +} + +pros::c::imu_accel_s_t Imu::get_accel() const { + return pros::c::imu_get_accel(_port); +} + +pros::c::imu_status_e_t Imu::get_status() const { + return pros::c::imu_get_status(_port); +} + +bool Imu::is_calibrating() const { + return get_status() & pros::c::E_IMU_STATUS_CALIBRATING; +} +} // namespace pros