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/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp
index db545381..0e6ae1cb 100644
--- a/ut-robomaster/src/communication/cv_board.cpp
+++ b/ut-robomaster/src/communication/cv_board.cpp
@@ -41,7 +41,8 @@ void CVBoard::sendOdometryData()
message.messageType = CV_MESSAGE_TYPE_ODOMETRY_DATA;
// TODO: Implement sending of data once odometry module is finished
-
+ OdometryData* data = reinterpret_cast(message.data);
+ // data->xPos = odometry->getPosition().getX();
message.setCRC16();
drivers->uart.write(UART_PORT, reinterpret_cast(&message), sizeof(message));
}
diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp
index 89e00d0e..e968cfea 100644
--- a/ut-robomaster/src/communication/cv_board.hpp
+++ b/ut-robomaster/src/communication/cv_board.hpp
@@ -4,7 +4,7 @@
#include "tap/communication/serial/dji_serial.hpp"
#include "tap/communication/serial/uart.hpp"
#include "tap/util_macros.hpp"
-
+#include "subsystems/odometry/odometry_subsystem.hpp"
#include "cv_message.hpp"
namespace src
@@ -14,6 +14,9 @@ class Drivers;
namespace communication
{
using tap::communication::serial::Uart;
+using subsystems::odometry::OdometrySubsystem;
+using subsystems::chassis::ChassisSubsystem;
+using subsystems::turret::TurretSubsystem;
class CVBoard : public tap::communication::serial::DJISerial
{
@@ -83,6 +86,11 @@ class CVBoard : public tap::communication::serial::DJISerial
/** Last turret aiming data received from the CV board */
TurretData lastTurretData;
+
+ //used for getting odom data
+ ChassisSubsystem* chassis;
+ TurretSubsystem* turret;
+ OdometrySubsystem *odom;
/** UART port used to communicate with the CV board */
static constexpr Uart::UartPort UART_PORT = Uart::Uart1;
diff --git a/ut-robomaster/src/robots/common/common_control.hpp b/ut-robomaster/src/robots/common/common_control.hpp
index 6118e3f5..ba56189d 100644
--- a/ut-robomaster/src/robots/common/common_control.hpp
+++ b/ut-robomaster/src/robots/common/common_control.hpp
@@ -11,6 +11,7 @@
#include "subsystems/sound/command_play_sound.hpp"
#include "subsystems/sound/sound_subsystem.hpp"
#include "subsystems/turret/turret_subsystem.hpp"
+#include "subsystems/odometry/odometry_subsystem.hpp"
#include "utils/power_limiter/barrel_cooldown.hpp"
#include "drivers.hpp"
@@ -23,6 +24,7 @@ using namespace subsystems::agitator;
using namespace subsystems::flywheel;
using namespace subsystems::turret;
using namespace subsystems::sound;
+using namespace subsystems::odometry;
using namespace commands;
using power_limiter::BarrelId;
diff --git a/ut-robomaster/src/robots/standard/standard_control.hpp b/ut-robomaster/src/robots/standard/standard_control.hpp
index d9aa5d20..725e6331 100644
--- a/ut-robomaster/src/robots/standard/standard_control.hpp
+++ b/ut-robomaster/src/robots/standard/standard_control.hpp
@@ -1,5 +1,7 @@
#include "robots/common/common_control_manual.hpp"
#include "subsystems/agitator/command_agitator_continuous.hpp"
+#include "subsystems/chassis/command_move_point.hpp"
+
class StandardControl : CommonControlManual
{
@@ -11,15 +13,19 @@ 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);
+ drivers->commandMapper.addMap(&pressOdom);
}
private:
// Subsystems
AgitatorSubsystem agitator{drivers, &flywheel, AGITATOR};
+ OdometrySubsystem odometry{drivers, &chassis, &turret};
// Commands
CommandAgitatorContinuous rotateAgitator_LeftMouse{drivers, &agitator, BarrelId::STANDARD1};
@@ -28,6 +34,8 @@ class StandardControl : CommonControlManual
&agitator,
BarrelId::STANDARD1,
true};
+ CommandOdomPoint movePoint{
+ drivers, &chassis, &odometry};
// Mappings
HoldCommandMapping leftMouseDown{
@@ -39,4 +47,9 @@ class StandardControl : CommonControlManual
drivers,
{&rotateAgitator_SwitchUp, &rotateFlywheel_SwitchMid},
RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)};
+
+ PressCommandMapping pressOdom{
+ drivers,
+ {&movePoint},
+ RemoteMapState({Remote::Key::B})};
};
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/chassis/command_move_point.cpp b/ut-robomaster/src/subsystems/chassis/command_move_point.cpp
new file mode 100644
index 00000000..5355610b
--- /dev/null
+++ b/ut-robomaster/src/subsystems/chassis/command_move_point.cpp
@@ -0,0 +1,112 @@
+#include "command_move_point.hpp"
+
+namespace commands
+{
+
+void CommandOdomPoint::initialize()
+{
+ //initialize the target points, eventually read the waypoints from CV board
+ desiredX = 0.5f;
+ desiredY = 1.0f;
+
+}
+
+void CommandOdomPoint::execute()
+{
+ //?
+ Vector2f inputMove = Vector2f(0.0f, 0.0005f);
+
+ //get current position from odometry
+ currentPosition = odometry->getPosition();
+ currYaw = odometry->getChassisYaw();
+ currX = currentPosition.getX();
+ currY = currentPosition.getY();
+
+ //calculate the angle to the target point, for moving the chassis to the target point
+ //returns -pi to pi
+ desiredYaw = atan2f(desiredY - currY, desiredX - currX);
+
+ currYaw += M_PI;
+ desiredYaw += M_PI;
+
+ float outputRot, offset;
+ float integral = 0, deriv;
+ prevError = 0.0f;
+ //turn the chassis first until reached desiredYaw
+ //threshold is 0.1 radians
+ for(float error = desiredYaw - currYaw; abs(error) > angle_threshold; error = desiredYaw - currYaw){
+ float output;
+ outputRot = 0.0f;
+ output = error * p;
+ integral = (integral + error) * i;
+ deriv = (error - prevError) * d;
+
+ output = output + integral + deriv;
+
+ //need to spin the chassis CCW
+ if (output > 0.5f)
+ {
+ outputRot = -0.05f;
+ }
+ //need to spin the chassis CW
+ else if (output < -0.5f)
+ {
+ outputRot = 0.05f;
+ }
+
+ offset = 0.0f;
+ if(error > 1.0f){
+ offset = -0.05f;
+ }
+ else if(error < -1.0f){
+ offset = 0.05f;
+ }
+
+
+ chassis->input(Vector2f(0.0f), outputRot + offset); //rotate the chassis
+
+ prevError = error;
+ //wait for a bit
+ while(!refreshTimer.execute()){}
+
+ currYaw = odometry->getChassisYaw();
+ }
+
+ //now rotated to correct angle, move to the target point
+ chassis->input(Vector2f(0.0f), 0.0f); //stop the rotation
+
+ inputMove = inputMove.rotate(desiredYaw); //rotate the movement vector relative to the desired yaw
+ //move to the target point
+ chassis->input(inputMove, 0.0f);
+
+
+ //keep moving until reached the target point
+ //threshold is 0.1m
+ for(float dist = sqrt(pow(desiredX - currX, 2) + pow(desiredY - currY, 2)); dist > dist_threshold;
+ dist = sqrt(pow(desiredX - currX, 2) + pow(desiredY - currY, 2))){
+ currX = currentPosition.getX();
+ currY = currentPosition.getY();
+ }
+
+ inputMove.setX(0);
+ inputMove.setY(0);
+
+ chassis->input(inputMove, 0.0f);
+
+}
+
+void CommandOdomPoint::end(bool) {chassis->input(Vector2f(0.0f), 0.0f);}
+
+bool CommandOdomPoint::isFinished() const {
+ //check if the chassis is at the target point within some threshold
+ // if (fabs(currX - desiredX) < 0.1f && fabs(currY - desiredY) < 0.1f)
+ // {
+ // return true;
+ // }
+ // else
+ // {
+ // return false;
+ // }
+
+ }
+} // namespace commands
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/chassis/command_move_point.hpp b/ut-robomaster/src/subsystems/chassis/command_move_point.hpp
new file mode 100644
index 00000000..ecc689a1
--- /dev/null
+++ b/ut-robomaster/src/subsystems/chassis/command_move_point.hpp
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "tap/control/command.hpp"
+
+#include "subsystems/chassis/chassis_subsystem.hpp"
+#include "subsystems/odometry/odometry_subsystem.hpp"
+#include "communication/cv_message.hpp"
+#include "utils/chassis_auto_align.hpp"
+
+
+#include "drivers.hpp"
+
+namespace commands
+{
+using namespace modm;
+using subsystems::odometry::OdometrySubsystem;
+using subsystems::chassis::ChassisSubsystem;
+
+using communication::OdometryData;
+
+class CommandOdomPoint : public tap::control::Command
+{
+public:
+ CommandOdomPoint(src::Drivers *drivers, ChassisSubsystem* chassis, OdometrySubsystem *odom)
+ : drivers(drivers),
+ chassis(chassis),
+ odometry(odom)
+ {
+ addSubsystemRequirement(chassis);
+ }
+
+ void initialize() override;
+
+ void execute() override;
+
+ void end(bool interrupted) override;
+
+ bool isFinished() const override;
+
+ const char *getName() const override { return "rotate odom command"; }
+
+private:
+ src::Drivers *drivers;
+ ChassisSubsystem *chassis;
+ OdometrySubsystem *odometry;
+ Vector2f currentPosition;
+
+ const float p = 1.0f;
+ const float i = 1.0f;
+ const float d = 1.0f;
+ const float dist_threshold = 0.1f; //m
+ const float angle_threshold = 0.3f; //rads
+ float prevError = 0.0f;
+
+ float currX;
+ float currY;
+
+ float desiredX;
+ float desiredY;
+
+ float currYaw;
+ float desiredYaw;
+
+ tap::arch::PeriodicMilliTimer refreshTimer{10};
+
+}; // class CommandOdomPoint
+} // namespace commands
\ 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 c08528fd..c175881d 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
@@ -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(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(currTime - prevTime) / 1'000'000.0f);
+ // displacement->set(myDisp * (static_cast(currTime - prevTime) / 1'000'000.0f));
+ // 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 +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
\ 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..f4b397c3 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
@@ -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
diff --git a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp
index d572a51e..b959dc8a 100644
--- a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp
@@ -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(1, getPosition().getX());
+ drivers->rtt.plot(2, getPosition().getY());
+ // drivers->rtt.plot(3, getLinearVelocity().getX());
+ }
}
bool OdometrySubsystem::hardwareOk() { return chassis->hardwareOk() && turret->hardwareOk(); }
diff --git a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp
index 2de0faae..35bac240 100644
--- a/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp
+++ b/ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp
@@ -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
\ No newline at end of file