From baa4c90145aba88dce353d575c39aef58f7d317d Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 21 May 2026 20:01:19 -0700 Subject: [PATCH 01/38] intiilaize imu.h --- src/software/embedded/services/imu.cpp | 51 +++++++++++++++++++++++++ src/software/embedded/services/imu.h | 52 ++++++++++++++++++++++++++ 2 files changed, 103 insertions(+) create mode 100644 src/software/embedded/services/imu.cpp create mode 100644 src/software/embedded/services/imu.h diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp new file mode 100644 index 0000000000..a24a8c6f16 --- /dev/null +++ b/src/software/embedded/services/imu.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include + +#include "software/logger/logger.h" + +// these functions taken from +// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c +__s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, + union i2c_smbus_data* data) +{ + struct i2c_smbus_ioctl_data args; + __s32 err; + + args.read_write = read_write; + args.command = command; + args.size = size; + args.data = data; + + err = ioctl(file, I2C_SMBUS, &args); + if (err == -1) + err = -errno; + return err; +} + +__s32 i2c_smbus_read_byte_data(int file, __u8 command) +{ + union i2c_smbus_data data; + int err; + + err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data); + if (err < 0) + return err; + + return 0x0FF & data.byte; +} + +__s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) +{ + union i2c_smbus_data data; + data.byte = value; + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); +} + + +ImuService:ImuService(): initialized_(false) +{ + + +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h new file mode 100644 index 0000000000..8f2a5576b7 --- /dev/null +++ b/src/software/embedded/services/imu.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include + +#include "proto/tbots_software_msgs.pb.h" +#include "software/geom/angular_velocity.h" + +/** + * Handles low level IMU I2C communication, and some minor offset filtering. + */ +class ImuService +{ + public: + ImuService(); + /* + * Polls the latest IMU reading of the angular velocity of the robot on the z axis + * @return the current angular velocty of the robot on the z axis + */ + std::optional pollHeadingVelocity(); + /* + * Polls the latest IMU reading of the linear acceleration of the robot on the z plane + * @return the current linear acceleration of the robot on the z plane + */ + std::optional pollLinearAcceleration(); + + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) * + (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); + + private: + bool initialized_=true; + int file_descriptor_=0; + doubel degrees_error_; + // Maps the maximum raw reading from 16-bit integer to be 2 times gravity + static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; + // Same for gyroscope, 1000 degrees per second + static constexpr double IMU_FULL_SCALE_DPS=1000.0; + + /// Various i2c registers. + static const uint8_t WHOAMI_REG = 0xf; + static const uint8_t ACCEL_CONTROL_REG = 0x10; + static const uint8_t GYRO_CONTROL_REG = 0x11; + static const uint8_t CTRL4_C = 0x13; + static const uint8_t CTRL6_C = 0x15; + static const uint8_t CTRL8_XL = 0x17; + static const uint8_t YAW_LEAST_SIG_REG = 0x26; + static const uint8_t YAW_MOST_SIG_REG = 0x27; +} From 7aaec7e17ec9d2d7c82b63f3eafd43d2fe4566e1 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 21 May 2026 20:04:32 -0700 Subject: [PATCH 02/38] finish imu.h --- src/software/embedded/services/imu.h | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 8f2a5576b7..5948452ea7 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -47,6 +47,17 @@ class ImuService static const uint8_t CTRL4_C = 0x13; static const uint8_t CTRL6_C = 0x15; static const uint8_t CTRL8_XL = 0x17; - static const uint8_t YAW_LEAST_SIG_REG = 0x26; - static const uint8_t YAW_MOST_SIG_REG = 0x27; + // Device path for the IMU + inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + // Gyroscope Z-axis (Yaw) Output Data Registers + static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t YAW_MOST_SIG_REG = 0x27; // OUTZ_H_G + + // Accelerometer X-axis Output Data Registers + static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL + static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL + + // Accelerometer Y-axis Output Data Registers + static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL + static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL } From 6d68e744f3753a07c2d5458dcd0534eb56f50742 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 21 May 2026 20:17:59 -0700 Subject: [PATCH 03/38] init imu.cpp --- src/software/embedded/services/imu.cpp | 105 ++++++++++++++++++++++++- src/software/embedded/services/imu.h | 5 +- 2 files changed, 108 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index a24a8c6f16..c68e09b045 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -2,7 +2,9 @@ #include #include #include +#include +#include "imu.h" #include "software/logger/logger.h" // these functions taken from @@ -44,8 +46,109 @@ __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) } -ImuService:ImuService(): initialized_(false) +ImuService::ImuService() : initialized_(false) { + // Establish connection to the IMU and verify that the who am I pin is correct. + file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR); + int ret = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b); + if (ret < 0) + { + LOG(WARNING) + << "Failed to initialize the IMU: failed to establish i2c connection."; + return; + } + if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106) + { + LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register " + << static_cast(WHOAMI_REG) << " read incorrectly."; + return; + } + // Attempt to enable gyro and accelerometer, checking that writes are successful + // See lsm6dsl datasheet for how to set these registers. + i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000); + if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to accelerometer config register " + << static_cast(ACCEL_CONTROL_REG) << " unsuccessful"; + return; + } + // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of + // datasheet for lsm6dsl, pg 21) + i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000); + if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to gyroscope config register " + << "unsuccessful"; + return; + } + // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000) + // CTRL4_C: bit 1 (LPF1_SEL_G) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010); + // CTRL6_C: bits 2:0 (FTYPE) = 000 + i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000); + // Enable Accelerometer digital LPF2 + // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000); + + initialized_ = true; + LOG(INFO) << "Initialized IMU! Calibrating..."; + degrees_error_ = 0; + // get 100 sample average of stationary reading, so all future readings can be + // corrected + double sum = 0; + int valid_samples = 0; + for (int i = 0; i < 100; i++) + { + auto poll = pollHeadingRate(); + if (poll.has_value()) + { + sum += poll->toDegrees(); + valid_samples++; + } + usleep(50000); + } + + if (valid_samples > 0) + { + degrees_error_ = sum / valid_samples; + LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps"; + } + else + { + LOG(WARNING) << "IMU Calibration failed: no valid samples received. Heading " + "stability will be poor."; + initialized_ = false; + } +} +std::optional ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg) +{ + int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); + int most_significant = i2c_smbus_read_byte_data(file_descriptor_, ms_reg); + + if (least_significant < 0 || most_significant < 0) + { + return std::nullopt; + } + + return (static_cast(most_significant) << 8) | + static_cast(least_significant); + +} + +std::optional ImuService::pollHeadingVelocity() +{ + if (!initialized_) + { + return std::nullopt; + } + + // Two separate registers for the Gyro output data. + int16_t full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG); + double degrees_per_sec = static_cast(full_word) / + static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); } diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 5948452ea7..0652bca6ed 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -32,6 +32,7 @@ class ImuService (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); private: + std::optional combineBits(); bool initialized_=true; int file_descriptor_=0; doubel degrees_error_; @@ -47,8 +48,10 @@ class ImuService static const uint8_t CTRL4_C = 0x13; static const uint8_t CTRL6_C = 0x15; static const uint8_t CTRL8_XL = 0x17; - // Device path for the IMU + + // Device path for the IMU inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + // Gyroscope Z-axis (Yaw) Output Data Registers static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G static constexpr uint8_t YAW_MOST_SIG_REG = 0x27; // OUTZ_H_G From f466385fc678db615d6c1a9afe0ea50861f5fc54 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 21 May 2026 20:35:57 -0700 Subject: [PATCH 04/38] finish imu.cpp --- src/software/embedded/services/imu.cpp | 52 +++++++++++++++++++++++--- src/software/embedded/services/imu.h | 1 + 2 files changed, 47 insertions(+), 6 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index c68e09b045..a48ec1e0b6 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -2,9 +2,10 @@ #include #include #include -#include +#include // For SHRT_MAX #include "imu.h" +#include "shared/constants.h" #include "software/logger/logger.h" // these functions taken from @@ -103,7 +104,8 @@ ImuService::ImuService() : initialized_(false) int valid_samples = 0; for (int i = 0; i < 100; i++) { - auto poll = pollHeadingRate(); + // Fixed: Call the actual implemented function name + auto poll = pollHeadingVelocity(); if (poll.has_value()) { sum += poll->toDegrees(); @@ -124,6 +126,7 @@ ImuService::ImuService() : initialized_(false) initialized_ = false; } } + std::optional ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg) { int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); @@ -134,9 +137,10 @@ std::optional ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg) return std::nullopt; } - return (static_cast(most_significant) << 8) | + uint16_t combined = (static_cast(most_significant) << 8) | static_cast(least_significant); - + + return static_cast(combined); } std::optional ImuService::pollHeadingVelocity() @@ -146,9 +150,45 @@ std::optional ImuService::pollHeadingVelocity() return std::nullopt; } - // Two separate registers for the Gyro output data. - int16_t full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG); + std::optional opt_full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG); + + if (!opt_full_word.has_value()) + { + return std::nullopt; + } + + int16_t full_word = opt_full_word.value(); + double degrees_per_sec = static_cast(full_word) / static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); } + +std::optional ImuService::pollLinearAcceleration() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_x = combineBits(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional opt_y = combineBits(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); + + if (!opt_x.has_value() || !opt_y.has_value()) + { + return std::nullopt; + } + + int16_t raw_x = opt_x.value(); + int16_t raw_y = opt_y.value(); + + double a_x = (static_cast(raw_x) / SHRT_MAX) + * ACCELEROMETER_FULL_SCALE_G + * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + double a_y = (static_cast(raw_y) / SHRT_MAX) + * ACCELEROMETER_FULL_SCALE_G + * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + return Eigen::Vector2d(a_x, a_y); +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 0652bca6ed..e74b3a4da6 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -3,6 +3,7 @@ #include #include #include +#include #include "proto/tbots_software_msgs.pb.h" #include "software/geom/angular_velocity.h" From 4199e76903e45ce8f7d726452fb817c6557cd70c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 21 May 2026 20:56:54 -0700 Subject: [PATCH 05/38] update imu cpp and add thunderloop test --- src/software/embedded/services/imu.cpp | 2 +- src/software/embedded/services/imu.h | 4 ++-- src/software/embedded/thunderloop.cpp | 13 +++++++++++-- src/software/embedded/thunderloop.h | 1 + 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index a48ec1e0b6..b4dc6c7af6 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -150,7 +150,7 @@ std::optional ImuService::pollHeadingVelocity() return std::nullopt; } - std::optional opt_full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG); + std::optional opt_full_word = combineBits(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG); if (!opt_full_word.has_value()) { diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index e74b3a4da6..d234a05335 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -54,8 +54,8 @@ class ImuService inline static const std::string IMU_DEVICE = "/dev/i2c-1"; // Gyroscope Z-axis (Yaw) Output Data Registers - static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G - static constexpr uint8_t YAW_MOST_SIG_REG = 0x27; // OUTZ_H_G + static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t HEADING_MOST_SIG_REG = 0x27; // OUTZ_H_G // Accelerometer X-axis Output Data Registers static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b8a440a683..02a08c090d 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -10,6 +10,7 @@ #include "shared/constants.h" #include "software/embedded/primitive_executor.h" #include "software/embedded/services/motor.h" +#include "software/embedded/services/imu.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" @@ -120,8 +121,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, motor_service_ = std::make_unique(robot_constants, loop_hz); g_motor_service = motor_service_.get(); - motor_service_->setup(); - LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; + mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; + + imu_service_ = std::make_unique(); + LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";otor_service_->setup(); LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ << ", CHANNEL ID: " << channel_id_ @@ -177,6 +180,12 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); + + std::optional imu_poll = imu_service_->pollHeadingVelocity(); + if (imu_poll.has_value()){ + + LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ; + } for (;;) { struct timespec time_since_prev_iter; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index cf70a12b68..7a524a2cbc 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -13,6 +13,7 @@ #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" +#include "software/embedded/services/imu.h" #include "software/embedded/toml_config/toml_config_client.h" #include "software/logger/logger.h" From 48ccb08f59da77cc2a6e0c46dbcd6b795bae9273 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 13:37:26 -0700 Subject: [PATCH 06/38] Refactor motor service and implement STSPIN motor controller --- src/proto/message_translation/tbots_protobuf_test.cpp | 3 +-- src/proto/primitive/primitive_msg_factory_test.cpp | 3 +-- src/shared/robot_constants.cpp | 0 src/software/ai/evaluation/time_to_travel_test.cpp | 3 +-- src/software/ai/hl/stp/tactic/primitive_test.cpp | 4 ++-- .../embedded/motor_controller/stspin_motor_controller.cpp | 2 ++ src/software/simulation/er_force_simulator_test.cpp | 4 ++-- 7 files changed, 9 insertions(+), 10 deletions(-) create mode 100644 src/shared/robot_constants.cpp diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..500c6611df 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -153,8 +153,7 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) // with the same parameters as the trajectory, finally, generate a second trajectory // from the parameters and make sure the two trajectories are equal. robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants(); - Point start_position(0.0, 0.0); + robot_constants::createRobotConstants(); Point start_position(0.0, 0.0); Point destination(0.0, 0.0); std::vector sub_destinations = std::get<0>(GetParam()); diff --git a/src/proto/primitive/primitive_msg_factory_test.cpp b/src/proto/primitive/primitive_msg_factory_test.cpp index 962258095e..b54c8f4d96 100644 --- a/src/proto/primitive/primitive_msg_factory_test.cpp +++ b/src/proto/primitive/primitive_msg_factory_test.cpp @@ -10,8 +10,7 @@ class PrimitiveFactoryTest : public testing::Test { protected: robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants(); -}; + robot_constants::createRobotConstants();}; TEST_F(PrimitiveFactoryTest, test_auto_chip_or_kick_equality) { diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/software/ai/evaluation/time_to_travel_test.cpp b/src/software/ai/evaluation/time_to_travel_test.cpp index 9daaceef3d..b46bfabc29 100644 --- a/src/software/ai/evaluation/time_to_travel_test.cpp +++ b/src/software/ai/evaluation/time_to_travel_test.cpp @@ -8,8 +8,7 @@ class TimeToTravel : public ::testing::Test { protected: robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants(); -}; + robot_constants::createRobotConstants();}; TEST_F(TimeToTravel, getTimeToTravelDistance_already_at_dest) { diff --git a/src/software/ai/hl/stp/tactic/primitive_test.cpp b/src/software/ai/hl/stp/tactic/primitive_test.cpp index 5ff973063c..8988855fb3 100644 --- a/src/software/ai/hl/stp/tactic/primitive_test.cpp +++ b/src/software/ai/hl/stp/tactic/primitive_test.cpp @@ -18,8 +18,8 @@ class PrimitiveTest : public testing::Test protected: robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); - std::shared_ptr world = TestUtil::createBlankTestingWorld(); + Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); + std::shared_ptr world = TestUtil::createBlankTestingWorld(); RobotNavigationObstacleFactory obstacle_factory = RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig()); }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 7f7164c407..1c57ec5f9b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -182,6 +182,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, sendAndReceiveFrame(motor, outgoing_frame); + sendMotorStatusToPlotJuggler(motor); + return motor_status_.at(motor).speed; } diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index 0a5713f1f6..922c5a6124 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -308,7 +308,7 @@ TEST(ErForceSimulatorFieldTest, check_field_A_configuration) { robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_A, robot_constants, realism_config); simulator->resetCurrentTime(); @@ -321,7 +321,7 @@ TEST(ErForceSimulatorFieldTest, check_field_B_configuration) { robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_B, robot_constants, realism_config); simulator->resetCurrentTime(); From 448900b4cbf9731f013cd26e85a89bf957012376 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 13:38:16 -0700 Subject: [PATCH 07/38] Run lint and format --- src/proto/message_translation/tbots_protobuf_test.cpp | 3 ++- src/proto/primitive/primitive_msg_factory_test.cpp | 3 ++- src/software/ai/evaluation/time_to_travel_test.cpp | 3 ++- src/software/ai/hl/stp/tactic/primitive_test.cpp | 4 ++-- src/software/simulation/er_force_simulator_test.cpp | 4 ++-- 5 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 500c6611df..0f82de69d6 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -153,7 +153,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) // with the same parameters as the trajectory, finally, generate a second trajectory // from the parameters and make sure the two trajectories are equal. robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants(); Point start_position(0.0, 0.0); + robot_constants::createRobotConstants(); + Point start_position(0.0, 0.0); Point destination(0.0, 0.0); std::vector sub_destinations = std::get<0>(GetParam()); diff --git a/src/proto/primitive/primitive_msg_factory_test.cpp b/src/proto/primitive/primitive_msg_factory_test.cpp index b54c8f4d96..962258095e 100644 --- a/src/proto/primitive/primitive_msg_factory_test.cpp +++ b/src/proto/primitive/primitive_msg_factory_test.cpp @@ -10,7 +10,8 @@ class PrimitiveFactoryTest : public testing::Test { protected: robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants();}; + robot_constants::createRobotConstants(); +}; TEST_F(PrimitiveFactoryTest, test_auto_chip_or_kick_equality) { diff --git a/src/software/ai/evaluation/time_to_travel_test.cpp b/src/software/ai/evaluation/time_to_travel_test.cpp index b46bfabc29..9daaceef3d 100644 --- a/src/software/ai/evaluation/time_to_travel_test.cpp +++ b/src/software/ai/evaluation/time_to_travel_test.cpp @@ -8,7 +8,8 @@ class TimeToTravel : public ::testing::Test { protected: robot_constants::RobotConstants robot_constants = - robot_constants::createRobotConstants();}; + robot_constants::createRobotConstants(); +}; TEST_F(TimeToTravel, getTimeToTravelDistance_already_at_dest) { diff --git a/src/software/ai/hl/stp/tactic/primitive_test.cpp b/src/software/ai/hl/stp/tactic/primitive_test.cpp index 8988855fb3..5ff973063c 100644 --- a/src/software/ai/hl/stp/tactic/primitive_test.cpp +++ b/src/software/ai/hl/stp/tactic/primitive_test.cpp @@ -18,8 +18,8 @@ class PrimitiveTest : public testing::Test protected: robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); - std::shared_ptr world = TestUtil::createBlankTestingWorld(); + Robot robot = TestUtil::createRobotAtPos(Point(0, 0)); + std::shared_ptr world = TestUtil::createBlankTestingWorld(); RobotNavigationObstacleFactory obstacle_factory = RobotNavigationObstacleFactory(TbotsProto::RobotNavigationObstacleConfig()); }; diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index 922c5a6124..0a5713f1f6 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -308,7 +308,7 @@ TEST(ErForceSimulatorFieldTest, check_field_A_configuration) { robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_A, robot_constants, realism_config); simulator->resetCurrentTime(); @@ -321,7 +321,7 @@ TEST(ErForceSimulatorFieldTest, check_field_B_configuration) { robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); std::shared_ptr simulator = std::make_shared( TbotsProto::FieldType::DIV_B, robot_constants, realism_config); simulator->resetCurrentTime(); From 69cbf9e408720255d756a47e77fe37a0952a1586 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 14:15:45 -0700 Subject: [PATCH 08/38] Improved robot localization and trajectory following --- .../message_translation/tbots_protobuf.cpp | 16 +- .../message_translation/tbots_protobuf.h | 11 +- .../tbots_protobuf_test.cpp | 4 +- src/shared/constants.h | 2 + src/shared/robot_constants.h | 56 ++- src/software/ai/evaluation/intercept.cpp | 2 +- .../ai/hl/stp/tactic/move_primitive.cpp | 2 +- src/software/embedded/BUILD | 22 ++ src/software/embedded/primitive_executor.cpp | 293 ++++++++++++--- src/software/embedded/primitive_executor.h | 82 +++-- src/software/embedded/robot_localizer.cpp | 336 ++++++++++++++++++ src/software/embedded/robot_localizer.h | 172 +++++++++ src/software/embedded/services/BUILD | 12 + src/software/embedded/services/imu.cpp | 149 ++++++++ src/software/embedded/services/imu.h | 60 ++++ src/software/embedded/thunderloop.cpp | 68 +++- src/software/embedded/thunderloop.h | 9 + src/software/physics/euclidean_to_wheel.cpp | 65 ++++ src/software/physics/euclidean_to_wheel.h | 2 + .../physics/euclidean_to_wheel_test.cpp | 48 +++ src/software/sensor_fusion/sensor_fusion.cpp | 2 + .../simulated_tests/requirements_lock.txt | 24 +- .../simulation/er_force_simulator.cpp | 65 ++-- src/software/simulation/er_force_simulator.h | 13 +- .../thunderscope/widget_setup_functions.py | 2 +- .../wifi_communication_manager.py | 8 +- src/software/util/pid/BUILD | 8 + src/software/util/pid/pid_controller.hpp | 66 ++++ src/software/world/robot.cpp | 3 +- 29 files changed, 1438 insertions(+), 164 deletions(-) create mode 100644 src/software/embedded/robot_localizer.cpp create mode 100644 src/software/embedded/robot_localizer.h create mode 100644 src/software/embedded/services/imu.cpp create mode 100644 src/software/embedded/services/imu.h create mode 100644 src/software/util/pid/BUILD create mode 100644 src/software/util/pid/pid_controller.hpp diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..23e7d0c51c 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -425,7 +425,8 @@ std::unique_ptr createCostVisualization( } std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( @@ -449,8 +450,7 @@ std::optional createTrajectoryPathFromParams( } auto trajectory = std::make_shared( - createPoint(params.start_position()), initial_destination, initial_velocity, - constraints); + start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); @@ -475,14 +475,14 @@ std::optional createTrajectoryPathFromParams( } BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( - createAngle(params.start_angle()), createAngle(params.final_angle()), - initial_velocity, - AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s), + start_angle, createAngle(params.final_angle()), initial_velocity, + AngularVelocity::fromRadians( + robot_constants.robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_m_per_s; + return robot_constants.robot_max_speed_trajectory_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 5976da5fcc..2831857e02 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -238,25 +238,28 @@ std::unique_ptr createCostVisualization( * Generate a 2D Trajectory Path given 2D trajectory parameters * * @param params 2D Trajectory Path + * @param start_position Initial position to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created * from the given parameters */ std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** - * Generate an angular trajectory Path given angular trajectory proto parameters + * Generate an angular trajectory path given angular trajectory proto parameters * - * @param params angular Trajectory Path + * @param params Angular trajectory path + * @param start_angle Initial angle to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return Generate angular trajectory */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..2735d57a3b 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = - createTrajectoryPathFromParams(params, initial_velocity, robot_constants); + auto converted_trajectory_path_opt = createTrajectoryPathFromParams( + params, createPoint(params.start_position()), initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/shared/constants.h b/src/shared/constants.h index ed94c0cde9..b0f3dbaa95 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -224,6 +224,8 @@ static const unsigned THUNDERLOOP_HZ = 300u; static const unsigned NUM_GENEVA_ANGLES = 5; +static constexpr double RTT_S = 0.03; + // Robot diagnostics constants constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M = 1.5; constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S = 1.5; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 93ceb390cd..23ba552ef6 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -60,6 +60,10 @@ struct RobotConstants // The maximum speed achievable by our robots, in metres per second [m/s] float robot_max_speed_m_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] + float robot_max_speed_trajectory_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -69,11 +73,23 @@ struct RobotConstants // The maximum angular speed achievable by our robots [rad/s] float robot_max_ang_speed_rad_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. + // [rad/s] + float robot_max_ang_speed_trajectory_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; // The radius of the wheel, in meters float wheel_radius_meters; + + // Various variances for the robot localizer + float kalman_process_noise_variance_rad_per_s_4; + + float kalman_vision_noise_variance_rad_2; + + float kalman_motor_sensor_noise_variance_rad_per_s_2; }; /** @@ -87,7 +103,7 @@ constexpr RobotConstants createRobotConstants() return { .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), .front_wheel_angle_deg = 32.0f, - .back_wheel_angle_deg = 44.0f, + .back_wheel_angle_deg = 46.0f, .front_of_robot_width_meters = 0.11f, .dribbler_width_meters = 0.07825f, @@ -100,15 +116,22 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 2.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_max_deceleration_m_per_s_2 = 1.5f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 4.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 1.0f, + .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() @@ -129,15 +152,22 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.000f, + .robot_max_speed_trajectory_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 0.5f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f}; } #endif diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 457ffb0639..2c729dd516 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_m_per_s; + double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 7e748c2068..325da493a3 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -40,7 +40,7 @@ MovePrimitive::MovePrimitive( angular_trajectory.generate( robot.orientation(), final_angle, robot.angularVelocity(), AngularVelocity::fromRadians( - robot.robotConstants().robot_max_ang_speed_rad_per_s), + robot.robotConstants().robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 5ee03cb54c..d58f3ae945 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -37,6 +37,7 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", + "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], @@ -58,11 +59,14 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded:robot_localizer", + "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", "//software/embedded/toml_config", "//software/logger:network_logger", + "//software/physics:velocity_conversion_util", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", "@tracy", @@ -100,3 +104,21 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_library( + name = "robot_localizer", + srcs = ["robot_localizer.cpp"], + hdrs = ["robot_localizer.h"], + deps = [ + "//proto:tbots_cc_proto", + "//software:constants", + "//software/embedded/services:imu", + "//software/geom:angle", + "//software/geom:angular_velocity", + "//software/geom:point", + "//software/geom:vector", + "//software/sensor_fusion/filter:kalman_filter", + "//software/util/scoped_timespec_timer", + "@eigen", + ], +) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..3d19b7664d 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -21,66 +21,169 @@ PrimitiveExecutor::PrimitiveExecutor( { } +bool PrimitiveExecutor::isLateralTrajectoryNew( + const std::optional& new_trajectory) const +{ + if (new_trajectory.has_value() != trajectory_path_.has_value()) + { + // Either we don't have a trajectory right now, and this one does, or we have one + // and this trajectory doesn't. + return true; + } + + if (!new_trajectory.has_value()) + { + return false; + } + + return distance(new_trajectory->getDestination(), + trajectory_path_->getDestination()) > + LATERAL_DESTINATION_THRESHOLD_METERS; +} + +bool PrimitiveExecutor::isAngularTrajectoryNew( + const BangBangTrajectory1DAngular& new_trajectory) const +{ + if (!angular_trajectory_.has_value()) + { + return true; + } + + return new_trajectory.getDestination() + .minDiff(angular_trajectory_->getDestination()) + .toDegrees() > ANGULAR_DESTINATION_THRESHOLD_DEGREES; +} + void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_msg) { current_primitive_ = primitive_msg; if (current_primitive_.has_move()) { - trajectory_path_ = createTrajectoryPathFromParams( - current_primitive_.move().xy_traj_params(), velocity_, robot_constants_); + const std::optional new_trajectory_path = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); + // Check if this new trajectory is "new". That is, if its destination differs + // meaningfully from our current trajectory. + if (isLateralTrajectoryNew(new_trajectory_path)) + { + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = Duration::fromSeconds(RTT_S); + x_pid.reset(); + y_pid.reset(); + LOG(INFO) << "new lateral trajectory accepted"; + } - angular_trajectory_ = + const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); - - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + orientation_, angular_velocity_, + robot_constants_); + LOG(INFO) << "angular has val " << angular_trajectory_.has_value(); + if (isAngularTrajectoryNew(new_angular_trajectory)) + { + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = Duration::fromSeconds(RTT_S); + w_pid.reset(); + LOG(INFO) << "new angular trajectory accepted"; + } } } -void PrimitiveExecutor::setStopPrimitive() +void PrimitiveExecutor::updateState(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) { - current_primitive_ = *createStopPrimitiveProto(); -} + position_ = position; + velocity_ = velocity; + orientation_ = orientation; + angular_velocity_ = angular_velocity; -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) -{ - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; + // If we are lagging behind trajectory too much, we have stalled! We need to + // regenerate trajectory. + // TODO: Add a timeout to following error + if (trajectory_path_.has_value()) + { + const double lateral_following_error = + (position_ - trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.toSeconds())) + .length(); + if (lateral_following_error > LATERAL_STALL_ERROR_MAX_METERS) + { + // regenerate lateral trajectory + trajectory_path_ = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); + } + } + if (angular_trajectory_.has_value()) + { + const double angular_following_error = + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) + .minDiff(orientation_) + .toDegrees(); + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"following_error_deg", angular_following_error}, + }); + if (angular_following_error > ANGULAR_STALL_ERROR_MAX_DEGREES) + { + // regenerate angular trajectory + angular_trajectory_ = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), orientation_, + AngularVelocity::fromRadians(angular_velocity_.toRadians()), + robot_constants_); + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); + } + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"following_error_after_deg", + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) + .minDiff(orientation_) + .toDegrees()}, + {"angular_pos_after_deg", + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) + .toDegrees()}, + }); + } } Vector PrimitiveExecutor::getTargetLinearVelocity() { - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - orientation_); - Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(position, trajectory_path_->getDestination()); + Vector target_velocity = + trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.toSeconds()); + + const Point expected_position = + trajectory_path_->getPosition(time_since_linear_trajectory_creation_.toSeconds()); + const double distance_to_destination = + distance(expected_position, trajectory_path_->getDestination()); // Dampen velocity as we get closer to the destination to reduce jittering - if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M) + if (distance_to_destination < MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M) { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; + target_velocity *= + distance_to_destination / MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M; } - return local_velocity; + + return target_velocity; } AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() { - orientation_ = - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()); - - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - orientation_.minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + AngularVelocity angular_velocity = angular_trajectory_->getVelocity( + time_since_angular_trajectory_creation_.toSeconds()); + + const Angle expected_orientation = angular_trajectory_->getPosition( + time_since_angular_trajectory_creation_.toSeconds()); + const double distance_to_destination = + expected_orientation.minDiff(angular_trajectory_->getDestination()).toDegrees(); + + // Dampen velocity as we get closer to the destination to reduce jittering + if (distance_to_destination < MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; + angular_velocity *= + distance_to_destination / MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES; } return angular_velocity; @@ -90,19 +193,19 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - time_since_trajectory_creation_ += time_step_; + time_since_angular_trajectory_creation_ += time_step_; + time_since_linear_trajectory_creation_ += time_step_; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) { case TbotsProto::Primitive::kStop: { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( - prim->direct_control()); + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); status.set_running_primitive(false); - return output; + return std::make_unique( + prim->direct_control()); } case TbotsProto::Primitive::kDirectControl: { @@ -113,26 +216,109 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { if (!trajectory_path_.has_value() || !angular_trajectory_.has_value()) { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); + LOG(INFO) << "Not moving because trajectory_path_ has value " + << trajectory_path_.has_value() << " angular has " + << angular_trajectory_.has_value(); + return std::make_unique( prim->direct_control()); - LOG(INFO) - << "Not moving because trajectory_path_ or angular_trajectory_ is not set"; - return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + const Point target_position = trajectory_path_->getDestination(); + const Angle target_orientation = angular_trajectory_->getDestination(); + Vector error_lateral = target_position - position_; + Angle error_angular = (target_orientation - orientation_).clamp(); + + Vector target_linear_velocity; + AngularVelocity target_angular_velocity; + // if close enough, use special PID to destination + if (error_lateral.lengthSquared() < LATERAL_PURE_PID_THRESHOLD_METERS) + { + target_linear_velocity = + globalToLocalVelocity({x_pid_close.step(error_lateral.x()), + y_pid_close.step(error_lateral.y())}, + orientation_); + LOG(INFO) << "Y: close to target, doing pure PID with effort " + << target_linear_velocity.y(); + LOG(INFO) << "X: close to target, doing pure PID with effort " + << target_linear_velocity.x(); + } + else + { + // FEEDFORWARD velocities from the trajectory + const Vector trajectory_linear_velocity = getTargetLinearVelocity(); + error_lateral = trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.toSeconds()) - + position_; + const Vector pid_effort_lateral(x_pid.step(error_lateral.x()), + y_pid.step(error_lateral.y())); + LOG(INFO) << "Y: far from target, PID control effort: " + << pid_effort_lateral.y() + << " FF: " << trajectory_linear_velocity.y(); + LOG(INFO) << "X: far from target, PID control effort: " + << pid_effort_lateral.x() + << " FF: " << trajectory_linear_velocity.x(); + target_linear_velocity = globalToLocalVelocity( + trajectory_linear_velocity + pid_effort_lateral, orientation_); + } + if (error_angular.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) + { + target_angular_velocity = AngularVelocity::fromRadians( + w_pid_close.step(error_angular.toRadians())); + LOG(INFO) << "AND: close to target, doing pure PID with effort: " + << target_angular_velocity.toDegrees(); + } + else + { + // FEEDFORWARD velocities from the trajectory + error_angular = + (angular_trajectory_->getPosition( + time_since_angular_trajectory_creation_.toSeconds()) - + orientation_) + .clamp(); + const AngularVelocity trajectory_angular_velocity = + getTargetAngularVelocity(); + const AngularVelocity pid_effort_angular = + AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians())); + target_angular_velocity = + (trajectory_angular_velocity + pid_effort_angular); + LOG(INFO) << "AND: far from target, PID control effort: " + << pid_effort_angular.toDegrees() + << " FF: " << trajectory_angular_velocity.toDegrees(); + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"following_error_prim_deg", + angular_trajectory_ + ->getPosition( + time_since_angular_trajectory_creation_.toSeconds()) + .minDiff(orientation_) + .toDegrees()}, + {"angular_pos_prim_deg", + angular_trajectory_ + ->getPosition( + time_since_angular_trajectory_creation_.toSeconds()) + .toDegrees()}, + {"angular_error_prim_deg", error_angular.toDegrees()}, + }); + } + + LOG(INFO) << "Local velocity x " << target_linear_velocity.x() << " y " + << target_linear_velocity.y(); + + // Make sure target linear velocity is clamped + target_linear_velocity = target_linear_velocity.normalize( + std::min(target_linear_velocity.length(), + static_cast(robot_constants_.robot_max_speed_m_per_s))); - auto output = createDirectControlPrimitive( - local_velocity, angular_velocity, + const auto prim = createDirectControlPrimitive( + target_linear_velocity, target_angular_velocity, convertDribblerModeToDribblerSpeed( current_primitive_.move().dribbler_mode(), robot_constants_), current_primitive_.move().auto_chip_or_kick()); + LOG(INFO) << "handle kmove end"; return std::make_unique( - output->direct_control()); + prim->direct_control()); } case TbotsProto::Primitive::PRIMITIVE_NOT_SET: { @@ -145,8 +331,3 @@ std::unique_ptr PrimitiveExecutor::stepPrimi } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..24684859e4 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -6,6 +6,7 @@ #include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/geom/vector.h" #include "software/time/duration.h" +#include "software/util/pid/pid_controller.hpp" #include "software/world/robot_state.h" #include "software/world/team_types.h" @@ -32,24 +33,15 @@ class PrimitiveExecutor void updatePrimitive(const TbotsProto::Primitive& primitive_msg); /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - - /** - * Update primitive executor with the current velocity of the robot + * Update primitive executor with the current velocity and orientation of the robot * - * @param local_velocity The current _local_ velocity + * @param position The current position + * @param velocity The current velocity + * @param orientation The current orientation of the robot * @param angular_velocity The current angular velocity */ - void updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity); - - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); + void updateState(const Point& position, const Vector& velocity, + const Angle& orientation, const AngularVelocity& angular_velocity); /** * Steps the current primitive and returns a direct control primitive with the @@ -69,6 +61,23 @@ class PrimitiveExecutor */ Vector getTargetLinearVelocity(); + /** + * + * @param new_trajectory The new trajectory requested by the AI. + * @return True if the new trajectory requested is meaningfully different from the + * current trajectory. That is, if the destinations are new. + */ + bool isLateralTrajectoryNew( + const std::optional& new_trajectory) const; + + /** + * + * @param new_trajectory The new trajectory requested by the AI. + * @return True if the new trajectory requested is meaningfully different from the + * current trajectory. That is, if the destinations are new. + */ + bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const; + /* * Returns the next target angular velocity the robot * @@ -77,24 +86,55 @@ class PrimitiveExecutor AngularVelocity getTargetAngularVelocity(); TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; + + std::optional trajectory_path_; + std::optional angular_trajectory_; + + Duration time_since_angular_trajectory_creation_; + Duration time_since_linear_trajectory_creation_; + + Point position_; Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; + + Point last_position_; + Angle last_orientation_; + TeamColour friendly_team_colour_; robot_constants::RobotConstants robot_constants_; - std::optional trajectory_path_; - std::optional angular_trajectory_; // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant // time step to be used, in Seconds Duration time_step_; RobotId robot_id_; - // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; + controls::PIDController x_pid = {0.8, 0, 0, 0}; + controls::PIDController y_pid = {0.8, 0, 0, 0}; + controls::PIDController w_pid = {.7, 0.000, 2, 0000}; + + // When close to target position, ignore trajectory velocity and use pure PID control. + // These PIDs should be used in that case. + controls::PIDController x_pid_close = {2, 0, 0, 0}; + controls::PIDController y_pid_close = {2, 0, 0, 0}; + controls::PIDController w_pid_close = {2, 0, 4, 0}; + + // If distance between current lateral trajectory destination and new one is larger + // than this, we change trajectories. + static constexpr double LATERAL_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; + + static constexpr double LATERAL_STALL_ERROR_MAX_METERS = .4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13; + + static constexpr double LATERAL_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. - static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; + static constexpr double MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M = 0.05; + + // The angular distance away from the destination at which we start dampening + // the angular velocity to avoid jittering around the destination. + static constexpr double MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES = 5; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp new file mode 100644 index 0000000000..5ab72f088c --- /dev/null +++ b/src/software/embedded/robot_localizer.cpp @@ -0,0 +1,336 @@ +#include "robot_localizer.h" + +#include + +RobotLocalizer::RobotLocalizer(const double process_noise_variance, + const double vision_noise_variance, + const double motor_sensor_noise_variance) + : process_linear_acceleration_noise_variance_(process_noise_variance), + process_angular_acceleration_noise_variance_(process_noise_variance) +{ + filter_.state_covariance = + Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); + + filter_.measurement_covariance = + Eigen::Vector( + vision_noise_variance, vision_noise_variance, vision_noise_variance, + vision_noise_variance, vision_noise_variance, vision_noise_variance, + motor_sensor_noise_variance, motor_sensor_noise_variance, + motor_sensor_noise_variance, ImuService::IMU_VARIANCE) + .asDiagonal(); + + last_step_time_ = std::chrono::steady_clock::now(); +} + +void RobotLocalizer::step(const Vector& target_linear_acceleration, + const AngularVelocity& target_angular_acceleration) +{ + FilterStep step{ + .prediction = FilterStep::Predict{}, + .update = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + const std::chrono::duration delta_time = step.time - last_step_time_; + const double delta_time_seconds = delta_time.count(); + last_step_time_ = step.time; + + // clang-format off + step.prediction->process_model << + 1, 0, 0, delta_time_seconds, 0, 0, + 0, 1, 0, 0, delta_time_seconds, 0, + 0, 0, 1, 0, 0, delta_time_seconds, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const double delta_time_squared = delta_time_seconds * delta_time_seconds; + const double delta_time_cubed = delta_time_squared * delta_time_seconds; + const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + + auto& process_covariance = step.prediction->process_covariance; + process_covariance.setZero(); + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = + delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ORIENTATION)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_squared * process_angular_acceleration_noise_variance_; + + auto& control_model = step.prediction->control_model; + control_model.setZero(); + + control_model(static_cast(StateIndex::X_POSITION), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds / 2; + + control_model(static_cast(StateIndex::Y_POSITION), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds / 2; + + control_model(static_cast(StateIndex::ORIENTATION), + static_cast(ControlIndex::ANGULAR_ACCELERATION)) = + delta_time_seconds / 2; + + control_model(static_cast(StateIndex::X_VELOCITY), + static_cast(ControlIndex::X_ACCELERATION)) = 1; + + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_ACCELERATION)) = 1; + + control_model(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(ControlIndex::ANGULAR_ACCELERATION)) = 1; + + step.prediction->control_input << target_linear_acceleration.x(), + target_linear_acceleration.y(), target_angular_acceleration.toRadians(); + + filter_.process_model = step.prediction->process_model; + filter_.process_covariance = step.prediction->process_covariance; + filter_.control_model = step.prediction->control_model; + + history.push_front(step); + filter_.predict(step.prediction->control_input); +} + +void RobotLocalizer::updateVision(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity, + const double age_seconds) +{ + if (history.empty()) + { + updateFilterWithVision(position, velocity, orientation, angular_velocity); + return; + } + + const auto current_time = std::chrono::steady_clock::now(); + const auto sample_age = std::chrono::duration(age_seconds); + + const auto rollback_point = std::find_if( + history.begin(), history.end(), + [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + + if (rollback_point == history.begin()) + { + // All history predates the sample, no need to rollback + updateFilterWithVision(position, velocity, orientation, angular_velocity); + return; + } + + auto replay_iter = std::make_reverse_iterator(rollback_point); + + // Truncate history after the rollback point + history.erase(rollback_point, history.end()); + + filter_.state_estimate = replay_iter->state_estimate; + filter_.state_covariance = replay_iter->state_covariance; + + updateFilterWithVision(position, velocity, orientation, angular_velocity); + + // Replay from the rollback point back to the current estimate + for (; replay_iter != history.rbegin(); --replay_iter) + { + if (replay_iter->prediction.has_value()) + { + const auto& prediction = replay_iter->prediction.value(); + filter_.process_model = prediction.process_model; + filter_.process_covariance = prediction.process_covariance; + filter_.control_model = prediction.control_model; + filter_.predict(prediction.control_input); + } + + if (replay_iter->update.has_value()) + { + const auto& update = replay_iter->update.value(); + filter_.measurement_model = update.measurement_model; + filter_.update(update.measurement); + } + } +} + +void RobotLocalizer::updateFilterWithVision(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) +{ + const double orientation_estimate = + filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); + + Eigen::Vector measurement; + measurement.setZero(); + + measurement(static_cast(MeasurementIndex::VISION_X_POSITION)) = + position.x(); + measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = + position.y(); + measurement(static_cast(MeasurementIndex::VISION_X_VELOCITY)) = + velocity.x(); + measurement(static_cast(MeasurementIndex::VISION_Y_VELOCITY)) = + velocity.y(); + + // Coterminal angle that is closest to current estimate + measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = + orientation_estimate + + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); + + measurement(static_cast(MeasurementIndex::VISION_ANGULAR_VELOCITY)) = + angular_velocity.toRadians(); + + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + // filter_.measurement_model( + // static_cast(MeasurementIndex::VISION_X_VELOCITY), + // static_cast(StateIndex::X_VELOCITY)) = 1; + // filter_.measurement_model( + // static_cast(MeasurementIndex::VISION_Y_VELOCITY), + // static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + // filter_.measurement_model( + // static_cast(MeasurementIndex::VISION_ANGULAR_VELOCITY), + // static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + filter_.update(measurement); +} + +void RobotLocalizer::updateMotorSensors(const Vector& velocity, + const AngularVelocity& angular_velocity) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = + velocity.x(); + update.measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = + velocity.y(); + update.measurement(static_cast( + MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +void RobotLocalizer::updateImu(const AngularVelocity& angular_velocity) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast( + MeasurementIndex::IMU_ANGULAR_VELOCITY)) = angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +Point RobotLocalizer::getPosition() const +{ + return Point( + filter_.state_estimate(static_cast(StateIndex::X_POSITION)), + filter_.state_estimate(static_cast(StateIndex::Y_POSITION))); +} + +Vector RobotLocalizer::getVelocity() const +{ + return Vector( + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)), + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY))); +} + +Angle RobotLocalizer::getOrientation() const +{ + return Angle::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ORIENTATION))) + .clamp(); +} + +AngularVelocity RobotLocalizer::getAngularVelocity() const +{ + return AngularVelocity::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY))); +} diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h new file mode 100644 index 0000000000..d7cd307ede --- /dev/null +++ b/src/software/embedded/robot_localizer.h @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include + +#include "proto/world.pb.h" +#include "software/embedded/services/imu.h" +#include "software/geom/angle.h" +#include "software/geom/point.h" +#include "software/geom/vector.h" +#include "software/sensor_fusion/filter/kalman_filter.hpp" +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + ANGULAR_VELOCITY); + +MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, + VISION_X_VELOCITY, VISION_Y_VELOCITY, VISION_ANGULAR_VELOCITY, MOTOR_X_VELOCITY, + MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); + +MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION, ANGULAR_ACCELERATION); + +/** + * Estimates robot orientation, angular velocity, and angular acceleration + * using a Kalman filter. + * + * The filter keeps a history of recent predict/update operations. When delayed + * vision data arrives, the localizer rewinds to the matching historical state, + * applies the delayed measurement, then replays newer steps to recover the + * current estimate. + */ +class RobotLocalizer +{ + public: + /** + * Creates a new robot localizer. + * + * The variances determine how strongly each source influences the estimate. + * + * @param process_noise_variance Variance applied to the process noise model. + * @param vision_noise_variance Variance of camera heading measurements. + * @param motor_sensor_noise_variance Variance of motor sensor angular velocity. + */ + explicit RobotLocalizer(double process_noise_variance, double vision_noise_variance, + double motor_sensor_noise_variance); + + /** + * Runs one prediction step using elapsed time since the previous call. + * + * @param target_linear_acceleration The current target linear acceleration of the + * robot + * @param target_angular_acceleration The current target angular acceleration of the + * robot + */ + void step(const Vector& target_linear_acceleration, + const AngularVelocity& target_angular_acceleration); + + /** + * Update the robot's position and velocity from data reported by vision. + * + * @param position Vision reading of the robot's position in world space + * @param velocity Vision reading of the robot's velocity in world space + * @param orientation Vision reading of the robot's orientation in world space + * @param angular_velocity Vision reading of the robot's angular velocity + * @param age_seconds Age in seconds of the vision snapshot (time since it was taken) + */ + void updateVision(const Point& position, const Vector& velocity, + const Angle& orientation, const AngularVelocity& angular_velocity, + double age_seconds); + + /** + * Update the robot's velocity from data reported by motor sensors + * (i.e. encoders or Hall sensors). + * + * @param velocity Motor sensor reading of the robot's velocity in local space + * @param angular_velocity Motor sensor reading of the robot's angular velocity + */ + void updateMotorSensors(const Vector& velocity, + const AngularVelocity& angular_velocity); + + /** + * Update the angular velocity from IMU. + * + * @param angular_velocity angular velocity of the robot + */ + void updateImu(const AngularVelocity& angular_velocity); + + /** + * Gets the estimated position of the robot in world space. + * + * @return the estimated position of the robot in world space + */ + Point getPosition() const; + + /** + * Gets the estimated velocity of the robot in world space. + * + * @return the estimated velocity of the robot in world space + */ + Vector getVelocity() const; + + /** + * Gets the estimated orientation of the robot in world space. + * + * @return estimated orientation of the robot in world space + */ + Angle getOrientation() const; + + /** + * Gets the estimated angular velocity of the robot. + * + * @return estimated angular velocity of the robot + */ + AngularVelocity getAngularVelocity() const; + + private: + /** + * Update the Kalman filter with the robot's position and velocity from vision. + * + * @param position Vision reading of the robot's position in world space + * @param velocity Vision reading of the robot's velocity in world space + * @param orientation Vision reading of the robot's orientation in world space + * @param angular_velocity Vision reading of the robot's angular velocity + */ + void updateFilterWithVision(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity); + + static constexpr size_t STATE_SIZE = reflective_enum::size(); + static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); + static constexpr size_t CONTROL_SIZE = reflective_enum::size(); + + /** + * Snapshot of a Kalman filter predict/update step needed for rollback/replay. + */ + struct FilterStep + { + struct Predict + { + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Vector control_input; + }; + + struct Update + { + Eigen::Matrix measurement_model; + Eigen::Vector measurement; + }; + + std::optional prediction; + std::optional update; + + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + + std::chrono::time_point time; + }; + + KalmanFilter filter_; + + // Process noise variance used in prediction + double process_linear_acceleration_noise_variance_; + double process_angular_acceleration_noise_variance_; + + std::chrono::time_point last_step_time_; + + // History is ordered newest-first (front is the most recent step) + std::deque history; +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 528b1eedc9..0943a4c012 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -23,6 +23,18 @@ cc_library( ], ) +cc_library( + name = "imu", + srcs = ["imu.cpp"], + hdrs = ["imu.h"], + deps = [ + "//proto:tbots_cc_proto", + "//software/geom:angular_velocity", + "//software/logger", + "@eigen", + ], +) + cc_library( name = "power", srcs = ["power.cpp"], diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp new file mode 100644 index 0000000000..d3a8481dfd --- /dev/null +++ b/src/software/embedded/services/imu.cpp @@ -0,0 +1,149 @@ +#include "imu.h" + +#include +#include +#include +#include + +#include "software/logger/logger.h" + +// these functions taken from +// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c +__s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, + union i2c_smbus_data* data) +{ + struct i2c_smbus_ioctl_data args; + __s32 err; + + args.read_write = read_write; + args.command = command; + args.size = size; + args.data = data; + + err = ioctl(file, I2C_SMBUS, &args); + if (err == -1) + err = -errno; + return err; +} + +__s32 i2c_smbus_read_byte_data(int file, __u8 command) +{ + union i2c_smbus_data data; + int err; + + err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data); + if (err < 0) + return err; + + return 0x0FF & data.byte; +} + +__s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) +{ + union i2c_smbus_data data; + data.byte = value; + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); +} + +ImuService::ImuService() : initialized_(false) +{ + // Establish connection to the IMU and verify that the who am I pin is correct. + file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR); + int ret = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b); + if (ret < 0) + { + LOG(WARNING) + << "Failed to initialize the IMU: failed to establish i2c connection."; + return; + } + if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106) + { + LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register " + << static_cast(WHOAMI_REG) << " read incorrectly."; + return; + } + // Attempt to enable gyro and accelerometer, checking that writes are successful + // See lsm6dsl datasheet for how to set these registers. + i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000); + if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to accelerometer config register " + << static_cast(ACCEL_CONTROL_REG) << " unsuccessful"; + return; + } + // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of + // datasheet for lsm6dsl, pg 21) + i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000); + if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to gyroscope config register " + << "unsuccessful"; + return; + } + + // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000) + // CTRL4_C: bit 1 (LPF1_SEL_G) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010); + // CTRL6_C: bits 2:0 (FTYPE) = 000 + i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000); + + // Enable Accelerometer digital LPF2 + // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000); + + initialized_ = true; + LOG(INFO) << "Initialized IMU! Calibrating..."; + degrees_error_ = 0; + // get 100 sample average of stationary reading, so all future readings can be + // corrected + double sum = 0; + int valid_samples = 0; + for (int i = 0; i < 100; i++) + { + auto poll = pollHeadingRate(); + if (poll.has_value()) + { + sum += poll->toDegrees(); + valid_samples++; + } + usleep(50000); + } + + if (valid_samples > 0) + { + degrees_error_ = sum / valid_samples; + LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps"; + } + else + { + LOG(WARNING) << "IMU Calibration failed: no valid samples received. Heading " + "stability will be poor."; + initialized_ = false; + } +} + +std::optional ImuService::pollHeadingRate() +{ + if (!initialized_) + { + return std::nullopt; + } + + // Two separate registers for the Gyro output data. + int least_significant = i2c_smbus_read_byte_data(file_descriptor_, YAW_LEAST_SIG_REG); + int most_significant = i2c_smbus_read_byte_data(file_descriptor_, YAW_MOST_SIG_REG); + + if (least_significant < 0 || most_significant < 0) + { + return std::nullopt; + } + + int16_t full_word = (static_cast(most_significant) << 8) | + static_cast(least_significant); + + double degrees_per_sec = static_cast(full_word) / + static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h new file mode 100644 index 0000000000..1924747869 --- /dev/null +++ b/src/software/embedded/services/imu.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include + +#include "proto/tbots_software_msgs.pb.h" +#include "software/geom/angular_velocity.h" + +/** + * Handles low level IMU I2C communication, and some minor offset filtering. + */ +class ImuService +{ + public: + /** + * Constructs and initializes a new IMU service object. + * + * If successfully initialized, will try to do a simple calibration of the IMU. + */ + ImuService(); + + /** + * Polls the IMU to return latest reading on the first time derivative of the + * heading/yaw. + * @return current measured AngularVelocity of the robot. + */ + std::optional pollHeadingRate(); + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * + (4.0 * 14.4222 / 1000.0 * M_PI / + 180.0); // stdev = 4mdeg/Hz noise density * sqrt(208) Hz * 1/1000 deg/mdeg * + // pi/180 rad/deg + private: + bool initialized_ = false; + int file_descriptor_ = 0; + double degrees_error_; + + // The full scale of the accelerometer, that is, for any accelerometer output x, the + // acceleration = x / SHRT_MAX * ACCELEROMETER_FULL_SCALE_G + static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; + // If this is changed, the IMU config needs to be different. + // The full scale of the IMU, that is, for IMU output a, angular velocity in DPS = a / + // SHRT_MAX * IMU_FULL_SCALE_DPS + static constexpr double IMU_FULL_SCALE_DPS = 1000.0; + + // Device path for the IMU + inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + + // Various i2c registers. + static const uint8_t WHOAMI_REG = 0xf; + static const uint8_t ACCEL_CONTROL_REG = 0x10; + static const uint8_t GYRO_CONTROL_REG = 0x11; + static const uint8_t CTRL4_C = 0x13; + static const uint8_t CTRL6_C = 0x15; + static const uint8_t CTRL8_XL = 0x17; + static const uint8_t YAW_LEAST_SIG_REG = 0x26; + static const uint8_t YAW_MOST_SIG_REG = 0x27; +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b7b300a393..d379459801 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,6 +4,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -14,6 +15,7 @@ #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" +#include "software/physics/velocity_conversion_util.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -83,7 +85,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + TeamColour::YELLOW, robot_id_), + robot_localizer_(robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2) { waitForNetworkUp(); @@ -121,7 +126,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, motor_service_ = std::make_unique(robot_constants); g_motor_service = motor_service_.get(); motor_service_->setup(); - LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; + LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; + + imu_service_ = std::make_unique(); + LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ << ", CHANNEL ID: " << channel_id_ @@ -229,6 +237,24 @@ void Thunderloop::runLoop() // Save new primitive primitive_ = new_primitive; + if (primitive_.has_move()) + { + const Point position = + createPoint(primitive_.move().xy_traj_params().start_position()); + + const Vector velocity = createVector( + primitive_.move().xy_traj_params().initial_velocity()); + + const Angle orientation = + createAngle(primitive_.move().w_traj_params().start_angle()); + + const AngularVelocity angular_velocity = createAngularVelocity( + primitive_.move().w_traj_params().initial_velocity()); + + robot_localizer_.updateVision(position, velocity, orientation, + angular_velocity, RTT_S / 2); + } + // Update primitive executor's primitive set { clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); @@ -244,12 +270,44 @@ void Thunderloop::runLoop() } } + std::optional imu_poll = imu_service_->pollHeadingRate(); + + if (imu_poll.has_value()) + { + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"imu_ang_vel", imu_poll.value().toDegrees()}, + }); + robot_localizer_.updateImu(imu_poll.value()); + } + if (motor_status_.has_value()) { auto status = motor_status_.value(); - primitive_executor_.updateVelocity( - createVector(status.local_velocity()), + + robot_localizer_.updateMotorSensors( + localToGlobalVelocity(createVector(status.local_velocity()), + robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())); + + // const Vector target_velocity = + // localToGlobalVelocity(createVector(status.target_local_velocity()), + // robot_localizer_.getOrientation()); + // const AngularVelocity target_angular_velocity = + // createAngularVelocity(status.target_angular_velocity()); + + // const Vector delta_velocity = target_velocity - last_target_velocity_; + // const AngularVelocity delta_angular_velocity = + // target_angular_velocity - last_target_angular_velocity_; + + // last_target_velocity_ = target_velocity; + // last_target_angular_velocity_ = target_angular_velocity; + + robot_localizer_.step(Vector(), AngularVelocity::zero()); + + primitive_executor_.updateState(robot_localizer_.getPosition(), + robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), + robot_localizer_.getAngularVelocity()); } // Timeout Overrides for Primitives @@ -268,7 +326,7 @@ void Thunderloop::runLoop() if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) { - primitive_executor_.setStopPrimitive(); + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } direct_control_ = diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 6d12cd4c0d..0dc17fbab5 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -10,6 +10,8 @@ #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" @@ -60,6 +62,7 @@ class Thunderloop std::unique_ptr motor_service_; std::unique_ptr network_service_; std::unique_ptr power_service_; + std::unique_ptr imu_service_; // TOML config client std::unique_ptr toml_config_client_; @@ -155,6 +158,9 @@ class Thunderloop // Primitive Executor PrimitiveExecutor primitive_executor_; + // Robot localization model + RobotLocalizer robot_localizer_; + // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; @@ -167,6 +173,9 @@ class Thunderloop // Path to the CPU thermal zone temperature file const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; + + Vector last_target_velocity_; + AngularVelocity last_target_angular_velocity_; }; /* diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 1b9cde8dac..7bacff40f4 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -8,6 +8,50 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" +#ifdef DEBUG_WHEEL +EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants) +{ + // Angles [rads] + auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.; + auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.; + + auto cos_p = std::cos(p); + auto sin_p = std::sin(p); + auto cos_t = std::cos(t); + auto sin_t = std::sin(t); + + // 1. Physical wheel coordinates in meters + // Mapped to the robot frame: +X = Right, +Y = Forward + double fr_x = 0.06632, fr_y = 0.03485; + double br_x = 0.05592, br_y = -0.04985; + + // 2. Unit drive vectors (extracted directly from the matrix's linear columns) + double fr_dx = -sin_p, fr_dy = cos_p; + double br_dx = sin_t, br_dy = cos_t; + + // 3. Dynamically calculate the rotational lever arms using the 2D cross product + // Lever Arm = | X * Dy - Y * Dx | + double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx)); + double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx)); + + // clang-format off + euclidean_to_wheel_velocity_D_ << + -sin_p, cos_p, rot_f, + -sin_p, -cos_p, rot_f, + sin_t, -cos_t, rot_b, + sin_t, cos_t, rot_b; + // clang-format on + + // Calculate Pseudo-inverse dynamically + wheel_to_euclidean_velocity_D_inverse_ = + (euclidean_to_wheel_velocity_D_.transpose() * euclidean_to_wheel_velocity_D_) + .inverse() * + euclidean_to_wheel_velocity_D_.transpose(); +} + +#else + EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) : robot_constants_(robot_constants) { @@ -56,6 +100,26 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ // clang-format on } +#endif + +#ifdef DEBUG_WHEEL +WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const +{ + // The generalized D matrix natively handles the w -> tangential velocity + // conversion because its third column already represents the lever arm in meters. + return euclidean_to_wheel_velocity_D_ * euclidean_velocity; +} + + +EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( + const WheelSpace_t& wheel_velocity) const +{ + // The D inverse matrix natively outputs w in rad/s. + return wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity; +} + +#else + WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const { // need to multiply the angular velocity by the robot radius to @@ -80,6 +144,7 @@ EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( return euclidean_velocity; } +#endif WheelSpace_t EuclideanToWheel::rampWheelVelocity( const WheelSpace_t& current_wheel_velocity, const WheelSpace_t& target_wheel_velocity, diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index b1fe981abe..a20f723159 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -7,6 +7,8 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" +#define DEBUG_WHEEL + /** * Vector representation of 2D Euclidean space. * diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index b546c9f01f..1cd3024560 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -106,6 +106,51 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } +#ifdef DEBUG_WHEEL +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) +{ + // Test +w/counter-clockwise + target_euclidean_velocity = {0, 0, 1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + + // Because the wheels are offset, their speed at 1 rad/s equals their + // exact physical lever arm (in meters), NOT the nominal robot_radius. + // Based on the physical offsets, the lever arm evaluates to 0.0746 meters. + double expected_lever_arm = 0.0746; + + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + expected_lever_arm, 0.001); + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); +} + +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) +{ + // Test -w/clockwise + target_euclidean_velocity = {0, 0, -1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + + // Because the wheels are offset, their speed at -1 rad/s equals their + // exact physical lever arm (in meters) multiplied by the negative velocity. + double expected_lever_arm = -0.0746; + + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + expected_lever_arm, 0.001); + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); +} +#else + TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) { // Test +w/counter-clockwise @@ -123,6 +168,7 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], robot_radius); } + TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) { // Test -w/clockwise @@ -143,6 +189,8 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) -robot_radius); } +#endif + TEST_F(EuclideanToWheelTest, test_conversion_is_linear) { target_euclidean_velocity = {3, 1, 5}; diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 42296afa2d..a35942b53d 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -100,6 +100,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_WrapperPacket& packet) updateWorld(packet.detection()); + // LOG(WARNING) << "ROBOTS DETECTED: "<< packet.detection().robots_blue().size(); if (!ball && (packet.detection().robots_blue().size() != 0 || packet.detection().robots_yellow().size() != 0)) { @@ -364,6 +365,7 @@ Team SensorFusion::createFriendlyTeam(const std::vector& robot_d { Team new_friendly_team = friendly_team_filter.getFilteredData( friendly_team, robot_detections, friendly_robot_id_with_ball_in_dribbler); + return new_friendly_team; } diff --git a/src/software/simulated_tests/requirements_lock.txt b/src/software/simulated_tests/requirements_lock.txt index 8bf6d03071..c6cc74cfd3 100644 --- a/src/software/simulated_tests/requirements_lock.txt +++ b/src/software/simulated_tests/requirements_lock.txt @@ -4,21 +4,21 @@ # # bazel run //software/simulated_tests:requirements.update # -attrs==23.2.0 \ - --hash=sha256:935dc3b529c262f6cf76e50877d35a4bd3c1de194fd41f47a2b7ae8f19971f30 \ - --hash=sha256:99b87a485a5820b23b879f04c2305b44b951b502fd64be915879d77a7e8fc6f1 +attrs==26.1.0 \ + --hash=sha256:c647aa4a12dfbad9333ca4e71fe62ddc36f4e63b2d260a37a8b83d2f043ac309 \ + --hash=sha256:d03ceb89cb322a8fd706d4fb91940737b6642aa36998fe130a9bc96c985eff32 # via pytest -iniconfig==2.0.0 \ - --hash=sha256:2d91e135bf72d31a410b17c16da610a82cb55f6b0477d1a902134b24a455b8b3 \ - --hash=sha256:b6a85871a79d2e3b22d2d1b94ac2824226a63c6b741c88f7ae975f18b6778374 +iniconfig==2.3.0 \ + --hash=sha256:c76315c77db068650d49c5b56314774a7804df16fee4402c1f19d6d15d8c4730 \ + --hash=sha256:f631c04d2c48c52b84d0d0549c99ff3859c98df65b3101406327ecc7d53fbf12 # via pytest -packaging==24.1 \ - --hash=sha256:026ed72c8ed3fcce5bf8950572258698927fd1dbda10a5e981cdf0ac37f4f002 \ - --hash=sha256:5b8f2217dbdbd2f7f384c41c628544e6d52f2d0f53c6d0c3ea61aa5d1d7ff124 +packaging==26.2 \ + --hash=sha256:5fc45236b9446107ff2415ce77c807cee2862cb6fac22b8a73826d0693b0980e \ + --hash=sha256:ff452ff5a3e828ce110190feff1178bb1f2ea2281fa2075aadb987c2fb221661 # via pytest -pluggy==1.5.0 \ - --hash=sha256:2cffa88e94fdc978c4c574f15f9e59b7f4201d439195c3715ca9e2486f1d0cf1 \ - --hash=sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669 +pluggy==1.6.0 \ + --hash=sha256:7dcc130b76258d33b90f61b658791dede3486c3e6bfb003ee5c9bfb396dd22f3 \ + --hash=sha256:e920276dd6813095e9377c0bc5566d94c932c33b27a3e3945d8389c374dd4746 # via pytest py==1.11.0 \ --hash=sha256:51c75c4126074b472f746a24399ad32f6053d1b34b68d2fa41e558e6f4a98719 \ diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..904c48d039 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -299,19 +299,22 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.yellow_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; + for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -320,20 +323,22 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.blue_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.blue_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -342,8 +347,8 @@ void ErForceSimulator::setRobotPrimitive( RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity) + const TbotsProto::World& world_msg, const Point& position, const Angle& orientation, + const Vector& velocity, const AngularVelocity& angular_velocity) { // Set to NEG_X because the world msg in this simulator is normalized // correctly @@ -352,8 +357,9 @@ void ErForceSimulator::setRobotPrimitive( if (robot_primitive_executor_iter != robot_primitive_executor_map.end()) { auto primitive_executor = robot_primitive_executor_iter->second; + primitive_executor->updateState(position, velocity, orientation, + angular_velocity); primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(local_velocity, angular_velocity); } else { @@ -370,16 +376,16 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( SSLSimulationProto::RobotControl robot_control; auto sim_state = getSimulatorState(); - std::map> current_velocity_map; + std::map> current_state; if (side == gameController::Team::BLUE) { const auto& sim_robots = sim_state.blue_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } else { const auto& sim_robots = sim_state.yellow_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } for (auto& primitive_executor_with_id : robot_primitive_executor_map) @@ -393,8 +399,9 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, primitive_executor_time_step_s); } else @@ -564,18 +571,18 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } -std::map> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map> +ErForceSimulator::getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map> map; for (const auto& sim_robot : sim_robots) { - const Vector local_vel = - globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), - Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); - robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; + const Point position = {sim_robot.p_x(), sim_robot.p_y()}; + const Angle orientation = Angle::fromRadians(sim_robot.angle()); + const Vector velocity = {sim_robot.v_x(), sim_robot.v_y()}; + const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); + map[sim_robot.id()] = {position, orientation, velocity, angular_vel}; } - return robot_to_local_velocity; + return map; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..9b65c81449 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -158,18 +158,19 @@ class ErForceSimulator RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity); + const TbotsProto::World& world_msg, const Point& position, + const Angle& orientation, const Vector& velocity, + const AngularVelocity& angular_velocity); /** - * Gets a map from robot id to local and angular velocity from repeated sim robots + * Gets a map from robot id to position and velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos * - * @return a map from robot id to local velocity and angular velocity + * @return a map from robot id to position and velocity */ - static std::map> - getRobotIdToLocalVelocityMap( + static std::map> + getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 59b4c565ae..110b12c538 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -159,7 +159,7 @@ def setup_gl_widget( gl_widget.add_layer(validation_layer) gl_widget.add_layer(trail_layer, False) gl_widget.add_layer(debug_shapes_layer, True) - gl_widget.add_layer(field_movement_layer, False) + gl_widget.add_layer(field_movement_layer, True) gl_widget.add_layer(max_dribble_layer, True) gl_widget.add_layer(referee_layer) diff --git a/src/software/thunderscope/wifi_communication_manager.py b/src/software/thunderscope/wifi_communication_manager.py index 7cb80f25b3..c53805cb54 100644 --- a/src/software/thunderscope/wifi_communication_manager.py +++ b/src/software/thunderscope/wifi_communication_manager.py @@ -409,7 +409,7 @@ def send_primitive(self, robot_id: int, primitive: Primitive) -> None: primitive_sender = self.primitive_senders[robot_id][1] if primitive_sender is not None: primitive_sender.send_proto(primitive, True) - else: - logger.warning( - f"Robot {robot_id} is not connected. Unable to send a primitive to it." - ) + # else: + # logger.warning( + # f"Robot {robot_id} is not connected. Unable to send a primitive to it." + # ) diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD new file mode 100644 index 0000000000..156e74085e --- /dev/null +++ b/src/software/util/pid/BUILD @@ -0,0 +1,8 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "pid", + hdrs = [ + "pid_controller.hpp", + ], +) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp new file mode 100644 index 0000000000..e52c66a011 --- /dev/null +++ b/src/software/util/pid/pid_controller.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +namespace controls +{ +template +class PIDController +{ + public: + /** + * Constructs a new PID controller. + **/ + PIDController(T k_p, T k_i, T k_d, T max_integral); + + /** + * Given an error, returns the control effort to minimize it. + * + * @param deltaTime The time passed since last step, for calculating integrator and + *derivative. If this function is calling in invariant intervals, deltaTime can be set + *to 1 and any effects it would have can be handled by tuning kD. + **/ + T step(T error, T delta_time = 1.0f); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + T k_p; + T k_i; + T k_d; + T max_integral; + + protected: + T integral; + std::optional last_error = std::nullopt; +}; +} // namespace controls + +template +controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) + : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; + +template +T controls::PIDController::step(T error, T delta_time) +{ + // If sign of error swaps, reset integrator + if (last_error.value_or(0) * error < 0) + { + integral = 0; + } + + integral += std::max(std::min(error * delta_time, max_integral), -max_integral); + // set derivative, if no last_error, just set to 0 + const T derivative = (last_error.value_or(error) - error) / delta_time; + last_error = error; + return error * k_p + integral * k_i + derivative * k_d; +} + +template +void controls::PIDController::reset() +{ + integral = 0; + last_error = std::nullopt; +} diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 66e2244d76..5a13d519a6 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -218,7 +218,8 @@ Duration Robot::getTimeToPosition(const Point& destination, double initial_velocity_1d = velocity().dot(dist_vector.normalize()); double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); - return getTimeToTravelDistance(dist, robot_constants_.robot_max_speed_m_per_s, + return getTimeToTravelDistance(dist, + robot_constants_.robot_max_speed_trajectory_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); } From 1fbc65ae870223a18f411558de160335d2c5beec Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 14:51:50 -0700 Subject: [PATCH 09/38] Revert temporary changes --- .../simulated_tests/requirements_lock.txt | 24 +++++++++---------- .../thunderscope/widget_setup_functions.py | 2 +- .../wifi_communication_manager.py | 8 +++---- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/software/simulated_tests/requirements_lock.txt b/src/software/simulated_tests/requirements_lock.txt index c6cc74cfd3..8bf6d03071 100644 --- a/src/software/simulated_tests/requirements_lock.txt +++ b/src/software/simulated_tests/requirements_lock.txt @@ -4,21 +4,21 @@ # # bazel run //software/simulated_tests:requirements.update # -attrs==26.1.0 \ - --hash=sha256:c647aa4a12dfbad9333ca4e71fe62ddc36f4e63b2d260a37a8b83d2f043ac309 \ - --hash=sha256:d03ceb89cb322a8fd706d4fb91940737b6642aa36998fe130a9bc96c985eff32 +attrs==23.2.0 \ + --hash=sha256:935dc3b529c262f6cf76e50877d35a4bd3c1de194fd41f47a2b7ae8f19971f30 \ + --hash=sha256:99b87a485a5820b23b879f04c2305b44b951b502fd64be915879d77a7e8fc6f1 # via pytest -iniconfig==2.3.0 \ - --hash=sha256:c76315c77db068650d49c5b56314774a7804df16fee4402c1f19d6d15d8c4730 \ - --hash=sha256:f631c04d2c48c52b84d0d0549c99ff3859c98df65b3101406327ecc7d53fbf12 +iniconfig==2.0.0 \ + --hash=sha256:2d91e135bf72d31a410b17c16da610a82cb55f6b0477d1a902134b24a455b8b3 \ + --hash=sha256:b6a85871a79d2e3b22d2d1b94ac2824226a63c6b741c88f7ae975f18b6778374 # via pytest -packaging==26.2 \ - --hash=sha256:5fc45236b9446107ff2415ce77c807cee2862cb6fac22b8a73826d0693b0980e \ - --hash=sha256:ff452ff5a3e828ce110190feff1178bb1f2ea2281fa2075aadb987c2fb221661 +packaging==24.1 \ + --hash=sha256:026ed72c8ed3fcce5bf8950572258698927fd1dbda10a5e981cdf0ac37f4f002 \ + --hash=sha256:5b8f2217dbdbd2f7f384c41c628544e6d52f2d0f53c6d0c3ea61aa5d1d7ff124 # via pytest -pluggy==1.6.0 \ - --hash=sha256:7dcc130b76258d33b90f61b658791dede3486c3e6bfb003ee5c9bfb396dd22f3 \ - --hash=sha256:e920276dd6813095e9377c0bc5566d94c932c33b27a3e3945d8389c374dd4746 +pluggy==1.5.0 \ + --hash=sha256:2cffa88e94fdc978c4c574f15f9e59b7f4201d439195c3715ca9e2486f1d0cf1 \ + --hash=sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669 # via pytest py==1.11.0 \ --hash=sha256:51c75c4126074b472f746a24399ad32f6053d1b34b68d2fa41e558e6f4a98719 \ diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 110b12c538..59b4c565ae 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -159,7 +159,7 @@ def setup_gl_widget( gl_widget.add_layer(validation_layer) gl_widget.add_layer(trail_layer, False) gl_widget.add_layer(debug_shapes_layer, True) - gl_widget.add_layer(field_movement_layer, True) + gl_widget.add_layer(field_movement_layer, False) gl_widget.add_layer(max_dribble_layer, True) gl_widget.add_layer(referee_layer) diff --git a/src/software/thunderscope/wifi_communication_manager.py b/src/software/thunderscope/wifi_communication_manager.py index c53805cb54..7cb80f25b3 100644 --- a/src/software/thunderscope/wifi_communication_manager.py +++ b/src/software/thunderscope/wifi_communication_manager.py @@ -409,7 +409,7 @@ def send_primitive(self, robot_id: int, primitive: Primitive) -> None: primitive_sender = self.primitive_senders[robot_id][1] if primitive_sender is not None: primitive_sender.send_proto(primitive, True) - # else: - # logger.warning( - # f"Robot {robot_id} is not connected. Unable to send a primitive to it." - # ) + else: + logger.warning( + f"Robot {robot_id} is not connected. Unable to send a primitive to it." + ) From a8e7d8049ee056e5e2571761c35b70dd9262236b Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 17 May 2026 16:12:08 -0700 Subject: [PATCH 10/38] Remove usage of velocity readings from vision in robot localizer --- src/software/embedded/primitive_executor.cpp | 72 +++++++------------- src/software/embedded/primitive_executor.h | 13 ++-- src/software/embedded/robot_localizer.cpp | 32 ++------- src/software/embedded/robot_localizer.h | 19 ++---- src/software/embedded/thunderloop.cpp | 26 +------ src/software/embedded/thunderloop.h | 3 - 6 files changed, 43 insertions(+), 122 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 3d19b7664d..c9d60b2926 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -21,7 +21,7 @@ PrimitiveExecutor::PrimitiveExecutor( { } -bool PrimitiveExecutor::isLateralTrajectoryNew( +bool PrimitiveExecutor::isLinearTrajectoryNew( const std::optional& new_trajectory) const { if (new_trajectory.has_value() != trajectory_path_.has_value()) @@ -38,7 +38,7 @@ bool PrimitiveExecutor::isLateralTrajectoryNew( return distance(new_trajectory->getDestination(), trajectory_path_->getDestination()) > - LATERAL_DESTINATION_THRESHOLD_METERS; + LINEAR_DESTINATION_THRESHOLD_METERS; } bool PrimitiveExecutor::isAngularTrajectoryNew( @@ -63,15 +63,16 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const std::optional new_trajectory_path = createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); + // Check if this new trajectory is "new". That is, if its destination differs // meaningfully from our current trajectory. - if (isLateralTrajectoryNew(new_trajectory_path)) + if (isLinearTrajectoryNew(new_trajectory_path)) { trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(RTT_S); x_pid.reset(); y_pid.reset(); - LOG(INFO) << "new lateral trajectory accepted"; + LOG(INFO) << "new linear trajectory accepted"; } const BangBangTrajectory1DAngular new_angular_trajectory = @@ -103,19 +104,21 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit // TODO: Add a timeout to following error if (trajectory_path_.has_value()) { - const double lateral_following_error = + const double linear_following_error = (position_ - trajectory_path_->getPosition( time_since_linear_trajectory_creation_.toSeconds())) .length(); - if (lateral_following_error > LATERAL_STALL_ERROR_MAX_METERS) + + if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) { - // regenerate lateral trajectory + // regenerate linear trajectory trajectory_path_ = createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); } } + if (angular_trajectory_.has_value()) { const double angular_following_error = @@ -123,29 +126,15 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) .minDiff(orientation_) .toDegrees(); - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"following_error_deg", angular_following_error}, - }); + if (angular_following_error > ANGULAR_STALL_ERROR_MAX_DEGREES) { // regenerate angular trajectory angular_trajectory_ = createAngularTrajectoryFromParams( current_primitive_.move().w_traj_params(), orientation_, - AngularVelocity::fromRadians(angular_velocity_.toRadians()), - robot_constants_); + angular_velocity_, robot_constants_); time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); } - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"following_error_after_deg", - angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) - .minDiff(orientation_) - .toDegrees()}, - {"angular_pos_after_deg", - angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) - .toDegrees()}, - }); } } @@ -227,17 +216,18 @@ std::unique_ptr PrimitiveExecutor::stepPrimi const Point target_position = trajectory_path_->getDestination(); const Angle target_orientation = angular_trajectory_->getDestination(); - Vector error_lateral = target_position - position_; + Vector error_linear = target_position - position_; Angle error_angular = (target_orientation - orientation_).clamp(); Vector target_linear_velocity; AngularVelocity target_angular_velocity; + // if close enough, use special PID to destination - if (error_lateral.lengthSquared() < LATERAL_PURE_PID_THRESHOLD_METERS) + if (error_linear.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) { target_linear_velocity = - globalToLocalVelocity({x_pid_close.step(error_lateral.x()), - y_pid_close.step(error_lateral.y())}, + globalToLocalVelocity({x_pid_close.step(error_linear.x()), + y_pid_close.step(error_linear.y())}, orientation_); LOG(INFO) << "Y: close to target, doing pure PID with effort " << target_linear_velocity.y(); @@ -248,19 +238,19 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { // FEEDFORWARD velocities from the trajectory const Vector trajectory_linear_velocity = getTargetLinearVelocity(); - error_lateral = trajectory_path_->getPosition( + error_linear = trajectory_path_->getPosition( time_since_linear_trajectory_creation_.toSeconds()) - - position_; - const Vector pid_effort_lateral(x_pid.step(error_lateral.x()), - y_pid.step(error_lateral.y())); + position_; + const Vector pid_effort_linear(x_pid.step(error_linear.x()), + y_pid.step(error_linear.y())); LOG(INFO) << "Y: far from target, PID control effort: " - << pid_effort_lateral.y() + << pid_effort_linear.y() << " FF: " << trajectory_linear_velocity.y(); LOG(INFO) << "X: far from target, PID control effort: " - << pid_effort_lateral.x() + << pid_effort_linear.x() << " FF: " << trajectory_linear_velocity.x(); target_linear_velocity = globalToLocalVelocity( - trajectory_linear_velocity + pid_effort_lateral, orientation_); + trajectory_linear_velocity + pid_effort_linear, orientation_); } if (error_angular.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) { @@ -286,20 +276,6 @@ std::unique_ptr PrimitiveExecutor::stepPrimi LOG(INFO) << "AND: far from target, PID control effort: " << pid_effort_angular.toDegrees() << " FF: " << trajectory_angular_velocity.toDegrees(); - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"following_error_prim_deg", - angular_trajectory_ - ->getPosition( - time_since_angular_trajectory_creation_.toSeconds()) - .minDiff(orientation_) - .toDegrees()}, - {"angular_pos_prim_deg", - angular_trajectory_ - ->getPosition( - time_since_angular_trajectory_creation_.toSeconds()) - .toDegrees()}, - {"angular_error_prim_deg", error_angular.toDegrees()}, - }); } LOG(INFO) << "Local velocity x " << target_linear_velocity.x() << " y " diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 24684859e4..1a777bcf4e 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -67,8 +67,7 @@ class PrimitiveExecutor * @return True if the new trajectory requested is meaningfully different from the * current trajectory. That is, if the destinations are new. */ - bool isLateralTrajectoryNew( - const std::optional& new_trajectory) const; + bool isLinearTrajectoryNew(const std::optional& new_trajectory) const; /** * @@ -111,7 +110,7 @@ class PrimitiveExecutor controls::PIDController x_pid = {0.8, 0, 0, 0}; controls::PIDController y_pid = {0.8, 0, 0, 0}; - controls::PIDController w_pid = {.7, 0.000, 2, 0000}; + controls::PIDController w_pid = {0.7, 0, 2, 0}; // When close to target position, ignore trajectory velocity and use pure PID control. // These PIDs should be used in that case. @@ -119,15 +118,15 @@ class PrimitiveExecutor controls::PIDController y_pid_close = {2, 0, 0, 0}; controls::PIDController w_pid_close = {2, 0, 4, 0}; - // If distance between current lateral trajectory destination and new one is larger + // If distance between current linear trajectory destination and new one is larger // than this, we change trajectories. - static constexpr double LATERAL_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; - static constexpr double LATERAL_STALL_ERROR_MAX_METERS = .4; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = .4; static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13; - static constexpr double LATERAL_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25; // The distance away from the destination at which we start dampening the velocity diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 5ab72f088c..db4258425c 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -13,7 +13,6 @@ RobotLocalizer::RobotLocalizer(const double process_noise_variance, filter_.measurement_covariance = Eigen::Vector( - vision_noise_variance, vision_noise_variance, vision_noise_variance, vision_noise_variance, vision_noise_variance, vision_noise_variance, motor_sensor_noise_variance, motor_sensor_noise_variance, motor_sensor_noise_variance, ImuService::IMU_VARIANCE) @@ -137,14 +136,12 @@ void RobotLocalizer::step(const Vector& target_linear_acceleration, filter_.predict(step.prediction->control_input); } -void RobotLocalizer::updateVision(const Point& position, const Vector& velocity, - const Angle& orientation, - const AngularVelocity& angular_velocity, +void RobotLocalizer::updateVision(const Point& position, const Angle& orientation, const double age_seconds) { if (history.empty()) { - updateFilterWithVision(position, velocity, orientation, angular_velocity); + updateFilterWithVision(position, orientation); return; } @@ -158,7 +155,7 @@ void RobotLocalizer::updateVision(const Point& position, const Vector& velocity, if (rollback_point == history.begin()) { // All history predates the sample, no need to rollback - updateFilterWithVision(position, velocity, orientation, angular_velocity); + updateFilterWithVision(position, orientation); return; } @@ -170,7 +167,7 @@ void RobotLocalizer::updateVision(const Point& position, const Vector& velocity, filter_.state_estimate = replay_iter->state_estimate; filter_.state_covariance = replay_iter->state_covariance; - updateFilterWithVision(position, velocity, orientation, angular_velocity); + updateFilterWithVision(position, orientation); // Replay from the rollback point back to the current estimate for (; replay_iter != history.rbegin(); --replay_iter) @@ -193,9 +190,8 @@ void RobotLocalizer::updateVision(const Point& position, const Vector& velocity, } } -void RobotLocalizer::updateFilterWithVision(const Point& position, const Vector& velocity, - const Angle& orientation, - const AngularVelocity& angular_velocity) +void RobotLocalizer::updateFilterWithVision(const Point& position, + const Angle& orientation) { const double orientation_estimate = filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); @@ -207,19 +203,12 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, const Vector& position.x(); measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - measurement(static_cast(MeasurementIndex::VISION_X_VELOCITY)) = - velocity.x(); - measurement(static_cast(MeasurementIndex::VISION_Y_VELOCITY)) = - velocity.y(); // Coterminal angle that is closest to current estimate measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); - measurement(static_cast(MeasurementIndex::VISION_ANGULAR_VELOCITY)) = - angular_velocity.toRadians(); - filter_.measurement_model.setZero(); filter_.measurement_model( static_cast(MeasurementIndex::VISION_X_POSITION), @@ -227,18 +216,9 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, const Vector& filter_.measurement_model( static_cast(MeasurementIndex::VISION_Y_POSITION), static_cast(StateIndex::Y_POSITION)) = 1; - // filter_.measurement_model( - // static_cast(MeasurementIndex::VISION_X_VELOCITY), - // static_cast(StateIndex::X_VELOCITY)) = 1; - // filter_.measurement_model( - // static_cast(MeasurementIndex::VISION_Y_VELOCITY), - // static_cast(StateIndex::Y_VELOCITY)) = 1; filter_.measurement_model( static_cast(MeasurementIndex::VISION_ORIENTATION), static_cast(StateIndex::ORIENTATION)) = 1; - // filter_.measurement_model( - // static_cast(MeasurementIndex::VISION_ANGULAR_VELOCITY), - // static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; filter_.update(measurement); } diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index d7cd307ede..10198438df 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -16,8 +16,8 @@ MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCIT ANGULAR_VELOCITY); MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, - VISION_X_VELOCITY, VISION_Y_VELOCITY, VISION_ANGULAR_VELOCITY, MOTOR_X_VELOCITY, - MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); + MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, + IMU_ANGULAR_VELOCITY); MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION, ANGULAR_ACCELERATION); @@ -57,16 +57,13 @@ class RobotLocalizer const AngularVelocity& target_angular_acceleration); /** - * Update the robot's position and velocity from data reported by vision. + * Update the robot's position and orientation from data reported by vision. * * @param position Vision reading of the robot's position in world space - * @param velocity Vision reading of the robot's velocity in world space * @param orientation Vision reading of the robot's orientation in world space - * @param angular_velocity Vision reading of the robot's angular velocity * @param age_seconds Age in seconds of the vision snapshot (time since it was taken) */ - void updateVision(const Point& position, const Vector& velocity, - const Angle& orientation, const AngularVelocity& angular_velocity, + void updateVision(const Point& position, const Angle& orientation, double age_seconds); /** @@ -116,16 +113,12 @@ class RobotLocalizer private: /** - * Update the Kalman filter with the robot's position and velocity from vision. + * Update the Kalman filter with the robot's position and orientation from vision. * * @param position Vision reading of the robot's position in world space - * @param velocity Vision reading of the robot's velocity in world space * @param orientation Vision reading of the robot's orientation in world space - * @param angular_velocity Vision reading of the robot's angular velocity */ - void updateFilterWithVision(const Point& position, const Vector& velocity, - const Angle& orientation, - const AngularVelocity& angular_velocity); + void updateFilterWithVision(const Point& position, const Angle& orientation); static constexpr size_t STATE_SIZE = reflective_enum::size(); static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index d379459801..eb54530bb5 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -242,17 +242,10 @@ void Thunderloop::runLoop() const Point position = createPoint(primitive_.move().xy_traj_params().start_position()); - const Vector velocity = createVector( - primitive_.move().xy_traj_params().initial_velocity()); - const Angle orientation = createAngle(primitive_.move().w_traj_params().start_angle()); - const AngularVelocity angular_velocity = createAngularVelocity( - primitive_.move().w_traj_params().initial_velocity()); - - robot_localizer_.updateVision(position, velocity, orientation, - angular_velocity, RTT_S / 2); + robot_localizer_.updateVision(position, orientation, RTT_S / 2); } // Update primitive executor's primitive set @@ -271,12 +264,8 @@ void Thunderloop::runLoop() } std::optional imu_poll = imu_service_->pollHeadingRate(); - if (imu_poll.has_value()) { - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"imu_ang_vel", imu_poll.value().toDegrees()}, - }); robot_localizer_.updateImu(imu_poll.value()); } @@ -289,19 +278,6 @@ void Thunderloop::runLoop() robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())); - // const Vector target_velocity = - // localToGlobalVelocity(createVector(status.target_local_velocity()), - // robot_localizer_.getOrientation()); - // const AngularVelocity target_angular_velocity = - // createAngularVelocity(status.target_angular_velocity()); - - // const Vector delta_velocity = target_velocity - last_target_velocity_; - // const AngularVelocity delta_angular_velocity = - // target_angular_velocity - last_target_angular_velocity_; - - // last_target_velocity_ = target_velocity; - // last_target_angular_velocity_ = target_angular_velocity; - robot_localizer_.step(Vector(), AngularVelocity::zero()); primitive_executor_.updateState(robot_localizer_.getPosition(), diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 0dc17fbab5..654bf77e44 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -173,9 +173,6 @@ class Thunderloop // Path to the CPU thermal zone temperature file const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; - - Vector last_target_velocity_; - AngularVelocity last_target_angular_velocity_; }; /* From dadcd2accdfd94f3d497a791f14fcba6b786bd10 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 17 May 2026 20:55:54 -0700 Subject: [PATCH 11/38] Revert temporary changes to sensor_fusion.cpp --- src/software/sensor_fusion/sensor_fusion.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index a35942b53d..42296afa2d 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -100,7 +100,6 @@ void SensorFusion::updateWorld(const SSLProto::SSL_WrapperPacket& packet) updateWorld(packet.detection()); - // LOG(WARNING) << "ROBOTS DETECTED: "<< packet.detection().robots_blue().size(); if (!ball && (packet.detection().robots_blue().size() != 0 || packet.detection().robots_yellow().size() != 0)) { @@ -365,7 +364,6 @@ Team SensorFusion::createFriendlyTeam(const std::vector& robot_d { Team new_friendly_team = friendly_team_filter.getFilteredData( friendly_team, robot_detections, friendly_robot_id_with_ball_in_dribbler); - return new_friendly_team; } From ab9e1184892025188846028adca169a613918486 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 17 May 2026 22:51:09 -0700 Subject: [PATCH 12/38] Delete empty robot_constants.cpp --- src/shared/robot_constants.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/shared/robot_constants.cpp diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp deleted file mode 100644 index e69de29bb2..0000000000 From d4b2a765a3aaed97268ba80e02c61284e2b1b677 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 18 May 2026 09:50:08 -0700 Subject: [PATCH 13/38] Changes to primitive executor --- src/software/embedded/primitive_executor.cpp | 33 +++----------------- src/software/embedded/primitive_executor.h | 21 ++++++++----- 2 files changed, 18 insertions(+), 36 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index c9d60b2926..c14d5d328f 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -13,8 +13,7 @@ PrimitiveExecutor::PrimitiveExecutor( const Duration time_step, const robot_constants::RobotConstants& robot_constants, const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), + : friendly_team_colour_(friendly_team_colour), robot_constants_(robot_constants), time_step_(time_step), robot_id_(robot_id) @@ -64,28 +63,24 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); - // Check if this new trajectory is "new". That is, if its destination differs - // meaningfully from our current trajectory. if (isLinearTrajectoryNew(new_trajectory_path)) { trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(RTT_S); x_pid.reset(); y_pid.reset(); - LOG(INFO) << "new linear trajectory accepted"; } const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), orientation_, angular_velocity_, robot_constants_); - LOG(INFO) << "angular has val " << angular_trajectory_.has_value(); + if (isAngularTrajectoryNew(new_angular_trajectory)) { angular_trajectory_ = new_angular_trajectory; time_since_angular_trajectory_creation_ = Duration::fromSeconds(RTT_S); w_pid.reset(); - LOG(INFO) << "new angular trajectory accepted"; } } } @@ -138,7 +133,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit } } -Vector PrimitiveExecutor::getTargetLinearVelocity() +Vector PrimitiveExecutor::getTargetLinearVelocity() const { Vector target_velocity = trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.toSeconds()); @@ -158,7 +153,7 @@ Vector PrimitiveExecutor::getTargetLinearVelocity() return target_velocity; } -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() +AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() const { AngularVelocity angular_velocity = angular_trajectory_->getVelocity( time_since_angular_trajectory_creation_.toSeconds()); @@ -229,10 +224,6 @@ std::unique_ptr PrimitiveExecutor::stepPrimi globalToLocalVelocity({x_pid_close.step(error_linear.x()), y_pid_close.step(error_linear.y())}, orientation_); - LOG(INFO) << "Y: close to target, doing pure PID with effort " - << target_linear_velocity.y(); - LOG(INFO) << "X: close to target, doing pure PID with effort " - << target_linear_velocity.x(); } else { @@ -243,21 +234,14 @@ std::unique_ptr PrimitiveExecutor::stepPrimi position_; const Vector pid_effort_linear(x_pid.step(error_linear.x()), y_pid.step(error_linear.y())); - LOG(INFO) << "Y: far from target, PID control effort: " - << pid_effort_linear.y() - << " FF: " << trajectory_linear_velocity.y(); - LOG(INFO) << "X: far from target, PID control effort: " - << pid_effort_linear.x() - << " FF: " << trajectory_linear_velocity.x(); target_linear_velocity = globalToLocalVelocity( trajectory_linear_velocity + pid_effort_linear, orientation_); } + if (error_angular.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) { target_angular_velocity = AngularVelocity::fromRadians( w_pid_close.step(error_angular.toRadians())); - LOG(INFO) << "AND: close to target, doing pure PID with effort: " - << target_angular_velocity.toDegrees(); } else { @@ -273,14 +257,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians())); target_angular_velocity = (trajectory_angular_velocity + pid_effort_angular); - LOG(INFO) << "AND: far from target, PID control effort: " - << pid_effort_angular.toDegrees() - << " FF: " << trajectory_angular_velocity.toDegrees(); } - LOG(INFO) << "Local velocity x " << target_linear_velocity.x() << " y " - << target_linear_velocity.y(); - // Make sure target linear velocity is clamped target_linear_velocity = target_linear_velocity.normalize( std::min(target_linear_velocity.length(), @@ -291,7 +269,6 @@ std::unique_ptr PrimitiveExecutor::stepPrimi convertDribblerModeToDribblerSpeed( current_primitive_.move().dribbler_mode(), robot_constants_), current_primitive_.move().auto_chip_or_kick()); - LOG(INFO) << "handle kmove end"; return std::make_unique( prim->direct_control()); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 1a777bcf4e..51b163e06d 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -55,13 +55,9 @@ class PrimitiveExecutor TbotsProto::PrimitiveExecutorStatus& status); private: - /* - * Compute the next target linear _local_ velocity the robot should be at. - * @returns Vector The target linear _local_ velocity - */ - Vector getTargetLinearVelocity(); - /** + * Check if the given trajectory is "new". That is, if its destination differs + * meaningfully from our current trajectory. * * @param new_trajectory The new trajectory requested by the AI. * @return True if the new trajectory requested is meaningfully different from the @@ -70,6 +66,8 @@ class PrimitiveExecutor bool isLinearTrajectoryNew(const std::optional& new_trajectory) const; /** + * Check if the given trajectory is "new". That is, if its destination differs + * meaningfully from our current trajectory. * * @param new_trajectory The new trajectory requested by the AI. * @return True if the new trajectory requested is meaningfully different from the @@ -77,12 +75,19 @@ class PrimitiveExecutor */ bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const; + /** + * Compute the next target linear _local_ velocity the robot should be at. + * + * @returns Vector The target linear _local_ velocity + */ + Vector getTargetLinearVelocity() const; + /* - * Returns the next target angular velocity the robot + * Returns the next target angular velocity the robot should be at. * * @returns AngularVelocity The target angular velocity */ - AngularVelocity getTargetAngularVelocity(); + AngularVelocity getTargetAngularVelocity() const; TbotsProto::Primitive current_primitive_; From fd921eecfa0873325ff09006a9eb0fa66ab8e1e3 Mon Sep 17 00:00:00 2001 From: sunghyuneun Date: Thu, 21 May 2026 18:29:43 -0700 Subject: [PATCH 14/38] Improved Euclidean to Wheel Readability / Logic? + Fixed Tests --- src/software/physics/euclidean_to_wheel.cpp | 44 +++---- src/software/physics/euclidean_to_wheel.h | 2 - .../physics/euclidean_to_wheel_test.cpp | 117 +++++++++++------- 3 files changed, 98 insertions(+), 65 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 7bacff40f4..46309c4fa7 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -8,7 +8,10 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -#ifdef DEBUG_WHEEL +// get rid of this stupid ifdef, and then look at the issue + +// Turn this to +#if CHECK_VERSION(2026) EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) : robot_constants_(robot_constants) { @@ -16,31 +19,29 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.; auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.; - auto cos_p = std::cos(p); - auto sin_p = std::sin(p); - auto cos_t = std::cos(t); - auto sin_t = std::sin(t); + // Beta is angle of wheel rolling direction relative to X-axis + // LOOK AT (software)/src/shared/robot_constants.h to see X-axis + double b_fr = -(M_PI - p); + double b_fl = -p; + double b_bl = t; + double b_br = (M_PI - t); - // 1. Physical wheel coordinates in meters - // Mapped to the robot frame: +X = Right, +Y = Forward - double fr_x = 0.06632, fr_y = 0.03485; - double br_x = 0.05592, br_y = -0.04985; + // Mapped to the robot frame: +X = Forward, +Y = Left + double fr_x = 0.03485, fr_y = -0.06632; + double fl_x = 0.03485, fl_y = 0.06632; + double bl_x = -0.04985, bl_y = 0.05592; + double br_x = -0.04985, br_y = -0.05592; - // 2. Unit drive vectors (extracted directly from the matrix's linear columns) - double fr_dx = -sin_p, fr_dy = cos_p; - double br_dx = sin_t, br_dy = cos_t; - // 3. Dynamically calculate the rotational lever arms using the 2D cross product - // Lever Arm = | X * Dy - Y * Dx | - double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx)); - double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx)); + // Assuming that CCW when looking end of shaft into motor is the positive direction. + // Formula is u_1 = v_x * cos(B_1) + v_y * sin(B_1) + W * (x_1 * sin(B_1) - y_1 * cos(B_1)) // clang-format off euclidean_to_wheel_velocity_D_ << - -sin_p, cos_p, rot_f, - -sin_p, -cos_p, rot_f, - sin_t, -cos_t, rot_b, - sin_t, cos_t, rot_b; + std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), + std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), + std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), + std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br)); // clang-format on // Calculate Pseudo-inverse dynamically @@ -102,7 +103,8 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ #endif -#ifdef DEBUG_WHEEL +// Why was this DEBUG_WHEEL +#if CHECK_VERSION(2026) WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const { // The generalized D matrix natively handles the w -> tangential velocity diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index a20f723159..b1fe981abe 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -7,8 +7,6 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -#define DEBUG_WHEEL - /** * Vector representation of 2D Euclidean space. * diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index 1cd3024560..556a22d764 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -40,74 +40,104 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero) // velocity, and vise-versa. TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_x) { - // Test +x/right + // Test +x/forward target_euclidean_velocity = {1, 0, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Front wheels must be - velocity, back wheels must be + velocity. + // Right wheels must be - velocity, left wheels must be + velocity. EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + + + // Right wheels must have same velocity magnitude as left wheels, but opposite sign. + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x) { - // Test -x/left + // Test -x/backward target_euclidean_velocity = {-1, 0, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Front wheels must be + velocity, back wheels must be - velocity. + // Right wheels must be + velocity, left wheels must be - velocity. EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + + // Right wheels must have same velocity magnitude as left wheels, but opposite sign. + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_y) { - // Test +y/forwards + // Test +y/left target_euclidean_velocity = {0, 1, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + // Back wheels must be + velocity, front wheels must be - velocity. + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); - - // Right wheels must have same velocity magnitude as left wheels, but opposite sign. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) { - // Test -y/backwards + // Test -y/right target_euclidean_velocity = {0, -1, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + // Back wheels must be - velocity, front wheels must be + velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); +} + +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) +{ + // Test w / counter-clockwise + target_euclidean_velocity = {0, 0, 1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + // Right wheels must be + velocity, Left wheels must be - velocity. EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); +} - // Right wheels must have same velocity magnitude as left wheels, but opposite sign. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) +{ + // Test w / clockwise + target_euclidean_velocity = {0, 0, -1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + + // Right wheels must be + velocity, Left wheels must be - velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } -#ifdef DEBUG_WHEEL -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) +// #ifdef DEBUG_WHEEL +#if CHECK_VERSION(2026) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) { // Test +w/counter-clockwise target_euclidean_velocity = {0, 0, 1}; @@ -116,20 +146,23 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) // Because the wheels are offset, their speed at 1 rad/s equals their // exact physical lever arm (in meters), NOT the nominal robot_radius. - // Based on the physical offsets, the lever arm evaluates to 0.0746 meters. - double expected_lever_arm = 0.0746; - EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + // i have no idea what it's yapping about above but i ignored it lol + + // Based on the physical offsets, the lever arm evaluates to 0.0749 meters. + double expected_lever_arm = 0.0749; + + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); } -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude) { // Test -w/clockwise target_euclidean_velocity = {0, 0, -1}; @@ -138,20 +171,20 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) // Because the wheels are offset, their speed at -1 rad/s equals their // exact physical lever arm (in meters) multiplied by the negative velocity. - double expected_lever_arm = -0.0746; + double expected_lever_arm = 0.0749; - EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); - EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, 0.001); } #else -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) { // Test +w/counter-clockwise target_euclidean_velocity = {0, 0, 1}; @@ -169,7 +202,7 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) } -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude) { // Test -w/clockwise target_euclidean_velocity = {0, 0, -1}; From f86b25d3c0f49900dc23f2d48148ba41ecaabfb6 Mon Sep 17 00:00:00 2001 From: sunghyuneun Date: Thu, 21 May 2026 20:27:43 -0700 Subject: [PATCH 15/38] Revert "Improved Euclidean to Wheel Readability / Logic? + Fixed Tests" Moved changes to PR #3731 This reverts commit 6aec7b3558653c3d67633e02e7527ea594ccf2fa. --- src/software/physics/euclidean_to_wheel.cpp | 44 ++++--- src/software/physics/euclidean_to_wheel.h | 2 + .../physics/euclidean_to_wheel_test.cpp | 117 +++++++----------- 3 files changed, 65 insertions(+), 98 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 46309c4fa7..7bacff40f4 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -8,10 +8,7 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" -// get rid of this stupid ifdef, and then look at the issue - -// Turn this to -#if CHECK_VERSION(2026) +#ifdef DEBUG_WHEEL EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) : robot_constants_(robot_constants) { @@ -19,29 +16,31 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.; auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.; - // Beta is angle of wheel rolling direction relative to X-axis - // LOOK AT (software)/src/shared/robot_constants.h to see X-axis - double b_fr = -(M_PI - p); - double b_fl = -p; - double b_bl = t; - double b_br = (M_PI - t); + auto cos_p = std::cos(p); + auto sin_p = std::sin(p); + auto cos_t = std::cos(t); + auto sin_t = std::sin(t); - // Mapped to the robot frame: +X = Forward, +Y = Left - double fr_x = 0.03485, fr_y = -0.06632; - double fl_x = 0.03485, fl_y = 0.06632; - double bl_x = -0.04985, bl_y = 0.05592; - double br_x = -0.04985, br_y = -0.05592; + // 1. Physical wheel coordinates in meters + // Mapped to the robot frame: +X = Right, +Y = Forward + double fr_x = 0.06632, fr_y = 0.03485; + double br_x = 0.05592, br_y = -0.04985; + // 2. Unit drive vectors (extracted directly from the matrix's linear columns) + double fr_dx = -sin_p, fr_dy = cos_p; + double br_dx = sin_t, br_dy = cos_t; - // Assuming that CCW when looking end of shaft into motor is the positive direction. - // Formula is u_1 = v_x * cos(B_1) + v_y * sin(B_1) + W * (x_1 * sin(B_1) - y_1 * cos(B_1)) + // 3. Dynamically calculate the rotational lever arms using the 2D cross product + // Lever Arm = | X * Dy - Y * Dx | + double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx)); + double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx)); // clang-format off euclidean_to_wheel_velocity_D_ << - std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), - std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), - std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), - std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br)); + -sin_p, cos_p, rot_f, + -sin_p, -cos_p, rot_f, + sin_t, -cos_t, rot_b, + sin_t, cos_t, rot_b; // clang-format on // Calculate Pseudo-inverse dynamically @@ -103,8 +102,7 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ #endif -// Why was this DEBUG_WHEEL -#if CHECK_VERSION(2026) +#ifdef DEBUG_WHEEL WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const { // The generalized D matrix natively handles the w -> tangential velocity diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index b1fe981abe..a20f723159 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -7,6 +7,8 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" +#define DEBUG_WHEEL + /** * Vector representation of 2D Euclidean space. * diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index 556a22d764..1cd3024560 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -40,104 +40,74 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero) // velocity, and vise-versa. TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_x) { - // Test +x/forward + // Test +x/right target_euclidean_velocity = {1, 0, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be - velocity, left wheels must be + velocity. + // Front wheels must be - velocity, back wheels must be + velocity. EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); - - - // Right wheels must have same velocity magnitude as left wheels, but opposite sign. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x) { - // Test -x/backward + // Test -x/left target_euclidean_velocity = {-1, 0, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, left wheels must be - velocity. + // Front wheels must be + velocity, back wheels must be - velocity. EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); - - // Right wheels must have same velocity magnitude as left wheels, but opposite sign. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], - -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_y) { - // Test +y/left + // Test +y/forwards target_euclidean_velocity = {0, 1, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be + velocity, front wheels must be - velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be + velocity, Left wheels must be - velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + + // Right wheels must have same velocity magnitude as left wheels, but opposite sign. + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) { - // Test -y/right + // Test -y/backwards target_euclidean_velocity = {0, -1, 0}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be - velocity, front wheels must be + velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); -} - -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) -{ - // Test w / counter-clockwise - target_euclidean_velocity = {0, 0, 1}; - calculated_wheel_speeds = - euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, Left wheels must be - velocity. EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); -} - -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) -{ - // Test w / clockwise - target_euclidean_velocity = {0, 0, -1}; - calculated_wheel_speeds = - euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - - // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + + // Right wheels must have same velocity magnitude as left wheels, but opposite sign. + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]); + EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], + -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } -// #ifdef DEBUG_WHEEL -#if CHECK_VERSION(2026) -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) +#ifdef DEBUG_WHEEL +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) { // Test +w/counter-clockwise target_euclidean_velocity = {0, 0, 1}; @@ -146,23 +116,20 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) // Because the wheels are offset, their speed at 1 rad/s equals their // exact physical lever arm (in meters), NOT the nominal robot_radius. + // Based on the physical offsets, the lever arm evaluates to 0.0746 meters. + double expected_lever_arm = 0.0746; - // i have no idea what it's yapping about above but i ignored it lol - - // Based on the physical offsets, the lever arm evaluates to 0.0749 meters. - double expected_lever_arm = 0.0749; - - EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); } -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) { // Test -w/clockwise target_euclidean_velocity = {0, 0, -1}; @@ -171,20 +138,20 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude) // Because the wheels are offset, their speed at -1 rad/s equals their // exact physical lever arm (in meters) multiplied by the negative velocity. - double expected_lever_arm = 0.0749; + double expected_lever_arm = -0.0746; - EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); - EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm, + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, 0.001); } #else -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) { // Test +w/counter-clockwise target_euclidean_velocity = {0, 0, 1}; @@ -202,7 +169,7 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) } -TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude) +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) { // Test -w/clockwise target_euclidean_velocity = {0, 0, -1}; From a874142166f2f998084e874e787dff094f8b5d6d Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 11:38:35 -0700 Subject: [PATCH 16/38] initialize velocity related stuff --- src/software/embedded/services/imu.cpp | 9 +++++++++ src/software/embedded/services/imu.h | 11 +++++++++++ src/software/embedded/thunderloop.h | 6 ++++++ 3 files changed, 26 insertions(+) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index b4dc6c7af6..dba8002fae 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -7,6 +7,7 @@ #include "imu.h" #include "shared/constants.h" #include "software/logger/logger.h" +#include "software/geom/angular_acceleration.h" // these functions taken from // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c @@ -164,6 +165,14 @@ std::optional ImuService::pollHeadingVelocity() return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); } +std::optional ImuService::pollHeadingAcceleration(std::optional prev_angular_velocity, double prev_time){ + if (!prev_angular_velocity.has_value()){ + return std::nullopt; + } + + + +} std::optional ImuService::pollLinearAcceleration() { if (!initialized_) diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index d234a05335..41eef0aba9 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -20,6 +20,11 @@ class ImuService * @return the current angular velocty of the robot on the z axis */ std::optional pollHeadingVelocity(); + /* + * Polls the latest IMU reading of the linear acceleration of the robot on the z plane + * @return the current linear acceleration of the robot on the z plane + */ + std::optional pollHeadingAcceleration(); /* * Polls the latest IMU reading of the linear acceleration of the robot on the z plane * @return the current linear acceleration of the robot on the z plane @@ -37,6 +42,7 @@ class ImuService bool initialized_=true; int file_descriptor_=0; doubel degrees_error_; + // Maps the maximum raw reading from 16-bit integer to be 2 times gravity static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; // Same for gyroscope, 1000 degrees per second @@ -50,6 +56,11 @@ class ImuService static const uint8_t CTRL6_C = 0x15; static const uint8_t CTRL8_XL = 0x17; + // Deviation from center of mass + double x_deviation; + double y_deviation; + + // Device path for the IMU inline static const std::string IMU_DEVICE = "/dev/i2c-1"; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 7a524a2cbc..e86dd2157a 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -16,6 +16,7 @@ #include "software/embedded/services/imu.h" #include "software/embedded/toml_config/toml_config_client.h" #include "software/logger/logger.h" +#include "software/geom/angular_velocity.h" class Thunderloop { @@ -119,6 +120,7 @@ class Thunderloop */ TbotsProto::PowerStatus pollPowerService(struct timespec& poll_time); + /** * Wait for networking communication to be established. This function is blocking. */ @@ -153,6 +155,10 @@ class Thunderloop int kick_constant_; int chip_pulse_width_; + // Store previous angular velocity for angular accleration calculations + std::optional prev_angular_velocity; + double prev_angular_velocity_time; + // Primitive Executor PrimitiveExecutor primitive_executor_; From fad037620d94e430e6fdedc6ffe1d956c597397a Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 13:00:10 -0700 Subject: [PATCH 17/38] remove leftover --- src/software/embedded/services/imu.cpp | 16 ++++------- src/software/embedded/services/imu.h | 32 ++++++++++++--------- src/software/embedded/services/services.cpp | 0 src/software/embedded/services/services.h | 0 4 files changed, 24 insertions(+), 24 deletions(-) create mode 100644 src/software/embedded/services/services.cpp create mode 100644 src/software/embedded/services/services.h diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index dba8002fae..36646935c8 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -7,7 +7,6 @@ #include "imu.h" #include "shared/constants.h" #include "software/logger/logger.h" -#include "software/geom/angular_acceleration.h" // these functions taken from // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c @@ -128,7 +127,7 @@ ImuService::ImuService() : initialized_(false) } } -std::optional ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg) +std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) { int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); int most_significant = i2c_smbus_read_byte_data(file_descriptor_, ms_reg); @@ -151,7 +150,7 @@ std::optional ImuService::pollHeadingVelocity() return std::nullopt; } - std::optional opt_full_word = combineBits(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG); + std::optional opt_full_word = readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG); if (!opt_full_word.has_value()) { @@ -165,14 +164,9 @@ std::optional ImuService::pollHeadingVelocity() return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); } -std::optional ImuService::pollHeadingAcceleration(std::optional prev_angular_velocity, double prev_time){ - if (!prev_angular_velocity.has_value()){ - return std::nullopt; - } - -} + std::optional ImuService::pollLinearAcceleration() { if (!initialized_) @@ -180,8 +174,8 @@ std::optional ImuService::pollLinearAcceleration() return std::nullopt; } - std::optional opt_x = combineBits(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); - std::optional opt_y = combineBits(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); + std::optional opt_x = readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional opt_y = readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); if (!opt_x.has_value() || !opt_y.has_value()) { diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 41eef0aba9..76afb603db 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -14,17 +14,17 @@ class ImuService { public: + /** + * Constructs and initializes a new IMU service object. + * + * If successfully initialized, will try to do a simple calibration of the IMU. + */ ImuService(); /* * Polls the latest IMU reading of the angular velocity of the robot on the z axis * @return the current angular velocty of the robot on the z axis */ std::optional pollHeadingVelocity(); - /* - * Polls the latest IMU reading of the linear acceleration of the robot on the z plane - * @return the current linear acceleration of the robot on the z plane - */ - std::optional pollHeadingAcceleration(); /* * Polls the latest IMU reading of the linear acceleration of the robot on the z plane * @return the current linear acceleration of the robot on the z plane @@ -38,10 +38,21 @@ class ImuService (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); private: - std::optional combineBits(); - bool initialized_=true; + + /* + * reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + + std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); + + bool initialized_=false; + int file_descriptor_=0; - doubel degrees_error_; + + double degrees_error_; // Maps the maximum raw reading from 16-bit integer to be 2 times gravity static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; @@ -56,11 +67,6 @@ class ImuService static const uint8_t CTRL6_C = 0x15; static const uint8_t CTRL8_XL = 0x17; - // Deviation from center of mass - double x_deviation; - double y_deviation; - - // Device path for the IMU inline static const std::string IMU_DEVICE = "/dev/i2c-1"; diff --git a/src/software/embedded/services/services.cpp b/src/software/embedded/services/services.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/software/embedded/services/services.h b/src/software/embedded/services/services.h new file mode 100644 index 0000000000..e69de29bb2 From 4d9e8cac40131411234f96c3852aef107e2a5a01 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 13:01:48 -0700 Subject: [PATCH 18/38] fix thunderloop --- src/software/embedded/thunderloop.cpp | 3 +++ src/software/embedded/thunderloop.h | 1 + 2 files changed, 4 insertions(+) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 02a08c090d..96b351206a 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -182,10 +182,13 @@ void Thunderloop::runLoop() std::optional imu_poll = imu_service_->pollHeadingVelocity(); + + //TODO: Replace with actual IMU data usage if (imu_poll.has_value()){ LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ; } + for (;;) { struct timespec time_since_prev_iter; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index e86dd2157a..0f7df7d902 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -62,6 +62,7 @@ class Thunderloop std::unique_ptr motor_service_; std::unique_ptr network_service_; std::unique_ptr power_service_; + std::unique_ptr imu_service_; // TOML config client std::unique_ptr toml_config_client_; From cfa79b920ed8ab683ea6a33f45b6473c6079e984 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 13:03:58 -0700 Subject: [PATCH 19/38] cleanup --- src/software/embedded/services/imu.h | 2 +- src/software/embedded/services/services.cpp | 0 src/software/embedded/services/services.h | 0 src/software/embedded/thunderloop.h | 5 ----- 4 files changed, 1 insertion(+), 6 deletions(-) delete mode 100644 src/software/embedded/services/services.cpp delete mode 100644 src/software/embedded/services/services.h diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 76afb603db..da062ec76f 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -40,7 +40,7 @@ class ImuService private: /* - * reads byte data from two registers, and combine them into a single value + * Reads byte data from two registers, and combine them into a single value * @parama ls_reg register of the least significant register * @parama ms_reg register of the most significant register * @return the combined integer value of the two registers diff --git a/src/software/embedded/services/services.cpp b/src/software/embedded/services/services.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/software/embedded/services/services.h b/src/software/embedded/services/services.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 0f7df7d902..b3da73b76c 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -16,7 +16,6 @@ #include "software/embedded/services/imu.h" #include "software/embedded/toml_config/toml_config_client.h" #include "software/logger/logger.h" -#include "software/geom/angular_velocity.h" class Thunderloop { @@ -156,10 +155,6 @@ class Thunderloop int kick_constant_; int chip_pulse_width_; - // Store previous angular velocity for angular accleration calculations - std::optional prev_angular_velocity; - double prev_angular_velocity_time; - // Primitive Executor PrimitiveExecutor primitive_executor_; From 3442db3693337015c7a0a34935815c977f3279c3 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 13:15:53 -0700 Subject: [PATCH 20/38] fix bug --- src/software/embedded/thunderloop.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 96b351206a..faa7e5e415 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -120,7 +120,9 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; motor_service_ = std::make_unique(robot_constants, loop_hz); - g_motor_service = motor_service_.get(); + g_motor_service = motor_service_.get(); + motor_service_->setup(); + mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; imu_service_ = std::make_unique(); From 82aa4af49767f78e14565cf444aa524b6718b324 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 13:49:01 -0700 Subject: [PATCH 21/38] fix build --- src/software/embedded/BUILD | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..785335c27e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -29,6 +29,7 @@ cc_library( "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", + "//software/embedded/services/imu", "//software/embedded/toml_config", "//software/logger:network_logger", "//software/tracy:tracy_constants", From 8add6992dd951730341a191c9564d9df3b353abe Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 14:08:36 -0700 Subject: [PATCH 22/38] add angular acceleration method --- src/software/embedded/BUILD | 1 + src/software/embedded/services/imu.cpp | 29 ++++++++++++++++++++++++++ src/software/embedded/services/imu.h | 13 +++++++++++- 3 files changed, 42 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..785335c27e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -29,6 +29,7 @@ cc_library( "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", + "//software/embedded/services/imu", "//software/embedded/toml_config", "//software/logger:network_logger", "//software/tracy:tracy_constants", diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index 36646935c8..65d404c9d9 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -161,9 +161,38 @@ std::optional ImuService::pollHeadingVelocity() double degrees_per_sec = static_cast(full_word) / static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); } + +std::optional ImuService::pollHeadingAcceleration() +{ + if (!initialized_) + { + return std::nullopt; + } + + if(!prev_angular_velocity_.has_value()){ + prev_angular_velocity_ = pollHeadingVelocity(); + return std::nullopt; + } + + auto curr_time = std::chrono::steady_clock::now(); + auto curr_angular_velocity = pollHeadingVelocity(); + + double dt = std::chrono::duration(curr_time - prev_time_).count(); + if (dt <= 0 || !curr_angular_velocity.has_value()) return std::nullopt; + + double alpha = (curr_angular_velocity->toRadians() - + prev_angular_velocity_->toRadians()) / dt; + + // store current time and angular velocity as previous + prev_angular_velocity_ = curr_angular_velocity; + prev_time_ = curr_time; + + return AngularAcceleration::fromRadians(alpha); +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index da062ec76f..6a102fa913 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -7,6 +7,7 @@ #include "proto/tbots_software_msgs.pb.h" #include "software/geom/angular_velocity.h" +#include "software/geom/angular_acceleration.h" /** * Handles low level IMU I2C communication, and some minor offset filtering. @@ -25,6 +26,11 @@ class ImuService * @return the current angular velocty of the robot on the z axis */ std::optional pollHeadingVelocity(); + /* + * Computes angular acceleration from successive angular velocity readings + * @return the current angular acceleration of the robot on the z axis + */ + std::optional pollHeadingAcceleration(); /* * Polls the latest IMU reading of the linear acceleration of the robot on the z plane * @return the current linear acceleration of the robot on the z plane @@ -81,4 +87,9 @@ class ImuService // Accelerometer Y-axis Output Data Registers static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL -} + + // prev time and angular accleration + std::optional prev_angular_velocity_; + std::chrono::steady_clock::time_point prev_time_; + +}; From b638f35333a6c8caa5161557def1f2b76a61aa22 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 14:10:21 -0700 Subject: [PATCH 23/38] fix BUILD --- src/software/embedded/services/BUILD | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index f763e4f8dc..69c2301f99 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -31,6 +31,19 @@ cc_library( ], ) +cc_library( + name = "imu", + srcs = ["imu.cpp"], + hdrs = ["imu.h"], + deps = [ + "//proto:tbots_cc_proto", + "//shared:constants", + "//software/geom:angular_velocity", + "//software/logger", + "@eigen", + ], +) + cc_binary( name = "robot_auto_test", srcs = ["robot_auto_test.cpp"], From e7d12d3ed0c581691a426e47c6cfd84fc4742a11 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 14:12:02 -0700 Subject: [PATCH 24/38] fix bugs --- src/software/embedded/BUILD | 2 +- src/software/embedded/services/imu.h | 2 +- src/software/embedded/thunderloop.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 785335c27e..b0321b44a9 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -29,7 +29,7 @@ cc_library( "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", - "//software/embedded/services/imu", + "//software/embedded/services:imu", "//software/embedded/toml_config", "//software/logger:network_logger", "//software/tracy:tracy_constants", diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index da062ec76f..f3c49515e7 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -81,4 +81,4 @@ class ImuService // Accelerometer Y-axis Output Data Registers static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL -} +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index faa7e5e415..652833a3f0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -123,10 +123,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, g_motor_service = motor_service_.get(); motor_service_->setup(); - mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; + LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; imu_service_ = std::make_unique(); - LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";otor_service_->setup(); + LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ << ", CHANNEL ID: " << channel_id_ @@ -183,7 +183,7 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - std::optional imu_poll = imu_service_->pollHeadingVelocity(); + std::optional imu_poll = imu_service_->pollHeadingVelocity(); //TODO: Replace with actual IMU data usage if (imu_poll.has_value()){ From 6df88379034ea0bff37546698f0ff270a799e6e5 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 14:53:18 -0700 Subject: [PATCH 25/38] unify poll function and introduce struct --- src/software/embedded/services/imu.cpp | 29 +++++++++++++------ src/software/embedded/services/imu.h | 40 +++++++++++++++++--------- 2 files changed, 47 insertions(+), 22 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index 65d404c9d9..9667126519 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -105,7 +105,7 @@ ImuService::ImuService() : initialized_(false) for (int i = 0; i < 100; i++) { // Fixed: Call the actual implemented function name - auto poll = pollHeadingVelocity(); + auto poll = pollAngularVelocity(); if (poll.has_value()) { sum += poll->toDegrees(); @@ -121,12 +121,25 @@ ImuService::ImuService() : initialized_(false) } else { - LOG(WARNING) << "IMU Calibration failed: no valid samples received. Heading " + LOG(WARNING) << "IMU Calibration failed: no valid samples received. Angular " "stability will be poor."; initialized_ = false; } } +std::optional ImuService::poll(){ + + std::optional AngularVelocity = pollAngularVelocity(); + std::optional AngularAceleration = pollAngularAcceleration(); + std::optional LinearAcceleration = pollLinearAcceleration(); + + if (!AngularVelocity.has_value() || !AngularAcceleration.has_value() || !LinearAcceleration.has_value()){ + return std::nullopt; + } + + return {AngularVelocity.value(), AngularAcceleration.value(), LinearAcceleration.value()}; + +} std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) { int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); @@ -143,14 +156,14 @@ std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_ return static_cast(combined); } -std::optional ImuService::pollHeadingVelocity() +std::optional ImuService::pollAngularVelocity() { if (!initialized_) { return std::nullopt; } - std::optional opt_full_word = readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG); + std::optional opt_full_word = readAndCombineByteData(GYRO_LEAST_SIG_REG, GYRO_MOST_SIG_REG); if (!opt_full_word.has_value()) { @@ -166,7 +179,7 @@ std::optional ImuService::pollHeadingVelocity() } -std::optional ImuService::pollHeadingAcceleration() +std::optional ImuService::pollAngularAcceleration() { if (!initialized_) { @@ -174,12 +187,13 @@ std::optional ImuService::pollHeadingAcceleration() } if(!prev_angular_velocity_.has_value()){ - prev_angular_velocity_ = pollHeadingVelocity(); + prev_angular_velocity_ = pollAngularVelocity(); + prev_time_ = std::chrono::steady_clock::now();; return std::nullopt; } auto curr_time = std::chrono::steady_clock::now(); - auto curr_angular_velocity = pollHeadingVelocity(); + auto curr_angular_velocity = pollAngularVelocity(); double dt = std::chrono::duration(curr_time - prev_time_).count(); if (dt <= 0 || !curr_angular_velocity.has_value()) return std::nullopt; @@ -187,7 +201,6 @@ std::optional ImuService::pollHeadingAcceleration() double alpha = (curr_angular_velocity->toRadians() - prev_angular_velocity_->toRadians()) / dt; - // store current time and angular velocity as previous prev_angular_velocity_ = curr_angular_velocity; prev_time_ = curr_time; diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 6a102fa913..f01c894053 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -12,6 +12,13 @@ /** * Handles low level IMU I2C communication, and some minor offset filtering. */ +struct ImuData{ + + AngularVelocity AngularVelocity; + AngularAcceleration AngularAceleration; + Eigen::Vector2d LinearAcceleration; + +}; class ImuService { public: @@ -21,30 +28,31 @@ class ImuService * If successfully initialized, will try to do a simple calibration of the IMU. */ ImuService(); + + std::optional poll(); + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) * + (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); + + private: + /* * Polls the latest IMU reading of the angular velocity of the robot on the z axis * @return the current angular velocty of the robot on the z axis */ - std::optional pollHeadingVelocity(); + std::optional pollAngularVelocity(); /* * Computes angular acceleration from successive angular velocity readings * @return the current angular acceleration of the robot on the z axis */ - std::optional pollHeadingAcceleration(); + std::optional pollAngularAcceleration(); /* * Polls the latest IMU reading of the linear acceleration of the robot on the z plane * @return the current linear acceleration of the robot on the z plane */ std::optional pollLinearAcceleration(); - - - // Variance from datasheet (in rad^2/s^2) - static constexpr double IMU_VARIANCE = - (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) * - (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); - - private: - /* * Reads byte data from two registers, and combine them into a single value * @parama ls_reg register of the least significant register @@ -77,8 +85,8 @@ class ImuService inline static const std::string IMU_DEVICE = "/dev/i2c-1"; // Gyroscope Z-axis (Yaw) Output Data Registers - static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G - static constexpr uint8_t HEADING_MOST_SIG_REG = 0x27; // OUTZ_H_G + static constexpr uint8_t GYRO_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t GYRO_MOST_SIG_REG = 0x27; // OUTZ_H_G // Accelerometer X-axis Output Data Registers static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL @@ -87,7 +95,11 @@ class ImuService // Accelerometer Y-axis Output Data Registers static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL - + + // IMU offset for acceleration calculation + static constexpr double IMU_OFFSET_X = 0.0; + static constexpr double IMU_OFFSET_Y = 0.0; + // prev time and angular accleration std::optional prev_angular_velocity_; std::chrono::steady_clock::time_point prev_time_; From b8a995e7737701519fc0f9f3dc2cab1f07b9226c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 15:14:59 -0700 Subject: [PATCH 26/38] finish transform linear acceleration and fix bugs --- src/software/embedded/services/BUILD | 1 + src/software/embedded/services/imu.cpp | 33 ++++++++++++++++++++------ src/software/embedded/services/imu.h | 21 +++++++++------- src/software/embedded/thunderloop.cpp | 7 +++--- 4 files changed, 43 insertions(+), 19 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 69c2301f99..6ff60c757d 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -39,6 +39,7 @@ cc_library( "//proto:tbots_cc_proto", "//shared:constants", "//software/geom:angular_velocity", + "//software/geom:angular_acceleration", "//software/logger", "@eigen", ], diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index 9667126519..4735ad064c 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -128,16 +128,16 @@ ImuService::ImuService() : initialized_(false) } std::optional ImuService::poll(){ - - std::optional AngularVelocity = pollAngularVelocity(); - std::optional AngularAceleration = pollAngularAcceleration(); - std::optional LinearAcceleration = pollLinearAcceleration(); - if (!AngularVelocity.has_value() || !AngularAcceleration.has_value() || !LinearAcceleration.has_value()){ + std::optional angular_velocity = pollAngularVelocity(); + std::optional angular_acceleration = pollAngularAcceleration(); + std::optional linear_acceleration = pollLinearAcceleration(); + + if (!angular_velocity.has_value() || !angular_acceleration.has_value() || !linear_acceleration.has_value()){ return std::nullopt; } - return {AngularVelocity.value(), AngularAcceleration.value(), LinearAcceleration.value()}; + return ImuData{angular_velocity.value(), angular_acceleration.value(), linear_acceleration.value()}; } std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) @@ -175,7 +175,7 @@ std::optional ImuService::pollAngularVelocity() double degrees_per_sec = static_cast(full_word) / static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; - return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); + return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_)*3.1415/180); } @@ -234,6 +234,25 @@ std::optional ImuService::pollLinearAcceleration() double a_y = (static_cast(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + // Transform using equations return Eigen::Vector2d(a_x, a_y); } + +std::optional ImuService::transformLinearAcceleration( + AngularVelocity omega, AngularAcceleration alpha, Eigen::Vector2d a_imu) +{ + Eigen::Vector2d r(IMU_OFFSET_X, IMU_OFFSET_Y); + + double w = omega.toRadians(); + double a = alpha.toRadians(); + + // tangential: alpha x r → (-alpha*ry, alpha*rx) + Eigen::Vector2d tangential(-a * r.y(), a * r.x()); + + // centripetal: omega^2 * r + Eigen::Vector2d centripetal = (w * w) * r; + + return a_imu + tangential - centripetal; +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index f01c894053..c9da597953 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -13,11 +14,9 @@ * Handles low level IMU I2C communication, and some minor offset filtering. */ struct ImuData{ - - AngularVelocity AngularVelocity; - AngularAcceleration AngularAceleration; - Eigen::Vector2d LinearAcceleration; - + AngularVelocity angular_velocity; + AngularAcceleration angular_acceleration; + Eigen::Vector2d linear_acceleration; }; class ImuService { @@ -33,8 +32,8 @@ class ImuService // Variance from datasheet (in rad^2/s^2) static constexpr double IMU_VARIANCE = - (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) * - (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0); + (4.0 * 14.4222 / 1000.0 * 3.1415 / 180.0) * + (4.0 * 14.4222 / 1000.0 * 3.1415 / 180.0); private: @@ -59,8 +58,14 @@ class ImuService * @parama ms_reg register of the most significant register * @return the combined integer value of the two registers */ - std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); + /* + * Reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + std::optional transformLinearAcceleration(AngularVelocity omega, AngularAceleration alpha, LinearAcceleration a); bool initialized_=false; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 652833a3f0..56f9047392 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -183,12 +183,11 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - std::optional imu_poll = imu_service_->pollHeadingVelocity(); - + std::optional imu_poll = imu_service_->poll(); + //TODO: Replace with actual IMU data usage if (imu_poll.has_value()){ - - LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ; + LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity.toRadians(); } for (;;) From a8397c658c33bfc03b770083a56169fb6b1cf485 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 15:50:01 -0700 Subject: [PATCH 27/38] cleanup --- src/shared/constants.h | 2 +- src/software/embedded/services/imu.cpp | 17 +++++++++-------- src/software/embedded/services/imu.h | 8 ++++---- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index ed94c0cde9..b3557dda6d 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -1,5 +1,5 @@ #pragma once -#include +#include // Some platformio targets don't support STL, so we can't // use unordered_map, string, .... We guard all networking stuff with diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index 4735ad064c..d2c32cd68d 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -130,14 +130,16 @@ ImuService::ImuService() : initialized_(false) std::optional ImuService::poll(){ std::optional angular_velocity = pollAngularVelocity(); - std::optional angular_acceleration = pollAngularAcceleration(); - std::optional linear_acceleration = pollLinearAcceleration(); + std::optional angular_acceleration = pollAngularAcceleration(angular_velocity); + std::optional imu_linear_acceleration = pollLinearAcceleration(); - if (!angular_velocity.has_value() || !angular_acceleration.has_value() || !linear_acceleration.has_value()){ + if (!angular_velocity.has_value() || !angular_acceleration.has_value() || !imu_linear_acceleration.has_value()){ return std::nullopt; } - return ImuData{angular_velocity.value(), angular_acceleration.value(), linear_acceleration.value()}; + Eigen::Vector2d linear_acceleration = transformLinearAcceleration(angular_velocity.value(), angular_acceleration.value(), imu_linear_acceleration.value(); + + return ImuData{angular_velocity.value(), angular_acceleration.value(), linear_acceleration}; } std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) @@ -175,11 +177,11 @@ std::optional ImuService::pollAngularVelocity() double degrees_per_sec = static_cast(full_word) / static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; - return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_)*3.1415/180); + return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_)*M_PI/180); } -std::optional ImuService::pollAngularAcceleration() +std::optional ImuService::pollAngularAcceleration(std::optional curr_angular_velocity) { if (!initialized_) { @@ -193,7 +195,6 @@ std::optional ImuService::pollAngularAcceleration() } auto curr_time = std::chrono::steady_clock::now(); - auto curr_angular_velocity = pollAngularVelocity(); double dt = std::chrono::duration(curr_time - prev_time_).count(); if (dt <= 0 || !curr_angular_velocity.has_value()) return std::nullopt; @@ -240,7 +241,7 @@ std::optional ImuService::pollLinearAcceleration() return Eigen::Vector2d(a_x, a_y); } -std::optional ImuService::transformLinearAcceleration( +Eigen::Vector2d ImuService::transformLinearAcceleration( AngularVelocity omega, AngularAcceleration alpha, Eigen::Vector2d a_imu) { Eigen::Vector2d r(IMU_OFFSET_X, IMU_OFFSET_Y); diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index c9da597953..46472c66d9 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -32,8 +32,8 @@ class ImuService // Variance from datasheet (in rad^2/s^2) static constexpr double IMU_VARIANCE = - (4.0 * 14.4222 / 1000.0 * 3.1415 / 180.0) * - (4.0 * 14.4222 / 1000.0 * 3.1415 / 180.0); + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0); private: @@ -46,7 +46,7 @@ class ImuService * Computes angular acceleration from successive angular velocity readings * @return the current angular acceleration of the robot on the z axis */ - std::optional pollAngularAcceleration(); + std::optional pollAngularAcceleration(std::optional curr_angular_velocity); /* * Polls the latest IMU reading of the linear acceleration of the robot on the z plane * @return the current linear acceleration of the robot on the z plane @@ -65,7 +65,7 @@ class ImuService * @parama ms_reg register of the most significant register * @return the combined integer value of the two registers */ - std::optional transformLinearAcceleration(AngularVelocity omega, AngularAceleration alpha, LinearAcceleration a); + Eigen::Vector2d transformLinearAcceleration(AngularVelocity omega, AngularAcceleration alpha, Eigen::Vector2d a); bool initialized_=false; From 437a9a812125fddfc4087ac4e85fa17d336b3fed Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 16:00:15 -0700 Subject: [PATCH 28/38] fix typo --- src/shared/constants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index b3557dda6d..ed94c0cde9 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -1,5 +1,5 @@ #pragma once -#include +#include // Some platformio targets don't support STL, so we can't // use unordered_map, string, .... We guard all networking stuff with From c17b2510b7b8a643c803bb3ee2870bf1175975d8 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 16:34:45 -0700 Subject: [PATCH 29/38] resolve comments --- src/software/embedded/services/imu.cpp | 21 +++++++++++---------- src/software/embedded/services/imu.h | 7 +++---- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index d2c32cd68d..f82111c271 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -10,7 +10,7 @@ // these functions taken from // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c -__s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, +static __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, union i2c_smbus_data* data) { struct i2c_smbus_ioctl_data args; @@ -27,7 +27,7 @@ __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, return err; } -__s32 i2c_smbus_read_byte_data(int file, __u8 command) +static __s32 i2c_smbus_read_byte_data(int file, __u8 command) { union i2c_smbus_data data; int err; @@ -39,7 +39,7 @@ __s32 i2c_smbus_read_byte_data(int file, __u8 command) return 0x0FF & data.byte; } -__s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) +static __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) { union i2c_smbus_data data; data.byte = value; @@ -114,6 +114,7 @@ ImuService::ImuService() : initialized_(false) usleep(50000); } + // TODO: More robust calibration if (valid_samples > 0) { degrees_error_ = sum / valid_samples; @@ -133,13 +134,15 @@ std::optional ImuService::poll(){ std::optional angular_acceleration = pollAngularAcceleration(angular_velocity); std::optional imu_linear_acceleration = pollLinearAcceleration(); - if (!angular_velocity.has_value() || !angular_acceleration.has_value() || !imu_linear_acceleration.has_value()){ - return std::nullopt; + if (angular_velocity.has_value() || angular_acceleration.has_value() || imu_linear_acceleration.has_value()){ + std::optional linear_acceleration = transformLinearAcceleration(angular_velocity.value(), angular_acceleration.value(), imu_linear_acceleration.value()); + } + else{ + std::optional linear_acceleration = std::nullopt; } - Eigen::Vector2d linear_acceleration = transformLinearAcceleration(angular_velocity.value(), angular_acceleration.value(), imu_linear_acceleration.value(); - return ImuData{angular_velocity.value(), angular_acceleration.value(), linear_acceleration}; + return ImuData{angular_velocity, angular_acceleration, linear_acceleration}; } std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) @@ -190,7 +193,7 @@ std::optional ImuService::pollAngularAcceleration(std::opti if(!prev_angular_velocity_.has_value()){ prev_angular_velocity_ = pollAngularVelocity(); - prev_time_ = std::chrono::steady_clock::now();; + prev_time_ = std::chrono::steady_clock::now(); return std::nullopt; } @@ -236,8 +239,6 @@ std::optional ImuService::pollLinearAcceleration() * ACCELEROMETER_FULL_SCALE_G * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; - // Transform using equations - return Eigen::Vector2d(a_x, a_y); } diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 46472c66d9..6d1a8f0b02 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -3,7 +3,6 @@ #include #include #include -#include #include #include "proto/tbots_software_msgs.pb.h" @@ -14,9 +13,9 @@ * Handles low level IMU I2C communication, and some minor offset filtering. */ struct ImuData{ - AngularVelocity angular_velocity; - AngularAcceleration angular_acceleration; - Eigen::Vector2d linear_acceleration; + std::optional angular_velocity; + std::optional angular_acceleration; + std::optional linear_acceleration; }; class ImuService { From 6eeb1e8c81637a99833587c292c08c2e0aff7dd1 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 17:08:40 -0700 Subject: [PATCH 30/38] fix linear acceleration scope bug --- src/software/embedded/services/imu.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index f82111c271..789796c7f3 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -134,11 +134,10 @@ std::optional ImuService::poll(){ std::optional angular_acceleration = pollAngularAcceleration(angular_velocity); std::optional imu_linear_acceleration = pollLinearAcceleration(); - if (angular_velocity.has_value() || angular_acceleration.has_value() || imu_linear_acceleration.has_value()){ - std::optional linear_acceleration = transformLinearAcceleration(angular_velocity.value(), angular_acceleration.value(), imu_linear_acceleration.value()); - } - else{ - std::optional linear_acceleration = std::nullopt; + std::optional linear_acceleration; + if (angular_velocity && angular_acceleration && imu_linear_acceleration) { + linear_acceleration = transformLinearAcceleration( + *angular_velocity, *angular_acceleration, *imu_linear_acceleration); } From be15a2a2557c9842b69eda66b8f1e4da45f3f670 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 23 May 2026 17:20:05 -0700 Subject: [PATCH 31/38] fix thunderloop log bug --- src/software/embedded/thunderloop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 56f9047392..2d54721ed0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -186,8 +186,8 @@ void Thunderloop::runLoop() std::optional imu_poll = imu_service_->poll(); //TODO: Replace with actual IMU data usage - if (imu_poll.has_value()){ - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity.toRadians(); + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()){ + LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); } for (;;) From 038c1f2d1fcdc84448420caa324c8aad984c66dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sun, 24 May 2026 00:26:25 +0000 Subject: [PATCH 32/38] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/BUILD | 2 +- src/software/embedded/services/BUILD | 2 +- src/software/embedded/services/imu.cpp | 106 +++++++------- src/software/embedded/services/imu.h | 189 +++++++++++++------------ src/software/embedded/thunderloop.cpp | 19 +-- src/software/embedded/thunderloop.h | 2 +- 6 files changed, 165 insertions(+), 155 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index b0321b44a9..67262ae526 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -26,10 +26,10 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", - "//software/embedded/services:imu", "//software/embedded/toml_config", "//software/logger:network_logger", "//software/tracy:tracy_constants", diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 6ff60c757d..a6b09d76e9 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -38,8 +38,8 @@ cc_library( deps = [ "//proto:tbots_cc_proto", "//shared:constants", - "//software/geom:angular_velocity", "//software/geom:angular_acceleration", + "//software/geom:angular_velocity", "//software/logger", "@eigen", ], diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp index 789796c7f3..e57fd7ff02 100644 --- a/src/software/embedded/services/imu.cpp +++ b/src/software/embedded/services/imu.cpp @@ -1,17 +1,19 @@ +#include "imu.h" + #include #include #include #include -#include // For SHRT_MAX -#include "imu.h" +#include // For SHRT_MAX + #include "shared/constants.h" #include "software/logger/logger.h" // these functions taken from // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c static __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, - union i2c_smbus_data* data) + union i2c_smbus_data* data) { struct i2c_smbus_ioctl_data args; __s32 err; @@ -128,21 +130,22 @@ ImuService::ImuService() : initialized_(false) } } -std::optional ImuService::poll(){ - - std::optional angular_velocity = pollAngularVelocity(); - std::optional angular_acceleration = pollAngularAcceleration(angular_velocity); - std::optional imu_linear_acceleration = pollLinearAcceleration(); - - std::optional linear_acceleration; - if (angular_velocity && angular_acceleration && imu_linear_acceleration) { - linear_acceleration = transformLinearAcceleration( - *angular_velocity, *angular_acceleration, *imu_linear_acceleration); - } +std::optional ImuService::poll() +{ + std::optional angular_velocity = pollAngularVelocity(); + std::optional angular_acceleration = + pollAngularAcceleration(angular_velocity); + std::optional imu_linear_acceleration = pollLinearAcceleration(); + std::optional linear_acceleration; + if (angular_velocity && angular_acceleration && imu_linear_acceleration) + { + linear_acceleration = transformLinearAcceleration( + *angular_velocity, *angular_acceleration, *imu_linear_acceleration); + } - return ImuData{angular_velocity, angular_acceleration, linear_acceleration}; + return ImuData{angular_velocity, angular_acceleration, linear_acceleration}; } std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) { @@ -156,7 +159,7 @@ std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_ uint16_t combined = (static_cast(most_significant) << 8) | static_cast(least_significant); - + return static_cast(combined); } @@ -167,50 +170,54 @@ std::optional ImuService::pollAngularVelocity() return std::nullopt; } - std::optional opt_full_word = readAndCombineByteData(GYRO_LEAST_SIG_REG, GYRO_MOST_SIG_REG); - + std::optional opt_full_word = + readAndCombineByteData(GYRO_LEAST_SIG_REG, GYRO_MOST_SIG_REG); + if (!opt_full_word.has_value()) { return std::nullopt; } int16_t full_word = opt_full_word.value(); - + double degrees_per_sec = static_cast(full_word) / static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; - - return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_)*M_PI/180); + + return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_) * M_PI / 180); } -std::optional ImuService::pollAngularAcceleration(std::optional curr_angular_velocity) +std::optional ImuService::pollAngularAcceleration( + std::optional curr_angular_velocity) { if (!initialized_) { return std::nullopt; } - - if(!prev_angular_velocity_.has_value()){ - prev_angular_velocity_ = pollAngularVelocity(); - prev_time_ = std::chrono::steady_clock::now(); - return std::nullopt; - } - auto curr_time = std::chrono::steady_clock::now(); + if (!prev_angular_velocity_.has_value()) + { + prev_angular_velocity_ = pollAngularVelocity(); + prev_time_ = std::chrono::steady_clock::now(); + return std::nullopt; + } - double dt = std::chrono::duration(curr_time - prev_time_).count(); - if (dt <= 0 || !curr_angular_velocity.has_value()) return std::nullopt; + auto curr_time = std::chrono::steady_clock::now(); - double alpha = (curr_angular_velocity->toRadians() - - prev_angular_velocity_->toRadians()) / dt; + double dt = std::chrono::duration(curr_time - prev_time_).count(); + if (dt <= 0 || !curr_angular_velocity.has_value()) + return std::nullopt; - prev_angular_velocity_ = curr_angular_velocity; - prev_time_ = curr_time; + double alpha = + (curr_angular_velocity->toRadians() - prev_angular_velocity_->toRadians()) / dt; + + prev_angular_velocity_ = curr_angular_velocity; + prev_time_ = curr_time; return AngularAcceleration::fromRadians(alpha); } - - + + std::optional ImuService::pollLinearAcceleration() { @@ -219,8 +226,10 @@ std::optional ImuService::pollLinearAcceleration() return std::nullopt; } - std::optional opt_x = readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); - std::optional opt_y = readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); + std::optional opt_x = + readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional opt_y = + readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); if (!opt_x.has_value() || !opt_y.has_value()) { @@ -230,19 +239,18 @@ std::optional ImuService::pollLinearAcceleration() int16_t raw_x = opt_x.value(); int16_t raw_y = opt_y.value(); - double a_x = (static_cast(raw_x) / SHRT_MAX) - * ACCELEROMETER_FULL_SCALE_G - * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; - - double a_y = (static_cast(raw_y) / SHRT_MAX) - * ACCELEROMETER_FULL_SCALE_G - * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; - + double a_x = (static_cast(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + double a_y = (static_cast(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + return Eigen::Vector2d(a_x, a_y); } -Eigen::Vector2d ImuService::transformLinearAcceleration( - AngularVelocity omega, AngularAcceleration alpha, Eigen::Vector2d a_imu) +Eigen::Vector2d ImuService::transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d a_imu) { Eigen::Vector2d r(IMU_OFFSET_X, IMU_OFFSET_Y); diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h index 6d1a8f0b02..e2ff0e0c02 100644 --- a/src/software/embedded/services/imu.h +++ b/src/software/embedded/services/imu.h @@ -2,110 +2,111 @@ #include #include -#include #include +#include #include "proto/tbots_software_msgs.pb.h" -#include "software/geom/angular_velocity.h" #include "software/geom/angular_acceleration.h" +#include "software/geom/angular_velocity.h" /** * Handles low level IMU I2C communication, and some minor offset filtering. */ -struct ImuData{ - std::optional angular_velocity; - std::optional angular_acceleration; - std::optional linear_acceleration; +struct ImuData +{ + std::optional angular_velocity; + std::optional angular_acceleration; + std::optional linear_acceleration; }; class ImuService { - public: - /** - * Constructs and initializes a new IMU service object. - * - * If successfully initialized, will try to do a simple calibration of the IMU. - */ - ImuService(); - - std::optional poll(); - - // Variance from datasheet (in rad^2/s^2) - static constexpr double IMU_VARIANCE = - (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * - (4.0 * 14.4222 / 1000.0 * M_PI / 180.0); - - private: - - /* - * Polls the latest IMU reading of the angular velocity of the robot on the z axis - * @return the current angular velocty of the robot on the z axis - */ - std::optional pollAngularVelocity(); - /* - * Computes angular acceleration from successive angular velocity readings - * @return the current angular acceleration of the robot on the z axis - */ - std::optional pollAngularAcceleration(std::optional curr_angular_velocity); - /* - * Polls the latest IMU reading of the linear acceleration of the robot on the z plane - * @return the current linear acceleration of the robot on the z plane - */ - std::optional pollLinearAcceleration(); - /* - * Reads byte data from two registers, and combine them into a single value - * @parama ls_reg register of the least significant register - * @parama ms_reg register of the most significant register - * @return the combined integer value of the two registers - */ - std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); - /* - * Reads byte data from two registers, and combine them into a single value - * @parama ls_reg register of the least significant register - * @parama ms_reg register of the most significant register - * @return the combined integer value of the two registers - */ - Eigen::Vector2d transformLinearAcceleration(AngularVelocity omega, AngularAcceleration alpha, Eigen::Vector2d a); - - bool initialized_=false; - - int file_descriptor_=0; - - double degrees_error_; - - // Maps the maximum raw reading from 16-bit integer to be 2 times gravity - static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; - // Same for gyroscope, 1000 degrees per second - static constexpr double IMU_FULL_SCALE_DPS=1000.0; - - /// Various i2c registers. - static const uint8_t WHOAMI_REG = 0xf; - static const uint8_t ACCEL_CONTROL_REG = 0x10; - static const uint8_t GYRO_CONTROL_REG = 0x11; - static const uint8_t CTRL4_C = 0x13; - static const uint8_t CTRL6_C = 0x15; - static const uint8_t CTRL8_XL = 0x17; - - // Device path for the IMU - inline static const std::string IMU_DEVICE = "/dev/i2c-1"; - - // Gyroscope Z-axis (Yaw) Output Data Registers - static constexpr uint8_t GYRO_LEAST_SIG_REG = 0x26; // OUTZ_L_G - static constexpr uint8_t GYRO_MOST_SIG_REG = 0x27; // OUTZ_H_G - - // Accelerometer X-axis Output Data Registers - static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL - static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL - - // Accelerometer Y-axis Output Data Registers - static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL - static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL - - // IMU offset for acceleration calculation - static constexpr double IMU_OFFSET_X = 0.0; - static constexpr double IMU_OFFSET_Y = 0.0; - - // prev time and angular accleration - std::optional prev_angular_velocity_; - std::chrono::steady_clock::time_point prev_time_; + public: + /** + * Constructs and initializes a new IMU service object. + * + * If successfully initialized, will try to do a simple calibration of the IMU. + */ + ImuService(); + + std::optional poll(); + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * (4.0 * 14.4222 / 1000.0 * M_PI / 180.0); + + private: + /* + * Polls the latest IMU reading of the angular velocity of the robot on the z axis + * @return the current angular velocty of the robot on the z axis + */ + std::optional pollAngularVelocity(); + /* + * Computes angular acceleration from successive angular velocity readings + * @return the current angular acceleration of the robot on the z axis + */ + std::optional pollAngularAcceleration( + std::optional curr_angular_velocity); + /* + * Polls the latest IMU reading of the linear acceleration of the robot on the z plane + * @return the current linear acceleration of the robot on the z plane + */ + std::optional pollLinearAcceleration(); + /* + * Reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); + /* + * Reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + Eigen::Vector2d transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d a); + + bool initialized_ = false; + + int file_descriptor_ = 0; + + double degrees_error_; + + // Maps the maximum raw reading from 16-bit integer to be 2 times gravity + static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; + // Same for gyroscope, 1000 degrees per second + static constexpr double IMU_FULL_SCALE_DPS = 1000.0; + + /// Various i2c registers. + static const uint8_t WHOAMI_REG = 0xf; + static const uint8_t ACCEL_CONTROL_REG = 0x10; + static const uint8_t GYRO_CONTROL_REG = 0x11; + static const uint8_t CTRL4_C = 0x13; + static const uint8_t CTRL6_C = 0x15; + static const uint8_t CTRL8_XL = 0x17; + + // Device path for the IMU + inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + + // Gyroscope Z-axis (Yaw) Output Data Registers + static constexpr uint8_t GYRO_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t GYRO_MOST_SIG_REG = 0x27; // OUTZ_H_G + + // Accelerometer X-axis Output Data Registers + static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL + static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL + + // Accelerometer Y-axis Output Data Registers + static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL + static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL + + // IMU offset for acceleration calculation + static constexpr double IMU_OFFSET_X = 0.0; + static constexpr double IMU_OFFSET_Y = 0.0; + // prev time and angular acceleration + std::optional prev_angular_velocity_; + std::chrono::steady_clock::time_point prev_time_; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 2d54721ed0..2f8366bf33 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -9,8 +9,8 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "software/embedded/primitive_executor.h" -#include "software/embedded/services/motor.h" #include "software/embedded/services/imu.h" +#include "software/embedded/services/motor.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" @@ -120,8 +120,8 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; motor_service_ = std::make_unique(robot_constants, loop_hz); - g_motor_service = motor_service_.get(); - motor_service_->setup(); + g_motor_service = motor_service_.get(); + motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; @@ -182,13 +182,14 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - - std::optional imu_poll = imu_service_->poll(); - //TODO: Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()){ - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } + std::optional imu_poll = imu_service_->poll(); + + // TODO: Replace with actual IMU data usage + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) + { + LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); + } for (;;) { diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index b3da73b76c..e553a78a54 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -10,10 +10,10 @@ #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" -#include "software/embedded/services/imu.h" #include "software/embedded/toml_config/toml_config_client.h" #include "software/logger/logger.h" From 32320931b8dd27682f7da9d8bc29de7a3f1f74d6 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sat, 23 May 2026 19:30:23 -0700 Subject: [PATCH 33/38] Use elapsed time between stepPrimitive calls instead of fixed time step --- src/shared/robot_constants.h | 2 +- .../stspin_motor_controller.cpp | 2 - src/software/embedded/primitive_executor.cpp | 80 ++++++++++++------- src/software/embedded/primitive_executor.h | 31 +++---- src/software/embedded/thunderloop.cpp | 3 +- 5 files changed, 64 insertions(+), 54 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 23ba552ef6..6ba5b92be3 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -124,7 +124,7 @@ constexpr RobotConstants createRobotConstants() // Robot's angular movement constants .robot_max_ang_speed_rad_per_s = 6.0f, .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 4.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, .wheel_radius_meters = 0.03f, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1c57ec5f9b..7f7164c407 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -182,8 +182,6 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, sendAndReceiveFrame(motor, outgoing_frame); - sendMotorStatusToPlotJuggler(motor); - return motor_status_.at(motor).speed; } diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index c14d5d328f..b0bda212fb 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,12 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants) { } @@ -65,8 +61,9 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (isLinearTrajectoryNew(new_trajectory_path)) { - trajectory_path_ = new_trajectory_path; - time_since_linear_trajectory_creation_ = Duration::fromSeconds(RTT_S); + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); x_pid.reset(); y_pid.reset(); } @@ -78,8 +75,9 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (isAngularTrajectoryNew(new_angular_trajectory)) { - angular_trajectory_ = new_angular_trajectory; - time_since_angular_trajectory_creation_ = Duration::fromSeconds(RTT_S); + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); w_pid.reset(); } } @@ -101,7 +99,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double linear_following_error = (position_ - trajectory_path_->getPosition( - time_since_linear_trajectory_creation_.toSeconds())) + time_since_linear_trajectory_creation_.count())) .length(); if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) @@ -110,7 +108,9 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit trajectory_path_ = createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); - time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); + + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); } } @@ -118,7 +118,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double angular_following_error = angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) + ->getPosition(time_since_angular_trajectory_creation_.count()) .minDiff(orientation_) .toDegrees(); @@ -128,7 +128,9 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit angular_trajectory_ = createAngularTrajectoryFromParams( current_primitive_.move().w_traj_params(), orientation_, angular_velocity_, robot_constants_); - time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); + + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); } } } @@ -136,10 +138,10 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit Vector PrimitiveExecutor::getTargetLinearVelocity() const { Vector target_velocity = - trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.toSeconds()); + trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.count()); const Point expected_position = - trajectory_path_->getPosition(time_since_linear_trajectory_creation_.toSeconds()); + trajectory_path_->getPosition(time_since_linear_trajectory_creation_.count()); const double distance_to_destination = distance(expected_position, trajectory_path_->getDestination()); @@ -155,11 +157,11 @@ Vector PrimitiveExecutor::getTargetLinearVelocity() const AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() const { - AngularVelocity angular_velocity = angular_trajectory_->getVelocity( - time_since_angular_trajectory_creation_.toSeconds()); + AngularVelocity angular_velocity = + angular_trajectory_->getVelocity(time_since_angular_trajectory_creation_.count()); - const Angle expected_orientation = angular_trajectory_->getPosition( - time_since_angular_trajectory_creation_.toSeconds()); + const Angle expected_orientation = + angular_trajectory_->getPosition(time_since_angular_trajectory_creation_.count()); const double distance_to_destination = expected_orientation.minDiff(angular_trajectory_->getDestination()).toDegrees(); @@ -177,8 +179,11 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() const std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - time_since_angular_trajectory_creation_ += time_step_; - time_since_linear_trajectory_creation_ += time_step_; + const auto current_time = std::chrono::steady_clock::now(); + time_since_linear_trajectory_creation_ += current_time - last_step_time_; + time_since_angular_trajectory_creation_ += current_time - last_step_time_; + last_step_time_ = current_time; + status.set_running_primitive(true); switch (current_primitive_.primitive_case()) @@ -230,7 +235,7 @@ std::unique_ptr PrimitiveExecutor::stepPrimi // FEEDFORWARD velocities from the trajectory const Vector trajectory_linear_velocity = getTargetLinearVelocity(); error_linear = trajectory_path_->getPosition( - time_since_linear_trajectory_creation_.toSeconds()) - + time_since_linear_trajectory_creation_.count()) - position_; const Vector pid_effort_linear(x_pid.step(error_linear.x()), y_pid.step(error_linear.y())); @@ -242,23 +247,40 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { target_angular_velocity = AngularVelocity::fromRadians( w_pid_close.step(error_angular.toRadians())); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"is_pure_pid", 100}, + }); } else { // FEEDFORWARD velocities from the trajectory - error_angular = - (angular_trajectory_->getPosition( - time_since_angular_trajectory_creation_.toSeconds()) - - orientation_) - .clamp(); + error_angular = (angular_trajectory_->getPosition( + time_since_angular_trajectory_creation_.count()) - + orientation_) + .clamp(); const AngularVelocity trajectory_angular_velocity = getTargetAngularVelocity(); const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians())); target_angular_velocity = - (trajectory_angular_velocity + pid_effort_angular); + trajectory_angular_velocity + pid_effort_angular; + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"is_pure_pid", 0}, + }); } + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"target_angular_velocity", target_angular_velocity.toDegrees()}, + {"angular_velocity", angular_velocity_.toDegrees()}, + {"orientation", orientation_.toDegrees()}, + {"expected_orientation", + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.count()) + .toDegrees()}, + }); + // Make sure target linear velocity is clamped target_linear_velocity = target_linear_velocity.normalize( std::min(target_linear_velocity.length(), diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 51b163e06d..9b39f68d84 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -1,4 +1,5 @@ #pragma once + #include "proto/primitive.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -15,19 +16,14 @@ class PrimitiveExecutor public: /** * Constructor - * @param time_step Time step which this primitive executor operates in - * @param robot_constants The robot constants for the robot which uses this primitive - * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor + * + * @param robot_constants The robot constants for the robot */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive + * * @param primitive_msg The primitive to start */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); @@ -94,8 +90,9 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - Duration time_since_angular_trajectory_creation_; - Duration time_since_linear_trajectory_creation_; + std::chrono::time_point last_step_time_; + std::chrono::duration time_since_linear_trajectory_creation_; + std::chrono::duration time_since_angular_trajectory_creation_; Point position_; Vector velocity_; @@ -105,14 +102,8 @@ class PrimitiveExecutor Point last_position_; Angle last_orientation_; - TeamColour friendly_team_colour_; robot_constants::RobotConstants robot_constants_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - RobotId robot_id_; - controls::PIDController x_pid = {0.8, 0, 0, 0}; controls::PIDController y_pid = {0.8, 0, 0, 0}; controls::PIDController w_pid = {0.7, 0, 2, 0}; @@ -121,7 +112,7 @@ class PrimitiveExecutor // These PIDs should be used in that case. controls::PIDController x_pid_close = {2, 0, 0, 0}; controls::PIDController y_pid_close = {2, 0, 0, 0}; - controls::PIDController w_pid_close = {2, 0, 4, 0}; + controls::PIDController w_pid_close = {2, 0, 0, 0}; // If distance between current linear trajectory destination and new one is larger // than this, we change trajectories. @@ -129,10 +120,10 @@ class PrimitiveExecutor static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; static constexpr double LINEAR_STALL_ERROR_MAX_METERS = .4; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 25; static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 45; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index eb54530bb5..08272f8b16 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -84,8 +84,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_), + primitive_executor_(robot_constants), robot_localizer_(robot_constants.kalman_process_noise_variance_rad_per_s_4, robot_constants.kalman_vision_noise_variance_rad_2, robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2) From 5f0c7df915f0e0449578f78456593653f14e3f19 Mon Sep 17 00:00:00 2001 From: Annie Sun <108475950+annieisawesome2@users.noreply.github.com> Date: Mon, 25 May 2026 20:56:29 -0700 Subject: [PATCH 34/38] Change network interface for PlotJugglerSink (#3741) * change network interface for PlotJugglerink * [pre-commit.ci lite] apply automatic fixes * use network_interface only * [pre-commit.ci lite] apply automatic fixes --------- Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> --- src/software/embedded/thunderloop.cpp | 4 +++- src/software/logger/network_logger.cpp | 13 ++++++++----- src/software/logger/network_logger.h | 6 ++++-- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b8a440a683..00a76d5717 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -8,6 +8,7 @@ #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" +#include "software/constants.h" #include "software/embedded/primitive_executor.h" #include "software/embedded/services/motor.h" #include "software/logger/logger.h" @@ -88,7 +89,8 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, waitForNetworkUp(); g3::overrideSetupSignals({}); - NetworkLoggerSingleton::initializeLogger(robot_id_, enable_log_merging); + NetworkLoggerSingleton::initializeLogger(robot_id_, enable_log_merging, + network_interface_); // catch all catch-able signals std::signal(SIGSEGV, tbotsExit); diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 518050305f..f14ed77157 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -5,7 +5,8 @@ std::shared_ptr NetworkLoggerSingleton::instance; -NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging) +NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface) { logWorker = g3::LogWorker::createLogWorker(); @@ -21,18 +22,20 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting - auto plotjuggler_handle = logWorker->addSink(std::make_unique(), - &PlotJugglerSink::sendToPlotJuggler); + auto plotjuggler_handle = + logWorker->addSink(std::make_unique(network_interface), + &PlotJugglerSink::sendToPlotJuggler); g3::initializeLogging(logWorker.get()); } -void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_merging) +void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface) { if (!instance) { NetworkLoggerSingleton::instance = std::shared_ptr( - new NetworkLoggerSingleton(robot_id, enable_log_merging)); + new NetworkLoggerSingleton(robot_id, enable_log_merging, network_interface)); } } diff --git a/src/software/logger/network_logger.h b/src/software/logger/network_logger.h index eec4a4b302..7fe021b845 100644 --- a/src/software/logger/network_logger.h +++ b/src/software/logger/network_logger.h @@ -21,7 +21,8 @@ class NetworkLoggerSingleton * Initializes a g3log logger for the calling program. This should only be * called once at the start of a program. */ - static void initializeLogger(RobotId robot_id, bool enable_log_merging); + static void initializeLogger(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface = "tbotswifi5"); /** * Updates the underlying UDP sender associated with this network sink. Useful when a @@ -31,7 +32,8 @@ class NetworkLoggerSingleton std::shared_ptr> new_sender); private: - NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging); + NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface); std::unique_ptr logWorker; std::unique_ptr> network_sink_handle; From aff884895b8e114259a7441e09eefc4a1c761b36 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 25 May 2026 22:34:02 -0700 Subject: [PATCH 35/38] Fix incorrect control model in robot localizer --- src/software/embedded/primitive_executor.cpp | 4 ++-- src/software/embedded/robot_localizer.cpp | 23 +++++++------------ src/software/embedded/robot_localizer.h | 10 +++----- src/software/embedded/thunderloop.cpp | 22 ++++++------------ .../simulation/er_force_simulator.cpp | 10 ++++---- 5 files changed, 24 insertions(+), 45 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index b0bda212fb..2fa21c80f6 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -61,7 +61,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (isLinearTrajectoryNew(new_trajectory_path)) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = std::chrono::duration::zero(); x_pid.reset(); @@ -75,7 +75,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (isAngularTrajectoryNew(new_angular_trajectory)) { - angular_trajectory_ = new_angular_trajectory; + angular_trajectory_ = new_angular_trajectory; time_since_angular_trajectory_creation_ = std::chrono::duration::zero(); w_pid.reset(); diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index db4258425c..e2ab7e219d 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -21,8 +21,7 @@ RobotLocalizer::RobotLocalizer(const double process_noise_variance, last_step_time_ = std::chrono::steady_clock::now(); } -void RobotLocalizer::step(const Vector& target_linear_acceleration, - const AngularVelocity& target_angular_acceleration) +void RobotLocalizer::step(const Vector& linear_acceleration) { FilterStep step{ .prediction = FilterStep::Predict{}, @@ -106,27 +105,21 @@ void RobotLocalizer::step(const Vector& target_linear_acceleration, control_model(static_cast(StateIndex::X_POSITION), static_cast(ControlIndex::X_ACCELERATION)) = - delta_time_seconds / 2; + delta_time_squared / 2; control_model(static_cast(StateIndex::Y_POSITION), static_cast(ControlIndex::Y_ACCELERATION)) = - delta_time_seconds / 2; - - control_model(static_cast(StateIndex::ORIENTATION), - static_cast(ControlIndex::ANGULAR_ACCELERATION)) = - delta_time_seconds / 2; + delta_time_squared / 2; control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_ACCELERATION)) = 1; + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds; control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_ACCELERATION)) = 1; - - control_model(static_cast(StateIndex::ANGULAR_VELOCITY), - static_cast(ControlIndex::ANGULAR_ACCELERATION)) = 1; + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds; - step.prediction->control_input << target_linear_acceleration.x(), - target_linear_acceleration.y(), target_angular_acceleration.toRadians(); + step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); filter_.process_model = step.prediction->process_model; filter_.process_covariance = step.prediction->process_covariance; diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 10198438df..dc8ef6f9ed 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -19,7 +19,7 @@ MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTA MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); -MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION, ANGULAR_ACCELERATION); +MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); /** * Estimates robot orientation, angular velocity, and angular acceleration @@ -48,13 +48,9 @@ class RobotLocalizer /** * Runs one prediction step using elapsed time since the previous call. * - * @param target_linear_acceleration The current target linear acceleration of the - * robot - * @param target_angular_acceleration The current target angular acceleration of the - * robot + * @param linear_acceleration The current linear acceleration of the robot */ - void step(const Vector& target_linear_acceleration, - const AngularVelocity& target_angular_acceleration); + void step(const Vector& linear_acceleration); /** * Update the robot's position and orientation from data reported by vision. diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index fc84c2e3e0..3a1e01fdf1 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -185,15 +185,6 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - - std::optional imu_poll = imu_service_->poll(); - - // TODO: Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) - { - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } - for (;;) { struct timespec time_since_prev_iter; @@ -273,7 +264,7 @@ void Thunderloop::runLoop() } const ImuData imu_poll = imu_service_->poll(); - + if (imu_poll.angular_velocity.has_value()) { robot_localizer_.updateImu(imu_poll.angular_velocity.value()); @@ -288,17 +279,18 @@ void Thunderloop::runLoop() robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())); - Vector linear_acceleration = Vector(); - if (imu_poll.linear_acceleration.has_value()) { - const auto a = imu_poll.linear_acceleration.value(); - linear_acceleration = Vector(a[1], a[0]); + Vector linear_acceleration; + if (imu_poll.linear_acceleration.has_value()) + { + const auto accel = imu_poll.linear_acceleration.value(); + linear_acceleration = Vector(accel[1], accel[0]); LOG(PLOTJUGGLER) << *createPlotJugglerValue({ {"linear_acceleration_x", linear_acceleration.x()}, {"linear_acceleration_y", linear_acceleration.y()}, }); } - robot_localizer_.step(linear_acceleration, AngularVelocity::zero()); + robot_localizer_.step(linear_acceleration); primitive_executor_.updateState(robot_localizer_.getPosition(), robot_localizer_.getVelocity(), diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 904c48d039..7593b87ce1 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -280,16 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } From d6347c4f799ecb88160e9d63cfa6482f94dd8d25 Mon Sep 17 00:00:00 2001 From: Samuel <92961466+StarrryNight@users.noreply.github.com> Date: Tue, 26 May 2026 21:38:04 -0700 Subject: [PATCH 36/38] IMU Integration (#3739) --- src/software/embedded/BUILD | 1 + src/software/embedded/services/BUILD | 14 ++ src/software/embedded/services/imu.cpp | 316 +++++++++++++++++++++++++ src/software/embedded/services/imu.h | 121 ++++++++++ src/software/embedded/thunderloop.cpp | 16 +- src/software/embedded/thunderloop.h | 2 + 6 files changed, 469 insertions(+), 1 deletion(-) create mode 100644 src/software/embedded/services/imu.cpp create mode 100644 src/software/embedded/services/imu.h diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..67262ae526 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -26,6 +26,7 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index f763e4f8dc..a6b09d76e9 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -31,6 +31,20 @@ cc_library( ], ) +cc_library( + name = "imu", + srcs = ["imu.cpp"], + hdrs = ["imu.h"], + deps = [ + "//proto:tbots_cc_proto", + "//shared:constants", + "//software/geom:angular_acceleration", + "//software/geom:angular_velocity", + "//software/logger", + "@eigen", + ], +) + cc_binary( name = "robot_auto_test", srcs = ["robot_auto_test.cpp"], diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp new file mode 100644 index 0000000000..bf21ddba20 --- /dev/null +++ b/src/software/embedded/services/imu.cpp @@ -0,0 +1,316 @@ +#include "imu.h" + +#include +#include +#include +#include + +#include // For SHRT_MAX + +#include "shared/constants.h" +#include "software/logger/logger.h" + +// these functions taken from +// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c +static __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, + union i2c_smbus_data* data) +{ + struct i2c_smbus_ioctl_data args; + __s32 err; + + args.read_write = read_write; + args.command = command; + args.size = size; + args.data = data; + + err = ioctl(file, I2C_SMBUS, &args); + if (err == -1) + err = -errno; + return err; +} + +static __s32 i2c_smbus_read_byte_data(int file, __u8 command) +{ + union i2c_smbus_data data; + int err; + + err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data); + if (err < 0) + return err; + + return 0x0FF & data.byte; +} + +static __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) +{ + union i2c_smbus_data data; + data.byte = value; + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); +} + + +ImuService::ImuService() : initialized_(false) +{ + // Establish connection to the IMU and verify that the who am I pin is correct. + file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR); + int ret = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b); + if (ret < 0) + { + LOG(WARNING) + << "Failed to initialize the IMU: failed to establish i2c connection."; + return; + } + if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106) + { + LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register " + << static_cast(WHOAMI_REG) << " read incorrectly."; + return; + } + // Attempt to enable gyro and accelerometer, checking that writes are successful + // See lsm6dsl datasheet for how to set these registers. + i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000); + if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to accelerometer config register " + << static_cast(ACCEL_CONTROL_REG) << " unsuccessful"; + return; + } + // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of + // datasheet for lsm6dsl, pg 21) + i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000); + if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to gyroscope config register " + << "unsuccessful"; + return; + } + + // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000) + // CTRL4_C: bit 1 (LPF1_SEL_G) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010); + // CTRL6_C: bits 2:0 (FTYPE) = 000 + i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000); + + // Enable Accelerometer digital LPF2 + // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000); + + initialized_ = true; + LOG(INFO) << "Initialized IMU! Calibrating..."; + degrees_error_ = 0; + // get 100 sample average of stationary reading, so all future readings can be + // corrected + double sum = 0; + int valid_samples = 0; + for (int i = 0; i < 100; i++) + { + // Fixed: Call the actual implemented function name + auto poll = pollAngularVelocity(); + if (poll.has_value()) + { + sum += poll->toDegrees(); + valid_samples++; + } + usleep(50000); + } + + // TODO (3421): More robust calibration + if (valid_samples > 0) + { + degrees_error_ = sum / valid_samples; + LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps"; + } + else + { + LOG(WARNING) << "IMU Calibration failed: no valid samples received. Angular " + "stability will be poor."; + initialized_ = false; + } + + // Temporary: enable when need to calibrate + // Eigen::Vector2d deviation = calibrate_imu(); + // LOG(INFO) << "error: " << deviation.x() << deviation.y() << "."; +} + +std::optional ImuService::poll() +{ + std::optional angular_velocity = pollAngularVelocity(); + std::optional angular_acceleration = + pollAngularAcceleration(angular_velocity); + std::optional imu_linear_acceleration = pollLinearAcceleration(); + + std::optional linear_acceleration; + if (angular_velocity && angular_acceleration && imu_linear_acceleration) + { + linear_acceleration = transformLinearAcceleration( + *angular_velocity, *angular_acceleration, *imu_linear_acceleration); + } + + + return ImuData{angular_velocity, angular_acceleration, linear_acceleration}; +} +std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) +{ + int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); + int most_significant = i2c_smbus_read_byte_data(file_descriptor_, ms_reg); + + if (least_significant < 0 || most_significant < 0) + { + return std::nullopt; + } + + uint16_t combined = (static_cast(most_significant) << 8) | + static_cast(least_significant); + + return static_cast(combined); +} + +std::optional ImuService::pollAngularVelocity() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_full_word = + readAndCombineByteData(GYRO_LEAST_SIG_REG, GYRO_MOST_SIG_REG); + + if (!opt_full_word.has_value()) + { + return std::nullopt; + } + + int16_t full_word = opt_full_word.value(); + + double degrees_per_sec = static_cast(full_word) / + static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + + return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_) * M_PI / 180); +} + + +std::optional ImuService::pollAngularAcceleration( + std::optional curr_angular_velocity) +{ + if (!initialized_) + { + return std::nullopt; + } + + if (!prev_angular_velocity_.has_value()) + { + prev_angular_velocity_ = pollAngularVelocity(); + prev_time_ = std::chrono::steady_clock::now(); + return std::nullopt; + } + + auto curr_time = std::chrono::steady_clock::now(); + + double dt = std::chrono::duration(curr_time - prev_time_).count(); + if (dt <= 0 || !curr_angular_velocity.has_value()) + return std::nullopt; + + double alpha = + (curr_angular_velocity->toRadians() - prev_angular_velocity_->toRadians()) / dt; + + prev_angular_velocity_ = curr_angular_velocity; + prev_time_ = curr_time; + + return AngularAcceleration::fromRadians(alpha); +} + + + +std::optional ImuService::pollLinearAcceleration() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_x = + readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional opt_y = + readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); + + if (!opt_x.has_value() || !opt_y.has_value()) + { + return std::nullopt; + } + + int16_t raw_x = opt_x.value(); + int16_t raw_y = opt_y.value(); + + double a_x = (static_cast(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + double a_y = (static_cast(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + return Eigen::Vector2d(a_x, a_y); +} + +Eigen::Vector2d ImuService::transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d imu_acceleration) +{ + Eigen::Vector2d r(IMU_OFFSET_X, IMU_OFFSET_Y); + + double w = omega.toRadians(); + double a = alpha.toRadians(); + + // tangential: alpha x r → (-alpha*ry, alpha*rx) + Eigen::Vector2d tangential(-a * r.y(), a * r.x()); + + // centripetal: omega^2 * r + Eigen::Vector2d centripetal = (w * w) * r; + + return imu_acceleration + tangential - centripetal; +} + +Eigen::Vector2d ImuService::calibrate_imu() +{ + LOG(INFO) << "Start IMU x,y calibration" << std::endl; + Eigen::MatrixXd A(2 * 100, 2); + Eigen::VectorXd b(2 * 100); + int valid = 0; + + for (int i = 0; i < 100; i++) + { + // Fixed: Call the actual implemented function name + auto omega_opt = pollAngularVelocity(); + if (!omega_opt.has_value()) + { + continue; + } + auto alpha_opt = pollAngularAcceleration(omega_opt); + auto acc = pollLinearAcceleration(); + if (omega_opt.has_value() && alpha_opt.has_value() && acc.has_value()) + { + double omega = omega_opt->toRadians(); + double alpha = alpha_opt->toRadians(); + + A(2 * valid, 0) = -omega * omega; + A(2 * valid, 1) = -alpha; + A(2 * valid + 1, 0) = alpha; + A(2 * valid + 1, 1) = -omega * omega; + + b(2 * valid) = acc->x(); + b(2 * valid + 1) = acc->y(); + valid++; + } + usleep(100000); + } + if (valid < 10) + { + LOG(WARNING) << "Calibration failed: insufficient valid samples"; + return Eigen::Vector2d(0.0, 0.0); + } + Eigen::MatrixXd A_valid = A.topRows(2 * valid); + Eigen::VectorXd b_valid = b.topRows(2 * valid); + Eigen::Vector2d X = + A_valid.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_valid); + return X; +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h new file mode 100644 index 0000000000..9053c7c102 --- /dev/null +++ b/src/software/embedded/services/imu.h @@ -0,0 +1,121 @@ +#pragma once + +#include +#include +#include +#include + +#include "proto/tbots_software_msgs.pb.h" +#include "software/geom/angular_acceleration.h" +#include "software/geom/angular_velocity.h" + +/** + * Handles low level IMU I2C communication, and some minor offset filtering. + */ +struct ImuData +{ + std::optional angular_velocity; + std::optional angular_acceleration; + std::optional linear_acceleration; +}; +class ImuService +{ + public: + /** + * Constructs and initializes a new IMU service object. + * + * If successfully initialized, will try to do a simple calibration of the IMU. + */ + ImuService(); + + std::optional poll(); + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * (4.0 * 14.4222 / 1000.0 * M_PI / 180.0); + + private: + /** + * Polls the latest IMU reading of the angular velocity of the robot on the z axis + * @return the current angular velocty of the robot on the z axis + */ + std::optional pollAngularVelocity(); + /** + * Computes angular acceleration from successive angular velocity readings + * @return the current angular acceleration of the robot on the z axis + */ + std::optional pollAngularAcceleration( + std::optional curr_angular_velocity); + /** + * Polls the latest IMU reading of the linear acceleration of the robot on the z plane + * @return the current linear acceleration of the robot on the z plane + */ + std::optional pollLinearAcceleration(); + /** + * Reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); + /** + * Transform linear acceleration to robot center of mass + * @parama omega angular veocity of the robot + * @param alpha angular acceleration of the robot + * @parama imu_acceleration lianer acceleration of imu + * @return the linear acceleration of the robot center of mass + */ + Eigen::Vector2d transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d imu_acceleration); + /** + * Finds the deviation of the imu from the robot center of mass by taking 100 data + * points, applying a least square regression on the relative acceleration formula, + * and solving for the distance + * @return the deviation of the imu from the robot center of mass + */ + Eigen::Vector2d calibrate_imu(); + + + bool initialized_ = false; + + int file_descriptor_ = 0; + + double degrees_error_; + + // Maps the maximum raw reading from 16-bit integer to be 2 times gravity + static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; + // Same for gyroscope, 1000 degrees per second + static constexpr double IMU_FULL_SCALE_DPS = 1000.0; + + /// Various i2c registers. + static const uint8_t WHOAMI_REG = 0xf; + static const uint8_t ACCEL_CONTROL_REG = 0x10; + static const uint8_t GYRO_CONTROL_REG = 0x11; + static const uint8_t CTRL4_C = 0x13; + static const uint8_t CTRL6_C = 0x15; + static const uint8_t CTRL8_XL = 0x17; + + // Device path for the IMU + inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + + // Gyroscope Z-axis (Yaw) Output Data Registers + static constexpr uint8_t GYRO_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t GYRO_MOST_SIG_REG = 0x27; // OUTZ_H_G + + // Accelerometer X-axis Output Data Registers + static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL + static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL + + // Accelerometer Y-axis Output Data Registers + static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL + static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL + + // IMU offset for acceleration calculation + static constexpr double IMU_OFFSET_X = 0.0; + static constexpr double IMU_OFFSET_Y = 0.0; + + // prev time and angular acceleration + std::optional prev_angular_velocity_; + std::chrono::steady_clock::time_point prev_time_; +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 00a76d5717..c9a329e087 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -10,6 +10,7 @@ #include "shared/constants.h" #include "software/constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" @@ -123,7 +124,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, motor_service_ = std::make_unique(robot_constants, loop_hz); g_motor_service = motor_service_.get(); motor_service_->setup(); - LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; + + LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; + + imu_service_ = std::make_unique(); + LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ << ", CHANNEL ID: " << channel_id_ @@ -179,6 +184,15 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); + + std::optional imu_poll = imu_service_->poll(); + + // TODO (3725): Replace with actual IMU data usage + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) + { + LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); + } + for (;;) { struct timespec time_since_prev_iter; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index cf70a12b68..a7404f8b65 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -10,6 +10,7 @@ #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" @@ -60,6 +61,7 @@ class Thunderloop std::unique_ptr motor_service_; std::unique_ptr network_service_; std::unique_ptr power_service_; + std::unique_ptr imu_service_; // TOML config client std::unique_ptr toml_config_client_; From 68182df5cf234b9bc7f8e4f52c98b41585c6675d Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sat, 30 May 2026 18:08:30 -0700 Subject: [PATCH 37/38] Tune constants --- src/shared/robot_constants.h | 6 ++--- .../stspin_motor_controller.h | 8 +++---- src/software/embedded/primitive_executor.cpp | 23 ------------------- src/software/embedded/primitive_executor.h | 18 +++++++-------- src/software/embedded/thunderloop.cpp | 9 -------- src/software/util/pid/pid_controller.hpp | 2 +- 6 files changed, 17 insertions(+), 49 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 6ba5b92be3..ef302965e0 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -113,13 +113,13 @@ constexpr RobotConstants createRobotConstants() .max_force_dribbler_speed_rpm = -12000, // Motor constant - .motor_max_acceleration_m_per_s_2 = 2.0f, + .motor_max_acceleration_m_per_s_2 = 4.0f, // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, .robot_max_speed_trajectory_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, - .robot_max_deceleration_m_per_s_2 = 1.5f, + .robot_max_acceleration_m_per_s_2 = 2.5f, + .robot_max_deceleration_m_per_s_2 = 2.0f, // Robot's angular movement constants .robot_max_ang_speed_rad_per_s = 6.0f, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 8402db8080..2a86e09884 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -57,11 +57,11 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; - static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700; - static constexpr int SPEED_PID_INTEGRAL_GAIN = 30; + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 1300; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 40; - static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; - static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; + static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 800; + static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 250; robot_constants::RobotConstants robot_constants_; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 2fa21c80f6..5732281874 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -247,10 +247,6 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { target_angular_velocity = AngularVelocity::fromRadians( w_pid_close.step(error_angular.toRadians())); - - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"is_pure_pid", 100}, - }); } else { @@ -265,27 +261,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians())); target_angular_velocity = trajectory_angular_velocity + pid_effort_angular; - - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"is_pure_pid", 0}, - }); } - LOG(PLOTJUGGLER) << *createPlotJugglerValue({ - {"target_angular_velocity", target_angular_velocity.toDegrees()}, - {"angular_velocity", angular_velocity_.toDegrees()}, - {"orientation", orientation_.toDegrees()}, - {"expected_orientation", - angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.count()) - .toDegrees()}, - }); - - // Make sure target linear velocity is clamped - target_linear_velocity = target_linear_velocity.normalize( - std::min(target_linear_velocity.length(), - static_cast(robot_constants_.robot_max_speed_m_per_s))); - const auto prim = createDirectControlPrimitive( target_linear_velocity, target_angular_velocity, convertDribblerModeToDribblerSpeed( diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 9b39f68d84..01ad540a12 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -104,25 +104,25 @@ class PrimitiveExecutor robot_constants::RobotConstants robot_constants_; - controls::PIDController x_pid = {0.8, 0, 0, 0}; - controls::PIDController y_pid = {0.8, 0, 0, 0}; - controls::PIDController w_pid = {0.7, 0, 2, 0}; + controls::PIDController x_pid = {0, 0, 0, 0}; + controls::PIDController y_pid = {0, 0, 0, 0}; + controls::PIDController w_pid = {0.5, 0, 0, 0}; // When close to target position, ignore trajectory velocity and use pure PID control. // These PIDs should be used in that case. - controls::PIDController x_pid_close = {2, 0, 0, 0}; - controls::PIDController y_pid_close = {2, 0, 0, 0}; - controls::PIDController w_pid_close = {2, 0, 0, 0}; + controls::PIDController x_pid_close = {2.0, 0.01, 0, 1}; + controls::PIDController y_pid_close = {3.0, 0.01, 0, 1}; + controls::PIDController w_pid_close = {0.3, 0.03, 2, 3}; // If distance between current linear trajectory destination and new one is larger // than this, we change trajectories. static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = .4; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 25; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.25; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 45; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.25; static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 45; // The distance away from the destination at which we start dampening the velocity diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 0435347db2..5b3294fd1b 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -187,15 +187,6 @@ void Thunderloop::runLoop() robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - - std::optional imu_poll = imu_service_->poll(); - - // TODO (3725): Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) - { - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } - for (;;) { struct timespec time_since_prev_iter; diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp index e52c66a011..86ebd0d23f 100644 --- a/src/software/util/pid/pid_controller.hpp +++ b/src/software/util/pid/pid_controller.hpp @@ -53,7 +53,7 @@ T controls::PIDController::step(T error, T delta_time) integral += std::max(std::min(error * delta_time, max_integral), -max_integral); // set derivative, if no last_error, just set to 0 - const T derivative = (last_error.value_or(error) - error) / delta_time; + const T derivative = (error - last_error.value_or(error)) / delta_time; last_error = error; return error * k_p + integral * k_i + derivative * k_d; } From 177cf2ce10d6f9f700bccbb7592ab4933e4bbdba Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 31 May 2026 14:48:31 -0700 Subject: [PATCH 38/38] Remove runaway protection in MotorService --- src/software/embedded/services/motor.cpp | 29 ------------------------ src/software/embedded/services/motor.h | 1 - 2 files changed, 30 deletions(-) diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 0c6515f5cf..132e94e681 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -138,35 +138,6 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor TbotsProto::MotorStatus motor_status = createMotorStatus(current_wheel_velocities, dribbler_rpm); - if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Front right motor runaway"; - } - else if (std::abs(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_LEFT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Front left motor runaway"; - } - else if (std::abs(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[BACK_LEFT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back left motor runaway"; - } - else if (std::abs(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[BACK_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back right motor runaway"; - } - // Convert to Euclidean velocity_delta const EuclideanSpace_t current_euclidean_velocity = euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 445fb03ff5..7a882be8e1 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -102,6 +102,5 @@ class MotorService static constexpr int MOTOR_RESET_TIME_THRESHOLD_S = 60; static constexpr int MOTOR_RESET_THRESHOLD_COUNT = 3; - static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; };