Derv/myswerve

chariot/inc/crt_chassis.hpp

@@ -21,9 +21,88 @@
 #pragma once
 
 /* Includes ------------------------------------------------------------------*/
+#include "crt_module.hpp"
+#include "para_chassis.hpp"
+#include "dvc_imu.hpp"
+#include "dvc_remotecontrol.hpp"
+#include "gsrl_common.h"
 
 /* Exported types ------------------------------------------------------------*/
 
+class CrtChassis
+{
+public:
+    // 底盘控制模式
+    enum ChassisMode
+    {
+        CHASSIS_NO_FORCE, // 无力/急停
+        MANUAL_CONTROL,   // 手动遥控
+        AUTO_CONTROL      // 自动/视觉控制
+    };
+
+    struct Config
+    {
+        CrtModule *module; // 舵轮模块实例（需外部管理生命周期）
+        float wheelX;      // 安装位置 X (m)
+        float wheelY;      // 安装位置 Y (m)
+        float wheelRadius; // 轮子半径 (m)，用于线速度转角速度
+    };
+
+    explicit CrtChassis(const std::vector<Config> &modules);
+
+    CrtChassis(MotorGM6020* front_SteerLeftMotor, MotorGM6020* front_SteerRightMotor,
+               MotorGM6020* back_SteerLeftMotor, MotorGM6020* back_SteerRightMotor,
+               MotorM3508* front_WheelLeftMotor, MotorM3508* front_WheelRightMotor,
+               MotorM3508* back_WheelLeftMotor, MotorM3508* back_WheelRightMotor,
+               IMU* imu);
+
+    CrtChassis() = default;
+
+    void init();
+    void setCmd(float vx, float vy, float wz);
+    void controlLoop(); // Task loop
+    void imuLoop();     // IMU update loop (high freq)
+    void receiveChassisMotorDataFromISR(const can_rx_message_t *rxMessage);
+    void receiveRemoteControlDataFromISR(const uint8_t *rxData);
+    void update(float dt);
+
+private:
+    std::vector<Config> m_modules; // 内部持有的模块列表
+
+    ChassisMode m_chassisMode;
+
+    float m_vx; // 缓存的目标 Vx
+    float m_vy; // 缓存的目标 Vy
+    float m_wz; // 缓存的目标 Wz
+
+    // 电机
+    MotorGM6020 *m_front_SteerLeftMotor;
+    MotorGM6020 *m_front_SteerRightMotor;
+    MotorGM6020 *m_back_SteerLeftMotor;
+    MotorGM6020 *m_back_SteerRightMotor;
+    MotorM3508 *m_front_WheelLeftMotor;
+    MotorM3508 *m_front_WheelRightMotor;
+    MotorM3508 *m_back_WheelLeftMotor;
+    MotorM3508 *m_back_WheelRightMotor;
+
+    // IMU
+    IMU *m_imu;
+    EulerAngle m_eulerAngle;
+
+    // 遥控器
+    Dr16RemoteControl m_remoteControl;
+
+    // 标志位
+    bool m_isInitComplete;
+
+private:
+    void modeSelect();
+    void targetSpeedplan();
+    void motorControl();
+    void transmitChassisMotorData();
+    inline fp32 rcStickDeadZoneFilter(const fp32 &rcStickValue);
+};
+
 /* Exported constants --------------------------------------------------------*/
 
 /* Exported macro ------------------------------------------------------------*/

chariot/src/crt_chassis.cpp

@@ -1,4 +1,4 @@
-/**
+/**
  *******************************************************************************
  * @file           : crt_chassis.cpp
  * @brief          : 舵轮底盘控制器实现
@@ -20,7 +20,10 @@
  */
 
 /* Includes ------------------------------------------------------------------*/
-
+#include "crt_chassis.hpp"
+#include "stm32f4xx_hal_can.h"
+#include "tsk_chassis.cpp"
+#include  "drv_misc.h"
 /* Typedef -------------------------------------------------------------------*/
 
 /* Define --------------------------------------------------------------------*/
@@ -32,3 +35,226 @@
 /* Function prototypes -------------------------------------------------------*/
 
 /* User code -----------------------------------------------------------------*/
+Chassis::Chassis(MotorGM6020* front_SteerLeftMotor, MotorGM6020* front_SteerRightMotor,
+                 MotorGM6020* back_SteerLeftMotor, MotorGM6020* back_SteerRightMotor,
+                 MotorM3508* front_WheelLeftMotor,MotorM3508* front_WheelRightMotor,
+                 MotorM3508* back_WheelLeftMotor,MotorM3508* back_WheelRightMotor,
+                 IMU* imu)
+    : m_front_SteerLeftMotor(front_SteerLeftMotor),
+      m_front_SteerRightMotor(front_SteerRightMotor),
+      m_back_SteerLeftMotor(back_SteerLeftMotor),
+      m_back_SteerRightMotor(back_SteerRightMotor),
+      m_front_WheelLeftMotor(front_WheelLeftMotor),
+      m_front_WheelRightMotor(front_WheelRightMotor),
+      m_back_WheelLeftMotor(back_WheelLeftMotor),
+      m_back_WheelRightMotor(back_WheelRightMotor),
+      m_imu(imu) {}
+
+void Chassis::init()
+{
+    DWT_Init();
+    CAN_Init(&hcan1, can1RxCallback);
+    CAN_Init(&hcan2, can2RxCallback);
+    UART_Init(&huart3, dr16RxCallback, 36);
+    m_imu->init();
+    m_isInitComplete = true;
+}
+
+void Chassis::controlLoop()
+{
+    if (!m_isInitComplete) return;
+    //模式选择
+    modeSelect();
+    //速度解算
+    targetSpeedplan();
+    //电机控制
+    motorControl();
+    //数据发送
+    transmitChassisMotorData();
+}
+
+void Chassis::imuLoop()
+{
+    if (!m_isInitComplete) return;
+    m_eulerAngle = m_imu->solveAttitude();
+}
+
+void Chassis::receiveChassisMotorDataFromISR(const can_rx_message_t *rxMessage)
+{
+    if (m_front_SteerLeftMotor && m_front_WheelLeftMotor->decodeCanRxMessageFromISR(rxMessage)) return;
+    if (m_front_SteerRightMotor && m_front_WheelRightMotor->decodeCanRxMessageFromISR(rxMessage)) return;
+    if (m_back_SteerLeftMotor && m_back_WheelLeftMotor->decodeCanRxMessageFromISR(rxMessage)) return;
+    if (m_back_SteerRightMotor && m_back_WheelRightMotor->decodeCanRxMessageFromISR(rxMessage)) return;
+}
+
+void Chassis::receiveRemoteControlDataFromISR(const uint8_t *rxData)
+{
+    m_remoteControl.receiveRxDataFromISR(rxData);
+}
+
+void Chassis::modeSelect()
+{
+    m_remoteControl.updateEvent();
+    if (!m_remoteControl.isDr16RemoteControlConnected()) {
+        m_chassisMode  = CHASSIS_NO_FORCE;
+        return;
+    }
+
+    switch (m_remoteControl.getLeftSwitchStatus()) {
+        case Dr16RemoteControl::SWITCH_DOWN:
+            m_chassisMode  = CHASSIS_NO_FORCE;
+            break;
+
+        case Dr16RemoteControl::SWITCH_MIDDLE:
+            m_chassisMode  = MANUAL_CONTROL;
+            break;
+
+        case Dr16RemoteControl::SWITCH_UP:
+            m_chassisMode  = AUTO_CONTROL;
+            break;
+
+        default:
+            break;
+    }
+}
+
+void Chassis::targetSpeedplan()
+{
+    fp32 vx = 0.0f; // 前后速度 (m/s)
+    fp32 vy = 0.0f; // 左右速度 (m/s)
+    fp32 wz = 0.0f; // 旋转角速度 (rad/s)
+
+    if (m_chassisMode == MANUAL_CONTROL)
+    {
+        const fp32 MAX_SPEED = 3.0f;
+        const fp32 MAX_OMEGA = 3.0f;
+
+        vx = rcStickDeadZoneFilter(m_remoteControl.getLeftStickY()) * MAX_SPEED;
+        vy = rcStickDeadZoneFilter(m_remoteControl.getLeftStickX()) * MAX_SPEED;
+        wz = rcStickDeadZoneFilter(m_remoteControl.getRightStickX()) * MAX_OMEGA;
+    }
+    else if (m_chassisMode == CHASSIS_NO_FORCE)
+    {
+        vx = 0.0f;
+        vy = 0.0f;
+        wz = 0.0f;
+    }
+
+    // 计算各轮速度分量
+    fp32 rx = halfWheelBase;   
+    fp32 ry = halfTrackWidth;
+
+    struct WheelVel { fp32 vx; fp32 vy; };
+    WheelVel wheelVels[4];
+
+    // 计算每个舵轮的目标线速度
+    wheelVels[0].vx = vx - wz * ry;
+    wheelVels[0].vy = vy + wz * rx;
+
+    wheelVels[1].vx = vx - wz * ry;
+    wheelVels[1].vy = vy - wz * rx;
+
+    wheelVels[2].vx = vx + wz * ry;
+    wheelVels[2].vy = vy - wz * rx;
+
+    wheelVels[3].vx = vx + wz * ry;
+    wheelVels[3].vy = vy + wz * rx;
+
+    // 应用目标速度和角度到电机
+    auto updateMotorPair = [&](MotorGM6020* steer, MotorM3508* drive, fp32 t_vx, fp32 t_vy) {
+        if (steer == nullptr || drive == nullptr) return;
+
+        fp32 targetSpeed = sqrtf(t_vx * t_vx + t_vy * t_vy);
+
+        // 如果速度非常小，则保持当前角度或归零速度
+        if (targetSpeed < 0.01f) {
+            drive->setTargetAngularVelocity(0.0f);
+            return;
+        }
+
+        fp32 targetAngle = atan2f(t_vy, t_vx);
+
+        fp32 currentAngle = steer->getCurrentAngle();
+        fp32 angleDiff = targetAngle - currentAngle;
+
+        // 确保角度差限制在 [-PI, PI] 之间
+        while (angleDiff > MATH_PI) angleDiff -= 2.0f * MATH_PI;
+        while (angleDiff < -MATH_PI) angleDiff += 2.0f * MATH_PI;
+
+        // 如果角度差大于 90 度，则反向驱动
+        if (fabsf(angleDiff) > (MATH_PI / 2.0f)) {
+            angleDiff = (angleDiff > 0) ? angleDiff - MATH_PI : angleDiff + MATH_PI;
+            targetSpeed = -targetSpeed;
+        }
+
+        // 计算最终目标角度
+        fp32 finalTargetAngle = currentAngle + angleDiff;
+        steer->setTargetAngle(finalTargetAngle);
+
+        // 将速度转换为角速度并设置给驱动电机
+        fp32 targetOmega = targetSpeed / wheelRadius;
+        drive->setTargetAngularVelocity(targetOmega);
+    };
+
+    // 更新每个舵轮电机
+    updateMotorPair(m_front_SteerLeftMotor, m_front_WheelLeftMotor, wheelVels[0].vx, wheelVels[0].vy);
+    updateMotorPair(m_back_SteerLeftMotor, m_back_WheelLeftMotor, wheelVels[1].vx, wheelVels[1].vy);
+    updateMotorPair(m_back_SteerRightMotor, m_back_WheelRightMotor, wheelVels[2].vx, wheelVels[2].vy);
+    updateMotorPair(m_front_SteerRightMotor, m_front_WheelRightMotor, wheelVels[3].vx, wheelVels[3].vy);
+}
+
+
+void Chassis::motorControl()
+{
+    // 无力模式时停止所有电机
+    if (m_chassisMode == CHASSIS_NO_FORCE)
+    {
+        auto stopMotor = [](Motor* m) {
+            if (m) m->setTargetTorqueCurrent(0); // 或者使用 openloopControl(0)
+        };
+        stopMotor(m_front_SteerLeftMotor);
+        stopMotor(m_front_WheelLeftMotor);
+        stopMotor(m_back_SteerLeftMotor);
+        stopMotor(m_back_WheelLeftMotor);
+        stopMotor(m_back_SteerRightMotor);
+        stopMotor(m_back_WheelRightMotor);
+        stopMotor(m_front_SteerRightMotor);
+        stopMotor(m_front_WheelRightMotor);
+        return;
+    }
+
+    // 正常模式下进行闭环控制
+    auto runControl = [](MotorGM6020* steer, MotorM3508* drive) {
+        if (steer) steer->angleClosedloopControl();
+        if (drive) drive->angularVelocityClosedloopControl();
+    };
+
+    // 运行电机控制
+    runControl(m_front_SteerLeftMotor, m_front_WheelLeftMotor);
+    runControl(m_back_SteerLeftMotor, m_back_WheelLeftMotor);
+    runControl(m_back_SteerRightMotor, m_back_WheelRightMotor);
+    runControl(m_front_SteerRightMotor, m_front_WheelRightMotor);
+
+}
+
+
+void Chassis::transmitChassisMotorData()
+{
+    //看具体接线再写
+}
+
+inline fp32 Chassis::rcStickDeadZoneFilter(const fp32 &rcStickValue)
+{
+    if (rcStickValue > DT7_STICK_DEAD_ZONE)
+        return (rcStickValue - DT7_STICK_DEAD_ZONE) / (1.0f - DT7_STICK_DEAD_ZONE);
+    else if (rcStickValue < -DT7_STICK_DEAD_ZONE)
+        return (rcStickValue + DT7_STICK_DEAD_ZONE) / (1.0f - DT7_STICK_DEAD_ZONE);
+    else
+        return 0.0f;
+}
+
+
+
+
+
+