Odometry cv

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<OdometryData*>(message.data);
+    // data->xPos = odometry->getPosition().getX();
     message.setCRC16();
     drivers->uart.write(UART_PORT, reinterpret_cast<uint8_t*>(&message), sizeof(message));
 }

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;

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;

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})};
 };

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

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