Merge branch 'odometry_cv' of https://github.com/ut-ras/robomaster into odometry_cv

Author: vinle0

.idea/.gitignore

@@ -11,7 +11,9 @@ class StandardControl : CommonControlManual
         CommonControlManual::initialize();
 
         agitator.initialize();
+        odometry.initialize();
         drivers->commandScheduler.registerSubsystem(&agitator);
+        drivers->commandScheduler.registerSubsystem(&odometry);
 
         drivers->commandMapper.addMap(&leftMouseDown);
         drivers->commandMapper.addMap(&leftSwitchUp);
@@ -20,6 +22,7 @@ class StandardControl : CommonControlManual
 private:
     // Subsystems
     AgitatorSubsystem agitator{drivers, &flywheel, AGITATOR};
+    OdometrySubsystem odometry{drivers, &chassis, &turret};
 
     // Commands
     CommandAgitatorContinuous rotateAgitator_LeftMouse{drivers, &agitator, BarrelId::STANDARD1};

.idea/discord.xml

@@ -1,4 +1,10 @@
 #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 +26,26 @@ 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)
+    {
+        modm::Vector3f myv = chassis->measureVelocity();
+        velocity->set(myv.x, myv.y, myv.z);
+
+        //  velocity.set(chassisRelVel.x, chassisRelVel.y);
+        // displacement->move(tickDisp);
+        float deltaT = (static_cast<float>(currTime - prevTime)) / 1'000'000.0f;
+        modm::Vector3f myDisp = lastDisp + *velocity * DT;
+        lastDisp = myDisp;
+        displacement->set(myDisp.x, myDisp.y, myDisp.z);
+        
+        // myDisp = *displacement + *velocity * (static_cast<float>(currTime - prevTime) / 1'000'000.0f);
+        // displacement->set(myDisp * (static_cast<float>(currTime - prevTime) / 1'000'000.0f));
+        // 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 +54,26 @@ 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;
+    return true;
 }
 
 }  // namespace subsystems::odometry

.idea/modules.xml

@@ -2,13 +2,14 @@
 #define SUBSYSTEMS_ODOMETRY_OBSERVER_DISPLACEMENT_HPP_
 
 #include "tap/algorithms/odometry/chassis_displacement_observer_interface.hpp"
-
 #include "subsystems/chassis/chassis_subsystem.hpp"
 
 #include "drivers.hpp"
 
+
 namespace subsystems::odometry
 {
+
 using modm::Vector3f;
 using tap::algorithms::odometry::ChassisDisplacementObserverInterface;
 using namespace tap::communication::sensors::imu;
@@ -28,6 +29,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
 

.idea/robomaster.iml

@@ -21,11 +21,24 @@ void OdometrySubsystem::initialize() {};
 
 void OdometrySubsystem::refresh()
 {
+    Vector3f lastAcc;  // m/s^2
+
     setAmputated(!hardwareOk());
     if (!isAmputated())
     {
         chassisTracker.update();
     }
+
+    // Vector3f velocity = chassis->measureVelocity();   // m/s^2
+    // chassisDisplacement.getVelocityChassisDisplacement(&velocity, &lastAcc);
+
+    if (refreshTimer.execute())
+    {
+
+        drivers->rtt.plot(3, getPosition().getX()); //problematic function
+        drivers->rtt.plot(2, getPosition().getY()); //problematic function
+        drivers->rtt.plot(1, getLinearVelocity().getX());
+    }
 }
 
 bool OdometrySubsystem::hardwareOk() { return chassis->hardwareOk() && turret->hardwareOk(); }

.idea/vcs.xml

@@ -4,6 +4,8 @@
 #include "tap/algorithms/odometry/odometry_2d_interface.hpp"
 #include "tap/algorithms/odometry/odometry_2d_tracker.hpp"
 #include "tap/control/subsystem.hpp"
+#include "tap/architecture/periodic_timer.hpp"
+
 
 #include "modm/math/geometry.hpp"
 #include "modm/math/geometry/location_2d.hpp"
@@ -48,5 +50,6 @@ class OdometrySubsystem : public Subsystem
     ChassisDisplacementObserver chassisDisplacement;
     ChassisWorldYawObserver chassisYaw;
     Odometry2DTracker chassisTracker;
+    tap::arch::PeriodicMilliTimer refreshTimer{100};
 };
 }  // namespace subsystems::odometry