finish

Author: tildezero

ut-robomaster/src/communication/cv_board.cpp

@@ -26,6 +26,9 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message)
             break;
         // case CV_MESSAGE_TYPE_ECHO:
         // echoData(message);
+        case CV_MESSAGE_TYPE_POSITION_REQUEST:
+            decodePositionRequest(message);
+            break;
         default:
             break;
     }
@@ -37,7 +40,7 @@ void CVBoard::sendMessage()
     sendColorData();
 }
 
-// void CVBoard::echoData(const ReceivedSerialMessage& message)
+//  void CVBoard::echoData(const ReceivedSerialMessage& message)
 // {
 //     std::string data_string;
 //     memcpy(&data_string, &message.data, sizeof(message.header.dataLength));
@@ -95,6 +98,17 @@ bool CVBoard::decodeTurretData(const ReceivedSerialMessage& message)
     return false;
 }
 
+bool CVBoard::decodePositionRequest(const ReceivedSerialMessage& message)
+{
+    if (message.header.dataLength == sizeof(lastPositionRequest))
+    {
+        memcpy(&lastPositionRequest, &message.data, sizeof(lastPositionRequest));
+        return true;
+    }
+    return false;
+}
+
 bool CVBoard::isOnline() const { return !offlineTimeout.isExpired(); }
 const TurretData& CVBoard::getTurretData() const { return lastTurretData; }
+const PositionRequest& CVBoard::getPositionRequest() const { return lastPositionRequest; }
 }  // namespace communication

ut-robomaster/src/communication/cv_board.hpp

@@ -46,7 +46,7 @@ class CVBoard : public tap::communication::serial::DJISerial
     /**
      * Echo the message sent by the CV board to the CV board
      */
-    // void echoData(const ReceivedSerialMessage& message);
+    void echoData(const ReceivedSerialMessage& message);
 
     /**
      * Sends odometry data to the CV board
@@ -66,6 +66,18 @@ class CVBoard : public tap::communication::serial::DJISerial
      */
     bool decodeTurretData(const ReceivedSerialMessage& message);
 
+    /**
+     * Decode a request to move to a position.
+     * @param message message from CV board
+     * @return true if valid, false otherwise
+     */
+    bool decodePositionRequest(const ReceivedSerialMessage& message);
+
+    /**
+     * Fetch the position request from the CV board
+     */
+    const PositionRequest& getPositionRequest() const;
+
     /**
      * @return true if a message has been received within the last OFFLINE_TIMEOUT_MS milliseconds,
      * false if otherwise
@@ -89,6 +101,11 @@ class CVBoard : public tap::communication::serial::DJISerial
     /** Last turret aiming data received from the CV board */
     TurretData lastTurretData;
 
+    PositionRequest lastPositionRequest = {
+        .x_requested = -999.9f,
+        .y_requested = -999.9f,
+        .time_sent = 0xdeadbeef};
+
     /** UART port used to communicate with the CV board */
     static constexpr Uart::UartPort UART_PORT = Uart::Uart1;
 

ut-robomaster/src/communication/cv_message.hpp

@@ -12,6 +12,7 @@ enum MessageTypes : uint8_t
     CV_MESSAGE_TYPE_TURRET_AIM = 2,
     CV_MESSAGE_TYPE_COLOR_DATA = 3,
     CV_MESSAGE_TYPE_ECHO = 4,
+    CV_MESSAGE_TYPE_POSITION_REQUEST = 5,
 };
 
 enum ColorTypes : uint8_t
@@ -38,6 +39,13 @@ struct TurretData
     bool hasTarget;
 } modm_packed;
 
+struct PositionRequest
+{
+    float x_requested;
+    float y_requested;
+    uint64_t time_sent;
+} modm_packed;
+
 struct OdometryData
 {
     float xPos;

ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp

@@ -3,10 +3,11 @@
 namespace commands
 {
 
+using namespace communication;
 // old ard code
-// void CommandSentryPosition::initialize() { moveTimer.stop(); }
+void CommandSentryPosition::initialize() { moveTimer.stop(); }
 
-void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); }
+// void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); }
 
 void CommandSentryPosition::execute()
 {
@@ -56,18 +57,22 @@ void CommandSentryPosition::execute()
 
     // chassis->input(inputMove, inputSpin);
 
-    // if (drivers->isGameActive() && moveTimer.isStopped())
-    // {
-    //     moveTimer.restart(10'000);  // 10s
-    // }
+    if (drivers->isGameActive() && moveTimer.isStopped())
+    {
+        moveTimer.restart(5'000);  // 5 secs
+    }
 
-    // if (!moveTimer.isExpired())
-    // {
-    //     chassis->input(Vector2f(0.0f), 0.0f);
-    //     return;
-    // }
+    if (!moveTimer.isExpired())
+    {
+        chassis->input(Vector2f(0.0f), 1.0f);  // spin!
+        return;
+    }
+
+    PositionRequest pos = drivers->cvBoard.getPositionRequest();
+    drivers->rtt << "x_pos = " << pos.x_requested << ", y_pos = " << pos.y_requested
+                 << ", time_sent = " << (float)pos.time_sent;
 
-    // // speen
+    // speen
     chassis->input(Vector2f(0.0f), 1.0f);
 }
 

ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp

@@ -1,12 +1,11 @@
 #pragma once
 
-//#include "tap/architecture/timeout.hpp"
+// #include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 
-
 #include "robots/robot_constants.hpp"
-//this is older sentry code dont know why it had chopped path
-//#include "chassis_subsystem.hpp"  
+// this is older sentry code dont know why it had chopped path
+// #include "chassis_subsystem.hpp"
 #include "subsystems/chassis/chassis_subsystem.hpp"
 #include "subsystems/turret/turret_subsystem.hpp"
 #include "utils/chassis_auto_align.hpp"
@@ -18,8 +17,8 @@ namespace commands
 using namespace tap::communication::serial;
 using namespace modm;
 using subsystems::chassis::ChassisSubsystem;
-// using tap::arch::MilliTimeout;
 using subsystems::turret::TurretSubsystem;
+using tap::arch::MilliTimeout;
 
 class CommandSentryPosition : public tap::control::Command
 {
@@ -52,7 +51,7 @@ class CommandSentryPosition : public tap::control::Command
 private:
     src::Drivers *drivers;
     ChassisSubsystem *chassis;
-    //MilliTimeout moveTimer;
+    MilliTimeout moveTimer;
     TurretSubsystem *turret;
 
     Vector2f keyboardInputMove = Vector2f(0.0f);

ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp

@@ -7,6 +7,27 @@ void CommandSentryAim::initialize() {}
 void CommandSentryAim::execute()
 {
     // turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f);
+    if (turret->getTargetLocalYaw() <= -55.0f)
+    {
+        turret->setTargetWorldAngles(50.0f, 10.0f);
+    }
+    else
+    {
+        turret->setTargetWorldAngles(-50.0f, -10.0f);
+    }
+
+    /*
+        PLAN (for suhas):
+        - see if setTargetWorldAngles() works as intended
+        - create targetWorldYaw/pitch variables
+        - twYaw should switch between -200 and 200 depending on what its set to
+            - if the angle is -200 set it to 200
+            - if the angle is 200 set it to -200
+            - else, keep current targetWorldAngle
+        - time how long it takes for pitch motor to oscillate
+        - use a timer to oscillate pitch to move ~10 deg to scan, or if that doesn't work, use the
+        - same logic to scan
+    */
 }
 
 void CommandSentryAim::end(bool) {}