upgrade taproot

ut-robomaster/src/communication/cv_board.cpp

@@ -24,6 +24,11 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message)
         case CV_MESSAGE_TYPE_TURRET_AIM:
             decodeTurretData(message);
             break;
+        // case CV_MESSAGE_TYPE_ECHO:
+        // echoData(message);
+        case CV_MESSAGE_TYPE_POSITION_REQUEST:
+            decodePositionRequest(message);
+            break;
         default:
             break;
     }
@@ -35,6 +40,22 @@ void CVBoard::sendMessage()
     sendColorData();
 }
 
+//  void CVBoard::echoData(const ReceivedSerialMessage& message)
+// {
+//     std::string data_string;
+//     memcpy(&data_string, &message.data, sizeof(message.header.dataLength));
+
+//     DJISerial::SerialMessage<sizeof(data_string)> msg;
+//     msg.messageType = CV_MESSAGE_TYPE_ECHO;
+
+//     std::string* data = reinterpret_cast<std::string*>(msg.data);
+
+//     data = &data_string;
+
+//     msg.setCRC16();
+//     drivers->uart.write(UART_PORT, reinterpret_cast<uint8_t*>(&msg), sizeof(msg));
+// }
+
 void CVBoard::sendOdometryData()
 {
     DJISerial::SerialMessage<sizeof(OdometryData)> message;
@@ -77,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

@@ -43,6 +43,11 @@ class CVBoard : public tap::communication::serial::DJISerial
      */
     void sendMessage();
 
+    /**
+     * Echo the message sent by the CV board to the CV board
+     */
+    void echoData(const ReceivedSerialMessage& message);
+
     /**
      * Sends odometry data to the CV board
      */
@@ -61,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
@@ -84,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

@@ -1,6 +1,8 @@
 #pragma once
 #include <stdint.h>
 
+#include <string>
+
 namespace communication
 {
 
@@ -9,6 +11,8 @@ enum MessageTypes : uint8_t
     CV_MESSAGE_TYPE_ODOMETRY_DATA = 1,
     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
@@ -35,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/main.cpp

@@ -1,5 +1,6 @@
 #include "tap/architecture/periodic_timer.hpp"
 #include "tap/architecture/profiler.hpp"
+#include "tap/motor/motorsim/dji_motor_sim_handler.hpp"
 
 #include "robots/robot_constants.hpp"
 #include "robots/robot_control.hpp"
@@ -24,7 +25,7 @@ static void initializeIo(src::Drivers *drivers)
     drivers->schedulerTerminalHandler.init();
     drivers->djiMotorTerminalSerialHandler.init();
     drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI);
-    drivers->bmi088.requestRecalibration();
+    drivers->bmi088.requestCalibration();
 }
 
 // Anything that you would like to be called place here. It will be called
@@ -33,7 +34,7 @@ static void initializeIo(src::Drivers *drivers)
 static void updateIo(src::Drivers *drivers)
 {
 #ifdef PLATFORM_HOSTED
-    tap::motorsim::SimHandler::updateSims();
+    tap::motor::motorsim::DjiMotorSimHandler::getInstance()->updateSims();
 #endif
 
     drivers->canRxHandler.pollCanData();

ut-robomaster/src/robots/hero/hero_constants.hpp

@@ -2,6 +2,7 @@
 
 #include "tap/algorithms/smooth_pid.hpp"
 #include "tap/communication/can/can_bus.hpp"
+#include "tap/communication/serial/ref_serial.hpp"
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
@@ -13,6 +14,7 @@ using motor_controller::PidConstants;
 using tap::can::CanBus;
 using namespace tap::motor;
 using namespace motors;
+using tap::communication::serial::RefSerial;
 
 // General constants ------------------------------------------------
 
@@ -41,10 +43,12 @@ constexpr float CAMERA_TO_PITCH = 0.0f;    // distance from main camera lens to
 constexpr float NOZZLE_TO_PITCH = 0.0f;    // distance from barrel nozzle to pitch axis (m)
 constexpr float CAMERA_TO_BARRELS = 0.0f;  // vertical ctc offset from camera lens to barrel (m)
 constexpr float CAMERA_X_OFFSET = 0.0f;    // horizontal offset of main camera lens (m)
-constexpr float PITCH_MIN = -0.3349f;      // rad
-constexpr float PITCH_MAX = 0.3534f;       // rad
+constexpr float PITCH_MIN = 0.15f;         // rad
+constexpr float PITCH_MAX = 0.60f;         // rad
+const float YAW_OFFSET = 0;                // deprecated on hero until gear ratios are fixed
+const float PITCH_OFFSET = 1.90f;
 
-static constexpr float YAW_REDUCTION = 1.0f;
+static constexpr float YAW_REDUCTION = 2.005459491f;
 static constexpr float PITCH_REDUCTION = 1.0f;
 
 // Tuning Constants -----------------------------------
@@ -95,8 +99,8 @@ static constexpr float YAW_INPUT_SCALE = 4.0f;
 static constexpr float PITCH_INPUT_SCALE = 5.0f;
 #endif
 
-static constexpr float MOUSE_SENS_YAW = 0.0045f;
-static constexpr float MOUSE_SENS_PITCH = 0.002f;
+static constexpr float MOUSE_SENS_YAW = 0.0090f;
+static constexpr float MOUSE_SENS_PITCH = 0.0025f;
 
 // Motor Constants -------------------------------------
 
@@ -105,28 +109,60 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
 constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;
 
 // chassis
-const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};
-
-// flywheels
 const MotorConfig
-    FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, false, "flywheel left", PID_FLYWHEEL, {}};
+    WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false};
 const MotorConfig
-    FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}};
+    WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false};
+
+// flywheels
+const MotorConfig FLYWHEEL_L{
+    M3508_NOGEARBOX,
+    MOTOR3,
+    CAN_SHOOTER,
+    false,
+    "flywheel left",
+    PID_FLYWHEEL,
+    {},
+    false};
+const MotorConfig FLYWHEEL_R{
+    M3508_NOGEARBOX,
+    MOTOR4,
+    CAN_SHOOTER,
+    true,
+    "flywheel right",
+    PID_FLYWHEEL,
+    {},
+    false};
 
 // agitator
-const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
-const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}};
+const MotorConfig AGITATOR{
+    M3508,
+    MOTOR1,
+    CAN_SHOOTER,
+    false,
+    "agitator",
+    PID_AGITATOR,
+    {},
+    false};  // water wheel
+const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}, false};
 
 // turret
+const MotorConfig YAW_L{
+    M3508,
+    MOTOR5,
+    CAN_TURRET,
+    false,
+    "yaw left",
+    {2.5f, 60.0f, 0.0f},
+    {45.0f, 0.0f, 0.0f},
+    false};
+const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}, false};
 const MotorConfig
-    YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
-const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
-const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
-const float YAW_OFFSET = 0;
-const float PITCH_OFFSET = 0;
+    PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false};
 
 // Velocities ----------------------------
 
@@ -153,4 +189,21 @@ const float UNJAM_SPEED = 12.0f;          // rev/s
 
 // Heat Buffers ---------------------------
 
-const uint16_t BARREL_HEAT_BUFFER = 100.0f;
+const uint16_t BARREL_HEAT_BUFFER_1V1 = 100;
+const uint16_t BARREL_HEAT_BUFFER_3V3[10] = {
+    100,  // Level 1
+    100,  // Level 2
+    100,  // Level 3
+    100,  // Level 4
+    100,  // Level 5
+    100,  // Level 6
+    100,  // Level 7
+    100,  // Level 8
+    100,  // Level 9
+    100   // Level 10
+};
+
+constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true;
+const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE =
+    RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3;
+const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1;

ut-robomaster/src/robots/robot_constants.hpp

@@ -10,8 +10,8 @@ const uint32_t REFRESH_PERIOD = 2;              // ms
 constexpr float DT = REFRESH_PERIOD / 1000.0f;  // refresh delta time (s)
 
 const float ANALOG_DEAD_ZONE = 0.1f;
-const float KEYBOARD_ACCEL = 5.0f;
-const float KEYBOARD_DECEL = 5.0f;
+const float KEYBOARD_ACCEL = 3.5f;
+const float KEYBOARD_DECEL = 3.5f;
 const float CHASSIS_AUTOALIGN_FACTOR = 0.5f;
 const float CHASSIS_AUTOALIGN_ANGLE = M_PI;