From 109925dd4666ac26f4cbe1a89e0878a39a0af43a Mon Sep 17 00:00:00 2001 From: suhas Date: Fri, 6 Dec 2024 18:08:18 -0600 Subject: [PATCH 1/2] start --- .../odometry/observer_displacement.cpp | 50 +++++++++++++------ .../odometry/observer_displacement.hpp | 1 + 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp index c08528fd..2ebd44e4 100644 --- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp +++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp @@ -1,5 +1,12 @@ #include "subsystems/odometry/observer_displacement.hpp" +#include "tap/architecture/clock.hpp" + +/* + * Notes: + * https://gitlab.com/aruw/controls/aruw-mcb/-/tree/develop/aruw-mcb-project/src/aruwsrc/algorithms/odometry?ref_type=heads + * */ + namespace subsystems::odometry { @@ -20,7 +27,18 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement( modm::Vector3f* const velocity, modm::Vector3f* const displacement) const { - bmi088::Bmi088* imu = &drivers->bmi088; + uint32_t currTime = tap::arch::clock::getTimeMicroseconds(); + if (prevTime != 0) + { + velocity = chassis->measureVelocity(); + // velocity.set(chassisRelVel.x, chassisRelVel.y); + + // displacement->move(tickDisp); + displacement += velocity * (static_cast(currTime - prevTime) / 1'000'000.0f); + } + + prevTime = currTime; + // bmi088::Bmi088* imu = &drivers->bmi088; // Attempt integration with Velocity Verlet // a(t) = last acceleration, a(t + dt) = current acceleration @@ -29,25 +47,25 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement( // TODO: Depending on when this subsystem gets initialized, // the first time this function runs, deltaT might be large - auto nowTime = imu->getPrevIMUDataReceivedTime(); // Units of us - auto dt = (nowTime - lastTime) / 1e6f; // Want units of s + // auto nowTime = imu->getPrevIMUDataReceivedTime(); // Units of us + // auto dt = (nowTime - lastTime) / 1e6f; // Want units of s - // z is 0 since we're moving on the x-y plane and gravity affects z - Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f}; - Vector3f nowDisp = lastDisp + lastVel * dt + lastAcc * dt * dt / 2.0f; - Vector3f nowVel = lastVel + (lastAcc + nowAcc) * dt / 2.0f; + // // z is 0 since we're moving on the x-y plane and gravity affects z + // Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f}; + // Vector3f nowDisp = lastDisp + lastVel * dt + lastAcc * dt * dt / 2.0f; + // Vector3f nowVel = lastVel + (lastAcc + nowAcc) * dt / 2.0f; - // Update by copy - lastTime = nowTime; - lastAcc = nowAcc; - lastVel = nowVel; - lastDisp = nowDisp; + // // Update by copy + // lastTime = nowTime; + // lastAcc = nowAcc; + // lastVel = nowVel; + // lastDisp = nowDisp; - // Return - *velocity = nowVel; - *displacement = nowDisp; + // // Return + // *velocity = nowVel; + // *displacement = nowDisp; - return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED; + // return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED; } } // namespace subsystems::odometry \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp index 42626172..a6f1b88d 100644 --- a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp +++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp @@ -28,6 +28,7 @@ class ChassisDisplacementObserver : public ChassisDisplacementObserverInterface mutable Vector3f lastVel; // m/s mutable Vector3f lastDisp; // m mutable uint32_t lastTime; // ms + mutable uint32_t prevTime = 0; }; } // namespace subsystems::odometry From 5e7bc33bf0aa900520f525eec77f10e2e4b8a399 Mon Sep 17 00:00:00 2001 From: suhas Date: Fri, 7 Feb 2025 17:38:04 -0600 Subject: [PATCH 2/2] push --- .idea/.gitignore | 8 ++++++++ .idea/discord.xml | 7 +++++++ .idea/modules.xml | 8 ++++++++ .idea/robomaster.iml | 9 +++++++++ .idea/vcs.xml | 6 ++++++ .../subsystems/odometry/observer_displacement.cpp | 12 +++++++++--- 6 files changed, 47 insertions(+), 3 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/discord.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/robomaster.iml create mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 00000000..13566b81 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/discord.xml b/.idea/discord.xml new file mode 100644 index 00000000..d8e95616 --- /dev/null +++ b/.idea/discord.xml @@ -0,0 +1,7 @@ + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 00000000..dade4032 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/robomaster.iml b/.idea/robomaster.iml new file mode 100644 index 00000000..d6ebd480 --- /dev/null +++ b/.idea/robomaster.iml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 00000000..35eb1ddf --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp index 2ebd44e4..5a3f622f 100644 --- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp +++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp @@ -30,12 +30,17 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement( uint32_t currTime = tap::arch::clock::getTimeMicroseconds(); if (prevTime != 0) { - velocity = chassis->measureVelocity(); + modm::Vector3f myv = chassis->measureVelocity(); + velocity->set(myv.x, myv.y, myv.z); + // velocity.set(chassisRelVel.x, chassisRelVel.y); // displacement->move(tickDisp); - displacement += velocity * (static_cast(currTime - prevTime) / 1'000'000.0f); - } + float deltaT = (static_cast(currTime - prevTime)) / 1'000'000.0f; + modm::Vector3f myDisp = myv * deltaT; + displacement->set(myDisp.x, myDisp.y, myDisp.z); + // displacement += velocity * (static_cast(currTime - prevTime) / 1'000'000.0f); + } prevTime = currTime; // bmi088::Bmi088* imu = &drivers->bmi088; @@ -66,6 +71,7 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement( // *displacement = nowDisp; // return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED; + return true; } } // namespace subsystems::odometry \ No newline at end of file