Skip to content

Commit

Permalink
start
Browse files Browse the repository at this point in the history
tildezero committed Dec 7, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 8993ccf commit 109925d
Showing 2 changed files with 35 additions and 16 deletions.
50 changes: 34 additions & 16 deletions ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
Original file line number Diff line number Diff line change
@@ -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<float>(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
Original file line number Diff line number Diff line change
@@ -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

0 comments on commit 109925d

Please sign in to comment.