diff --git a/hal/src/main/java/org/wpilib/hardware/hal/simulation/IMUDataJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/simulation/IMUDataJNI.java new file mode 100644 index 0000000000..3cac41839b --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/simulation/IMUDataJNI.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal.simulation; + +import org.wpilib.hardware.hal.JNIWrapper; + +/** JNI for imu data. */ +public class IMUDataJNI extends JNIWrapper { + public static native void setAngleX(double angleRad); + + public static native void setAngleY(double angleRad); + + public static native void setAngleZ(double angleRad); + + public static native void setGyroRateX(double rateRadPerSec); + + public static native void setGyroRateY(double rateRadPerSec); + + public static native void setGyroRateZ(double rateRadPerSec); + + public static native void setAccelX(double accelMpss); + + public static native void setAccelY(double accelMpss); + + public static native void setAccelZ(double accelMpss); + + public static native void setYaw(double angleRad); +} diff --git a/hal/src/main/native/cpp/jni/simulation/IMUDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/IMUDataJNI.cpp new file mode 100644 index 0000000000..84919e3302 --- /dev/null +++ b/hal/src/main/native/cpp/jni/simulation/IMUDataJNI.cpp @@ -0,0 +1,132 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "org_wpilib_hardware_hal_simulation_IMUDataJNI.h" +#include "wpi/hal/simulation/IMUData.h" + +extern "C" { + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAngleX + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleX + (JNIEnv*, jclass, jdouble angle) +{ + return HALSIM_SetIMUAngleX(angle); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAngleY + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleY + (JNIEnv*, jclass, jdouble angle) +{ + return HALSIM_SetIMUAngleY(angle); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAngleZ + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAngleZ + (JNIEnv*, jclass, jdouble angle) +{ + return HALSIM_SetIMUAngleZ(angle); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setGyroRateX + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateX + (JNIEnv*, jclass, jdouble rate) +{ + return HALSIM_SetIMUGyroRateX(rate); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setGyroRateY + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateY + (JNIEnv*, jclass, jdouble rate) +{ + return HALSIM_SetIMUGyroRateY(rate); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setGyroRateZ + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setGyroRateZ + (JNIEnv*, jclass, jdouble rate) +{ + return HALSIM_SetIMUGyroRateZ(rate); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAccelX + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelX + (JNIEnv*, jclass, jdouble accel) +{ + return HALSIM_SetIMUAccelX(accel); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAccelY + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelY + (JNIEnv*, jclass, jdouble accel) +{ + return HALSIM_SetIMUAccelY(accel); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setAccelZ + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setAccelZ + (JNIEnv*, jclass, jdouble accel) +{ + return HALSIM_SetIMUAccelZ(accel); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_IMUDataJNI + * Method: setYaw + * Signature: (D)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_IMUDataJNI_setYaw + (JNIEnv*, jclass, jdouble angle) +{ + return HALSIM_SetIMUYaw(angle); +} + +} // extern "C" diff --git a/hal/src/main/native/include/wpi/hal/simulation/IMUData.h b/hal/src/main/native/include/wpi/hal/simulation/IMUData.h new file mode 100644 index 0000000000..2637298927 --- /dev/null +++ b/hal/src/main/native/include/wpi/hal/simulation/IMUData.h @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/hal/Types.h" +#include "wpi/hal/simulation/NotifyListener.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void HALSIM_SetIMUAngleX(double angle); +void HALSIM_SetIMUAngleY(double angle); +void HALSIM_SetIMUAngleZ(double angle); + +void HALSIM_SetIMUGyroRateX(double rate); +void HALSIM_SetIMUGyroRateY(double rate); +void HALSIM_SetIMUGyroRateZ(double rate); + +void HALSIM_SetIMUAccelX(double accel); +void HALSIM_SetIMUAccelY(double accel); +void HALSIM_SetIMUAccelZ(double accel); + +void HALSIM_SetIMUYaw(double angle); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 38757a1275..379fed41e9 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -74,6 +74,7 @@ void InitializeHAL() { InitializeDIOData(); InitializeDriverStationData(); InitializeEncoderData(); + InitializeIMUData(); InitializeI2CData(); InitializeCTREPCMData(); InitializeREVPHData(); diff --git a/hal/src/main/native/sim/HALInitializer.hpp b/hal/src/main/native/sim/HALInitializer.hpp index 4e297b2015..41d35109fd 100644 --- a/hal/src/main/native/sim/HALInitializer.hpp +++ b/hal/src/main/native/sim/HALInitializer.hpp @@ -26,6 +26,7 @@ extern void InitializeDIOData(); extern void InitializeDutyCycle(); extern void InitializeDriverStationData(); extern void InitializeEncoderData(); +extern void InitializeIMUData(); extern void InitializeI2CData(); extern void InitializeCTREPCMData(); extern void InitializeREVPHData(); diff --git a/hal/src/main/native/sim/IMU.cpp b/hal/src/main/native/sim/IMU.cpp index e42407384f..b6c6f2527d 100644 --- a/hal/src/main/native/sim/IMU.cpp +++ b/hal/src/main/native/sim/IMU.cpp @@ -4,34 +4,62 @@ #include "wpi/hal/IMU.h" +#include "mockdata/IMUDataInternal.hpp" + +using namespace wpi::hal; + extern "C" { -// TODO(Ryan) implement sim void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { - *accel = {}; + *accel = { + .timestamp = 0, + .x = SimIMUData->accelX, + .y = SimIMUData->accelY, + .z = SimIMUData->accelZ, + }; } void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { - *rate = {}; + *rate = { + .timestamp = 0, + .x = SimIMUData->gyroRateX, + .y = SimIMUData->gyroRateY, + .z = SimIMUData->gyroRateZ, + }; } void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) { - *angles = {}; + *angles = { + .timestamp = 0, + .x = SimIMUData->angleX, + .y = SimIMUData->angleY, + .z = SimIMUData->angleZ, + }; } void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles, int32_t* status) { - *angles = {}; + *angles = { + .timestamp = 0, + .x = SimIMUData->angleX, + .y = SimIMUData->angleY, + .z = SimIMUData->angleZ, + }; } void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) { - *angles = {}; + *angles = { + .timestamp = 0, + .x = SimIMUData->angleX, + .y = SimIMUData->angleY, + .z = SimIMUData->angleZ, + }; } void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) { *quat = {}; } double HAL_GetIMUYawFlat(int64_t* timestamp) { - return 0; + return SimIMUData->yaw; } double HAL_GetIMUYawLandscape(int64_t* timestamp) { - return 0; + return SimIMUData->yaw; } double HAL_GetIMUYawPortrait(int64_t* timestamp) { - return 0; + return SimIMUData->yaw; } } // extern "C" diff --git a/hal/src/main/native/sim/mockdata/IMUData.cpp b/hal/src/main/native/sim/mockdata/IMUData.cpp new file mode 100644 index 0000000000..f1063a6e9e --- /dev/null +++ b/hal/src/main/native/sim/mockdata/IMUData.cpp @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "IMUDataInternal.hpp" + +using namespace wpi::hal; + +namespace wpi::hal { +namespace init { +void InitializeIMUData() { + static IMUData imu; + ::wpi::hal::SimIMUData = &imu; +} +} // namespace init + +IMUData* SimIMUData; +} // namespace wpi::hal + +extern "C" { +#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \ + HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, IMU##CAPINAME, \ + SimIMUData, LOWERNAME) + +DEFINE_CAPI(double, AngleX, angleX) +DEFINE_CAPI(double, AngleY, angleY) +DEFINE_CAPI(double, AngleZ, angleZ) + +DEFINE_CAPI(double, GyroRateX, gyroRateX) +DEFINE_CAPI(double, GyroRateY, gyroRateY) +DEFINE_CAPI(double, GyroRateZ, gyroRateZ) + +DEFINE_CAPI(double, AccelX, accelX) +DEFINE_CAPI(double, AccelY, accelY) +DEFINE_CAPI(double, AccelZ, accelZ) + +DEFINE_CAPI(double, Yaw, yaw) + +} // extern "C" diff --git a/hal/src/main/native/sim/mockdata/IMUDataInternal.hpp b/hal/src/main/native/sim/mockdata/IMUDataInternal.hpp new file mode 100644 index 0000000000..b341b59723 --- /dev/null +++ b/hal/src/main/native/sim/mockdata/IMUDataInternal.hpp @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/hal/simulation/SimDataValue.hpp" + +namespace wpi::hal { + +class IMUData { + HAL_SIMDATAVALUE_DEFINE_NAME(AngleX) + HAL_SIMDATAVALUE_DEFINE_NAME(AngleY) + HAL_SIMDATAVALUE_DEFINE_NAME(AngleZ) + + HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateX) + HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateY) + HAL_SIMDATAVALUE_DEFINE_NAME(GyroRateZ) + + HAL_SIMDATAVALUE_DEFINE_NAME(AccelX) + HAL_SIMDATAVALUE_DEFINE_NAME(AccelY) + HAL_SIMDATAVALUE_DEFINE_NAME(AccelZ) + + HAL_SIMDATAVALUE_DEFINE_NAME(Yaw) + + public: + SimDataValue angleX{0}; + SimDataValue angleY{0}; + SimDataValue angleZ{0}; + + SimDataValue gyroRateX{0}; + SimDataValue gyroRateY{0}; + SimDataValue gyroRateZ{0}; + + SimDataValue accelX{0}; + SimDataValue accelY{0}; + SimDataValue accelZ{0}; + + SimDataValue yaw{0}; +}; +extern IMUData* SimIMUData; + +} // namespace wpi::hal diff --git a/hal/src/main/native/systemcore/mockdata/IMUData.cpp b/hal/src/main/native/systemcore/mockdata/IMUData.cpp new file mode 100644 index 0000000000..2c2a6cdbc1 --- /dev/null +++ b/hal/src/main/native/systemcore/mockdata/IMUData.cpp @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hal/simulation/SimDataValue.hpp" + +extern "C" { +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, IMU##CAPINAME, RETURN) + +DEFINE_CAPI(double, AngleX, 0) +DEFINE_CAPI(double, AngleY, 0) +DEFINE_CAPI(double, AngleZ, 0) + +DEFINE_CAPI(double, GyroRateX, 0) +DEFINE_CAPI(double, GyroRateY, 0) +DEFINE_CAPI(double, GyroRateZ, 0) + +DEFINE_CAPI(double, AccelX, 0) +DEFINE_CAPI(double, AccelY, 0) +DEFINE_CAPI(double, AccelZ, 0) + +DEFINE_CAPI(double, Yaw, 0) + +} // extern "C" diff --git a/wpilibc/robotpy_pybind_build_info.bzl b/wpilibc/robotpy_pybind_build_info.bzl index c21da75fbd..6a98cd3b28 100644 --- a/wpilibc/robotpy_pybind_build_info.bzl +++ b/wpilibc/robotpy_pybind_build_info.bzl @@ -1441,6 +1441,16 @@ def wpilib_simulation_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = ("wpi::sim::NiDsPS5ControllerSim", "wpi__sim__NiDsPS5ControllerSim.hpp"), ], ), + struct( + class_name = "OnboardIMUSim", + yml_file = "semiwrap/simulation/OnboardIMUSim.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/simulation/OnboardIMUSim.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::sim::OnboardIMUSim", "wpi__sim__OnboardIMUSim.hpp"), + ], + ), struct( class_name = "PWMSim", yml_file = "semiwrap/simulation/PWMSim.yml", diff --git a/wpilibc/src/main/native/cpp/hardware/imu/OnboardIMU.cpp b/wpilibc/src/main/native/cpp/hardware/imu/OnboardIMU.cpp index 4ee5173105..34451cb2dd 100644 --- a/wpilibc/src/main/native/cpp/hardware/imu/OnboardIMU.cpp +++ b/wpilibc/src/main/native/cpp/hardware/imu/OnboardIMU.cpp @@ -148,7 +148,7 @@ wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelY() { int32_t status = 0; HAL_GetIMUAcceleration(&val, &status); WPILIB_CheckErrorStatus(status, "Onboard IMU"); - return wpi::units::meters_per_second_squared_t{val.x}; + return wpi::units::meters_per_second_squared_t{val.y}; } wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelZ() { @@ -156,5 +156,5 @@ wpi::units::meters_per_second_squared_t OnboardIMU::GetAccelZ() { int32_t status = 0; HAL_GetIMUAcceleration(&val, &status); WPILIB_CheckErrorStatus(status, "Onboard IMU"); - return wpi::units::meters_per_second_squared_t{val.x}; + return wpi::units::meters_per_second_squared_t{val.z}; } diff --git a/wpilibc/src/main/native/cpp/simulation/OnboardIMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/OnboardIMUSim.cpp new file mode 100644 index 0000000000..1076353a39 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/OnboardIMUSim.cpp @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/simulation/OnboardIMUSim.hpp" + +#include "wpi/hal/simulation/IMUData.h" + +namespace wpi::sim { + +void OnboardIMUSim::SetAngleX(wpi::units::radian_t angle) { + HALSIM_SetIMUAngleX(angle.to()); +} +void OnboardIMUSim::SetAngleY(wpi::units::radian_t angle) { + HALSIM_SetIMUAngleY(angle.to()); +} +void OnboardIMUSim::SetAngleZ(wpi::units::radian_t angle) { + HALSIM_SetIMUAngleZ(angle.to()); +} + +void OnboardIMUSim::SetGyroRateX(wpi::units::radians_per_second_t rate) { + HALSIM_SetIMUGyroRateX(rate.to()); +} + +void OnboardIMUSim::SetGyroRateY(wpi::units::radians_per_second_t rate) { + HALSIM_SetIMUGyroRateY(rate.to()); +} + +void OnboardIMUSim::SetGyroRateZ(wpi::units::radians_per_second_t rate) { + HALSIM_SetIMUGyroRateZ(rate.to()); +} + +void OnboardIMUSim::SetAccelX(wpi::units::meters_per_second_squared_t accel) { + HALSIM_SetIMUAccelX(accel.to()); +} + +void OnboardIMUSim::SetAccelY(wpi::units::meters_per_second_squared_t accel) { + HALSIM_SetIMUAccelY(accel.to()); +} + +void OnboardIMUSim::SetAccelZ(wpi::units::meters_per_second_squared_t accel) { + HALSIM_SetIMUAccelZ(accel.to()); +} + +void OnboardIMUSim::SetYaw(wpi::units::radian_t angle) { + HALSIM_SetIMUYaw(angle.to()); +} + +} // namespace wpi::sim diff --git a/wpilibc/src/main/native/include/wpi/simulation/OnboardIMUSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/OnboardIMUSim.hpp new file mode 100644 index 0000000000..a17eafd2a6 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/simulation/OnboardIMUSim.hpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/units/acceleration.hpp" +#include "wpi/units/angle.hpp" +#include "wpi/units/angular_velocity.hpp" + +namespace wpi::sim { + +class OnboardIMUSim { + public: + void SetAngleX(wpi::units::radian_t angle); + void SetAngleY(wpi::units::radian_t angle); + void SetAngleZ(wpi::units::radian_t angle); + + void SetGyroRateX(wpi::units::radians_per_second_t rate); + void SetGyroRateY(wpi::units::radians_per_second_t rate); + void SetGyroRateZ(wpi::units::radians_per_second_t rate); + + void SetAccelX(wpi::units::meters_per_second_squared_t accel); + void SetAccelY(wpi::units::meters_per_second_squared_t accel); + void SetAccelZ(wpi::units::meters_per_second_squared_t accel); + + void SetYaw(wpi::units::radian_t angle); +}; + +} // namespace wpi::sim diff --git a/wpilibc/src/main/python/pyproject.toml b/wpilibc/src/main/python/pyproject.toml index 62baa9eb9f..ea95817ac3 100644 --- a/wpilibc/src/main/python/pyproject.toml +++ b/wpilibc/src/main/python/pyproject.toml @@ -276,6 +276,7 @@ JoystickSim = "wpi/simulation/JoystickSim.hpp" LinearSystemSim = "wpi/simulation/LinearSystemSim.hpp" NiDsPS4ControllerSim = "wpi/simulation/NiDsPS4ControllerSim.hpp" NiDsPS5ControllerSim = "wpi/simulation/NiDsPS5ControllerSim.hpp" +OnboardIMUSim = "wpi/simulation/OnboardIMUSim.hpp" PWMSim = "wpi/simulation/PWMSim.hpp" PneumaticsBaseSim = "wpi/simulation/PneumaticsBaseSim.hpp" PowerDistributionSim = "wpi/simulation/PowerDistributionSim.hpp" diff --git a/wpilibc/src/main/python/semiwrap/simulation/OnboardIMUSim.yml b/wpilibc/src/main/python/semiwrap/simulation/OnboardIMUSim.yml new file mode 100644 index 0000000000..f57592004a --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/simulation/OnboardIMUSim.yml @@ -0,0 +1,13 @@ +classes: + wpi::sim::OnboardIMUSim: + methods: + SetAngleX: + SetAngleY: + SetAngleZ: + SetGyroRateX: + SetGyroRateY: + SetGyroRateZ: + SetAccelX: + SetAccelY: + SetAccelZ: + SetYaw: diff --git a/wpilibc/src/main/python/wpilib/simulation/__init__.py b/wpilibc/src/main/python/wpilib/simulation/__init__.py index 22d290d21d..9341691a78 100644 --- a/wpilibc/src/main/python/wpilib/simulation/__init__.py +++ b/wpilibc/src/main/python/wpilib/simulation/__init__.py @@ -34,6 +34,7 @@ from ._simulation import ( NiDsPS5ControllerSim, NiDsStadiaControllerSim, NiDsXboxControllerSim, + OnboardIMUSim, OpModeOptions, PWMMotorControllerSim, PWMSim, @@ -92,6 +93,7 @@ __all__ = [ "NiDsPS5ControllerSim", "NiDsStadiaControllerSim", "NiDsXboxControllerSim", + "OnboardIMUSim", "OpModeOptions", "PWMMotorControllerSim", "PWMSim", diff --git a/wpilibc/src/test/native/cpp/OnboardIMUTest.cpp b/wpilibc/src/test/native/cpp/OnboardIMUTest.cpp new file mode 100644 index 0000000000..91fa0c70b1 --- /dev/null +++ b/wpilibc/src/test/native/cpp/OnboardIMUTest.cpp @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hardware/imu/OnboardIMU.hpp" + +#include + +#include "wpi/simulation/OnboardIMUSim.hpp" + +using namespace wpi; + +TEST(OnboardIMUTest, SimDevices) { + OnboardIMU imu{OnboardIMU::FLAT}; + + EXPECT_EQ(0.0, imu.GetAngleX().value()); + EXPECT_EQ(0.0, imu.GetAngleY().value()); + EXPECT_EQ(0.0, imu.GetAngleZ().value()); + + EXPECT_EQ(0.0, imu.GetGyroRateX().value()); + EXPECT_EQ(0.0, imu.GetGyroRateY().value()); + EXPECT_EQ(0.0, imu.GetGyroRateZ().value()); + + EXPECT_EQ(0.0, imu.GetAccelX().value()); + EXPECT_EQ(0.0, imu.GetAccelY().value()); + EXPECT_EQ(0.0, imu.GetAccelZ().value()); + + EXPECT_EQ(0.0, imu.GetYaw().value()); + + sim::OnboardIMUSim sim{}; + + sim.SetAngleX(wpi::units::radian_t{1}); + sim.SetAngleY(wpi::units::radian_t{2}); + sim.SetAngleZ(wpi::units::radian_t{3}); + + sim.SetGyroRateX(wpi::units::radians_per_second_t{3.504}); + sim.SetGyroRateY(wpi::units::radians_per_second_t{1.91}); + sim.SetGyroRateZ(wpi::units::radians_per_second_t{22.9}); + + sim.SetAccelX(wpi::units::meters_per_second_squared_t{-1}); + sim.SetAccelY(wpi::units::meters_per_second_squared_t{-2}); + sim.SetAccelZ(wpi::units::meters_per_second_squared_t{-3}); + + sim.SetYaw(wpi::units::radian_t{1.234}); + + EXPECT_EQ(1.0, imu.GetAngleX().value()); + EXPECT_EQ(2.0, imu.GetAngleY().value()); + EXPECT_EQ(3.0, imu.GetAngleZ().value()); + + EXPECT_EQ(3.504, imu.GetGyroRateX().value()); + EXPECT_EQ(1.91, imu.GetGyroRateY().value()); + EXPECT_EQ(22.9, imu.GetGyroRateZ().value()); + + EXPECT_EQ(-1.0, imu.GetAccelX().value()); + EXPECT_EQ(-2.0, imu.GetAccelY().value()); + EXPECT_EQ(-3.0, imu.GetAccelZ().value()); + + EXPECT_EQ(1.234, imu.GetYaw().value()); +} diff --git a/wpilibc/src/test/python/test_onboard_imu.py b/wpilibc/src/test/python/test_onboard_imu.py new file mode 100644 index 0000000000..382a738c2d --- /dev/null +++ b/wpilibc/src/test/python/test_onboard_imu.py @@ -0,0 +1,41 @@ +from wpilib import OnboardIMU +from wpilib.simulation import OnboardIMUSim + +def test_sim_device() -> None: + + imu = OnboardIMU(OnboardIMU.MountOrientation.FLAT) + sim = OnboardIMUSim() + + assert 0.0 == imu.getAngleX() + assert 0.0 == imu.getAngleY() + assert 0.0 == imu.getAngleZ() + assert 0.0 == imu.getGyroRateX() + assert 0.0 == imu.getGyroRateY() + assert 0.0 == imu.getGyroRateZ() + assert 0.0 == imu.getAccelX() + assert 0.0 == imu.getAccelY() + assert 0.0 == imu.getAccelZ() + + sim.setAngleX(1) + sim.setAngleY(2) + sim.setAngleZ(3) + + sim.setGyroRateX(3.504) + sim.setGyroRateY(1.91) + sim.setGyroRateZ(22.9) + + sim.setAccelX(-1) + sim.setAccelY(-2) + sim.setAccelZ(-3) + + assert 1.0 == imu.getAngleX() + assert 2.0 == imu.getAngleY() + assert 3.0 == imu.getAngleZ() + + assert 3.504 == imu.getGyroRateX() + assert 1.91 == imu.getGyroRateY() + assert 22.9 == imu.getGyroRateZ() + + assert -1.0 == imu.getAccelX() + assert -2.0 == imu.getAccelY() + assert -3.0 == imu.getAccelZ() \ No newline at end of file diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp index 8a48593c8f..db334c1703 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -132,8 +132,7 @@ void Drivetrain::SimulationPeriodic() { leftEncoderSim.SetRate(drivetrainSimulator.GetLeftVelocity().value()); rightEncoderSim.SetDistance(drivetrainSimulator.GetRightPosition().value()); rightEncoderSim.SetRate(drivetrainSimulator.GetRightVelocity().value()); - // gyroSim.SetAngle(-drivetrainSimulator.GetHeading().Degrees().value()); - // // TODO(Ryan): fixup when sim implemented + imuSim.SetYaw(-drivetrainSimulator.GetHeading().Degrees()); } void Drivetrain::Periodic() { diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp index 694415b61f..bbd9361be9 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp @@ -23,6 +23,7 @@ #include "wpi/nt/NetworkTableInstance.hpp" #include "wpi/simulation/DifferentialDrivetrainSim.hpp" #include "wpi/simulation/EncoderSim.hpp" +#include "wpi/simulation/OnboardIMUSim.hpp" #include "wpi/smartdashboard/Field2d.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/angular_velocity.hpp" @@ -166,6 +167,7 @@ class Drivetrain { // Simulation classes wpi::sim::EncoderSim leftEncoderSim{leftEncoder}; wpi::sim::EncoderSim rightEncoderSim{rightEncoder}; + wpi::sim::OnboardIMUSim imuSim; wpi::Field2d fieldSim; wpi::Field2d fieldApproximation; wpi::math::LinearSystem<2, 2, 2> drivetrainSystem = diff --git a/wpilibj/src/main/java/org/wpilib/simulation/OnboardIMUSim.java b/wpilibj/src/main/java/org/wpilib/simulation/OnboardIMUSim.java new file mode 100644 index 0000000000..0786c6ee1f --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/simulation/OnboardIMUSim.java @@ -0,0 +1,104 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.simulation; + +import org.wpilib.hardware.hal.simulation.IMUDataJNI; + +/** Class to control a simulated onboard IMU. */ +public final class OnboardIMUSim { + private OnboardIMUSim() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Change the x-angle. + * + * @param angleRad the new angle + */ + public static void setAngleX(double angleRad) { + IMUDataJNI.setAngleX(angleRad); + } + + /** + * Change the y-angle. + * + * @param angleRad the new angle + */ + public static void setAngleY(double angleRad) { + IMUDataJNI.setAngleY(angleRad); + } + + /** + * Change the z-angle. + * + * @param angleRad the new angle + */ + public static void setAngleZ(double angleRad) { + IMUDataJNI.setAngleZ(angleRad); + } + + /** + * Change the x-gyro rate. + * + * @param rateRadPerSec the new rate + */ + public static void setGyroRateX(double rateRadPerSec) { + IMUDataJNI.setGyroRateX(rateRadPerSec); + } + + /** + * Change the y-gyro rate. + * + * @param rateRadPerSec the new rate + */ + public static void setGyroRateY(double rateRadPerSec) { + IMUDataJNI.setGyroRateY(rateRadPerSec); + } + + /** + * Change the z-gyro rate. + * + * @param rateRadPerSec the new rate + */ + public static void setGyroRateZ(double rateRadPerSec) { + IMUDataJNI.setGyroRateZ(rateRadPerSec); + } + + /** + * Change the x-accel rate. + * + * @param accelMpss the new acceleration + */ + public static void setAccelX(double accelMpss) { + IMUDataJNI.setAccelX(accelMpss); + } + + /** + * Change the y-accel rate. + * + * @param accelMpss the new acceleration + */ + public static void setAccelY(double accelMpss) { + IMUDataJNI.setAccelY(accelMpss); + } + + /** + * Change the z-accel rate. + * + * @param accelMpss the new acceleration + */ + public static void setAccelZ(double accelMpss) { + IMUDataJNI.setAccelZ(accelMpss); + } + + /** + * Change the yaw. + * + * @param angleRad the new yaw angle + */ + public static void setYaw(double angleRad) { + IMUDataJNI.setYaw(angleRad); + } +} diff --git a/wpilibj/src/test/java/org/wpilib/hardware/imu/OnboardIMUTest.java b/wpilibj/src/test/java/org/wpilib/hardware/imu/OnboardIMUTest.java new file mode 100644 index 0000000000..73bb49eae9 --- /dev/null +++ b/wpilibj/src/test/java/org/wpilib/hardware/imu/OnboardIMUTest.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.imu; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.wpilib.simulation.OnboardIMUSim; + +class OnboardIMUTest { + @Test + void testOnboardIMU() { + OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT); + + assertEquals(0.0, imu.getAngleX()); + assertEquals(0.0, imu.getAngleY()); + assertEquals(0.0, imu.getAngleZ()); + + assertEquals(0.0, imu.getGyroRateX()); + assertEquals(0.0, imu.getGyroRateY()); + assertEquals(0.0, imu.getGyroRateZ()); + + assertEquals(0.0, imu.getAccelX()); + assertEquals(0.0, imu.getAccelY()); + assertEquals(0.0, imu.getAccelZ()); + + assertEquals(0.0, imu.getYawRadians()); + + OnboardIMUSim.setAngleX(1); + OnboardIMUSim.setAngleY(2); + OnboardIMUSim.setAngleZ(3); + + OnboardIMUSim.setGyroRateX(3.504); + OnboardIMUSim.setGyroRateY(1.91); + OnboardIMUSim.setGyroRateZ(22.9); + + OnboardIMUSim.setAccelX(-1); + OnboardIMUSim.setAccelY(-2); + OnboardIMUSim.setAccelZ(-3); + + OnboardIMUSim.setYaw(1.234); + + assertEquals(1.0, imu.getAngleX()); + assertEquals(2.0, imu.getAngleY()); + assertEquals(3.0, imu.getAngleZ()); + + assertEquals(3.504, imu.getGyroRateX()); + assertEquals(1.91, imu.getGyroRateY()); + assertEquals(22.9, imu.getGyroRateZ()); + + assertEquals(-1.0, imu.getAccelX()); + assertEquals(-2.0, imu.getAccelY()); + assertEquals(-3.0, imu.getAccelZ()); + + assertEquals(1.234, imu.getYawRadians()); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java index 7e138975e4..96cb7a74a1 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java @@ -30,6 +30,7 @@ import org.wpilib.networktables.DoubleArrayEntry; import org.wpilib.networktables.DoubleArrayTopic; import org.wpilib.simulation.DifferentialDrivetrainSim; import org.wpilib.simulation.EncoderSim; +import org.wpilib.simulation.OnboardIMUSim; import org.wpilib.smartdashboard.Field2d; import org.wpilib.smartdashboard.SmartDashboard; import org.wpilib.system.RobotController; @@ -253,8 +254,7 @@ public class Drivetrain { leftEncoderSim.setRate(drivetrainSimulator.getLeftVelocity()); rightEncoderSim.setDistance(drivetrainSimulator.getRightPosition()); rightEncoderSim.setRate(drivetrainSimulator.getRightVelocity()); - // gyroSim.setAngle(-drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup - // when sim implemented + OnboardIMUSim.setYaw(-drivetrainSimulator.getHeading().getRadians()); } /** This function is called periodically, no matter the mode. */