diff --git a/docs/ObjectOrientedModel/Motor.md b/docs/ObjectOrientedModel/Motor.md new file mode 100644 index 0000000000000000000000000000000000000000..e191733d8c0c0045874cf22c03df59287eb5dea9 --- /dev/null +++ b/docs/ObjectOrientedModel/Motor.md @@ -0,0 +1,187 @@ +# 电机 + +依据 UIML 的原始设计，电机类是设计成面向对象、具有多态性、通过稳定 API 交流的对象。 +电机类的设计准则可以理解成能够实现这样的功能：只要在配置文件里设置好参数，可以不需要动代码，就能兼容使用不同云台电机的云台。 + +在应用中，控制一个电机转动到某个指定位置，应当以以下步骤进行： + +1. 服务创建电机对象。电机对象会将自己绑定到某个外设上，以和电机硬件进行通信； +2. 服务按需设置电机对象的控制方式，并根据需要设置其目标值（速度，角度等）； +3. 服务可以调用一些方法设置电机对象的属性，也可以读取。部分属性的值可以改变电机的控制特性，也可以改变反馈值的行为。 +4. 如果电机具有反馈（如 CAN 反馈报文），可以从电机对象的属性值中读出反馈信息。 + +## 数据单位与格式约定 + +角度：以 **角度（360 度为圆周）** 为准。 + +速度：以 **RPM** 为准。 + +## 类继承关系 + +当前 UIML 内置了一些电机类的实现，在此简要说明。 + +- BasicMotor （电机基类） + - DummyMotor （无法生成配置字典中 `type` 对应种类电机时，产生的不实现任何功能的对象） + - CanMotor （连接在 CAN 总线上的电机，以 CAN 报文交换指令。对象内含 CAN 端点信息结构体 + ） + - DjiCanMotor （大疆的通用 RoboMaster 电机，它们具有类似的控制逻辑，提取到此类中） + - M3508 + - M6020 + - M2006 + - DamiaoJ6006 （达妙科技 DM-J6006） + - DcMotor **（FIXME）** + - Servo **（FIXME）** + +## API + +电机基类定义了 6 个纯虚函数，你的电机类需要实现全部的函数。 + +### `void Init(ConfItem *conf)` + +初始化本电机对象。UIML 不对电机类使用构造函数。`conf` 通常应当指向一个电机的配置字典，由拥有该电机对象的服务提供。 + +如果电机硬件需要特殊序列或其他操作完成初始化，请在此函数内完成。 + +特别注意：请务必在此调用上一级电机类的构造函数。 + +### `bool SetMode(MotorCtrlMode mode)` + +设置电机的控制模式。定义了以下四种模式： + +- 扭矩模式（目标值为扭矩值/电流） + + 扭矩模式可以实现为直接向电机发送目标电流值的形式。 + + 目标值的单位是电机相关的，并没有定义。 + +- 速度模式（目标值为转速，RPM） + + 速度模式可以实现为 PID 速度环（参见 `DjiCanMotor`）。 + +- 角度模式（目标值为电机上传感器返回的角度值的累积量，度） + + 角度模式可以实现为 PID 串级角度环（参见 `DjiCanMotor`）。 + +- 急停模式（进入后不可再退出，电机会被强制停止输出。被急停后只能通过重置单片机恢复运行） + +如果设置了不能使用的模式，此方法应返回 `false`。 + +### `bool SetTarget(float target)` + +设置电机应当执行的目标值。其唯一一个浮点数参数的含义随当前模式不同而改变。见 `bool SetMode(MotorCtrlMode mode)`。 + +### `bool SetData(MotorDataType type, float angle)` + +设置电机的公开属性的值。不是所有电机都应当实现所有的属性。当试图设置未实现的属性时，此方法应返回 `false`。 + +### `float GetData(MotorDataType type)` + +获取电机的公开属性的值。不是所有电机都应当实现所有的属性。当试图获取未实现的属性时，此方法返回 `0.0f`。 + +### `void EmergencyStop()` + +使电机进入急停状态。效果同 `SetMode(MOTOR_STOP_MODE)`。 + +## 新增电机 + +以使用最为广泛的 CAN 总线电机为例。实现可参考 `DjiCanMotor` 与其子类 `M3508`。 + +CAN 电机一般都具有反馈报文。收到的 CAN 报文会通过软总线广播，我们为了让电机接收到报文，需要先定义一个软总线广播的接收端点。 + +```cpp +void DjiCanMotor::CanRxCallback(const char *endpoint, SoftBusFrame *frame, void *bindData) +{ + // 校验绑定电机对象 + if (!bindData) + return; + + U_S(DjiCanMotor); // [ 作用是将bindData转换为(DjiCanMotor*)型指针，名称为self ] + + // 校验电机ID + if (self->m_canInfo.rxID != Bus_GetListValue(frame, 0).U16) + return; + + auto data = reinterpret_cast(Bus_GetListValue(frame, 1).Ptr); + if (data) + self->CanRxUpdate(data); // 更新电机数据 +} +``` + +然后，实现 `Init` 函数。需要在 `Init` 函数中首先给出 CAN 的发送和接收报文的 ID。 +当然对于任何电机对象，初始化的第一步都总是应该先从配置字典中读取该电机硬件通信的外设与总线上的地址。 + +然后，设置默认的电机工作模式变量，初始化私有的状态变量，并订阅 CAN 反馈报文。 + +```cpp +void DjiCanMotor::Init(ConfItem *dict) +{ + U_C(dict); + // 公共初始化部分 + // 注意：必须已经由子类设定好了减速比与CAN信息才能到达这里 + + // 默认模式为扭矩模式 + m_mode = MOTOR_TORQUE_MODE; + + // 初始化电机pid + m_speedPid.Init(Conf["speed-pid"]); + m_anglePid.Init(Conf["angle-pid"]); + m_anglePid.outer.maxOutput *= m_reductionRatio; // 将输出轴速度限幅放大到转子上 + + // 读取电机方向，为1正向为-1反向 + m_direction = Conf["direction"].get(1); + if (m_direction != -1 && m_direction != 1) + m_direction = 1; + + // 订阅can信息 + char name[] = "/can_/recv"; + name[4] = m_canInfo.canX + '0'; + Bus_SubscribeTopic(this, DjiCanMotor::CanRxCallback, name); + + m_target = 0.0f; + m_stallTime = 0; + m_totalAngle = 0; + m_motorReducedRpm = 0; +} +``` + +如果电机需要在单片机上计算 PID 控制，请在此处完成 PID 控制器的初始化。 + +接下来实现控制部分。如果电机需要单片机对其进行角度反馈控制，那么电机必须有角度反馈能力，且电机类或者电调本身实现了角度累积功能。 +UIML 中累积角度值一般称为“Total angle”。`DjiCanMotor` 的实现为： + +```cpp +// 统计电机累计转过的圈数 +void DjiCanMotor::UpdateAngle() +{ + int32_t angleDiff = 0; + // 计算角度增量（加过零检测） + if (m_motorAngle - m_lastAngle < -4000) + angleDiff = m_motorAngle + (8191 - m_lastAngle); + else if (m_motorAngle - m_lastAngle > 4000) + angleDiff = -m_lastAngle - (8191 - m_motorAngle); + else + angleDiff = m_motorAngle - m_lastAngle; + // 将角度增量加入计数器 + m_totalAngle += angleDiff; + // 记录角度 + m_lastAngle = m_motorAngle; +} +``` + +此处 `m_totalAngle` 将作为 PID 串级角度控制的反馈值。 + +如果是自带控制算法的电机，可以不在单片机上实现控制算法，但有一个使用约定： +若电机支持设置串级控制时的速度上限，应通过电机类的 `SpeedLimit` 属性设置。见 `DamiaoJ6006`： + +```cpp +bool DamiaoJ6006::SetData(MotorDataType type, float data) +{ + switch (type) + { +// ... + + case SpeedLimit: + m_autonomousAngleModeSpeedLimit = data; + return true; +// ... +``` diff --git a/tools/include/motor.h b/tools/include/motor.h index b35cdbbd7930fd40eb78e725fd32fcacc0d4b578..bfd6818861f7d03258621b25831155a1822ca6df 100644 --- a/tools/include/motor.h +++ b/tools/include/motor.h @@ -4,6 +4,7 @@ #include "config.h" #include "pid.h" #include "softbus.h" +#include #include #include #include @@ -22,12 +23,26 @@ typedef enum __packed // （可获取的）数据类型 enum MotorDataType { + // 电机当前的角度反馈值 CurrentAngle, + + // 电机反馈角度的累积值 TotalAngle, + + // 电机反馈角度的原始数值 RawAngle, + + // 电机反馈的速度值 Speed, + + // 电机反馈的速度值，经过某减速比换算后的值。仅对减速电机有意义 SpeedReduced, + + // 电机配置的“方向”参数，1为正转，-1为反转。 Direction, + + // 电机自主进行角度环控制时的速度上限（在DamiaoJ6006中加入） + SpeedLimit, }; class BasicMotor @@ -39,7 +54,7 @@ class BasicMotor virtual void EmergencyStop() = 0; virtual bool SetTarget(float target) = 0; - virtual bool SetTotalAngle(float angle) = 0; + virtual bool SetData(MotorDataType type, float angle) = 0; virtual float GetData(MotorDataType type); /** @@ -97,7 +112,7 @@ class DummyMotor : public BasicMotor virtual void EmergencyStop() override{}; virtual bool SetTarget(float target) override { return false; } - virtual bool SetTotalAngle(float angle) override { return false; } + virtual bool SetData(MotorDataType type, float angle) override { return false; } virtual float GetData(MotorDataType type) override { return 0.0f; } }; @@ -110,7 +125,7 @@ class DjiCanMotor : public CanMotor virtual void EmergencyStop() override; virtual bool SetTarget(float target) override; - virtual bool SetTotalAngle(float angle) override; + virtual bool SetData(MotorDataType type, float angle) override; virtual float GetData(MotorDataType type) override; // 各种电机编码值与角度的换算 @@ -199,6 +214,72 @@ class M2006 final : public DjiCanMotor virtual void CanRxUpdate(uint8_t *rxData) override; }; +class DamiaoJ6006 : public CanMotor +{ + public: + virtual void Init(ConfItem *conf) override; + virtual bool SetMode(MotorCtrlMode mode) override; + virtual void EmergencyStop() override; + + virtual bool SetTarget(float target) override; + virtual bool SetData(MotorDataType type, float angle) override; + virtual float GetData(MotorDataType type) override; + + protected: + // 编码器与角度换算 + // *16384/360 + static inline int32_t DegToCode(float deg) { return (int32_t)(deg * 45.5111f); } + static inline float CodeToDeg(int32_t code) { return (float)(code / 45.5111f); } + // CAN接收中断回调 + static void CanRxCallback(const char *endpoint, SoftBusFrame *frame, void *bindData); + // 急停回调 + static void EmergencyStopCallback(const char *endpoint, SoftBusFrame *frame, void *bindData); + + // 电机自主角度环时的速度上限 + float m_autonomousAngleModeSpeedLimit; + // 累积角度值（角度） + float m_totalAngle; + // 角度(16位)、速度(12位)、扭矩(12位) + uint16_t m_motorAngle, m_motorSpeed, m_motorTorque; + // 驱动温度、线圈温度 + uint8_t m_motorDriverTemp, m_motorCoilTemp; + + // 定时器 + osTimerId_t m_timer; + + private: + // 尝试解除错误状态 + // 因为常见的错误状态是电机过热，所以这里只尝试解除过热错误，温度阈值为60度 + void AttemptResolveError(); + + // 下面的指令属于控制指令 + // 对电机发送使能/失能指令 + void CommandEnable(bool enable); + + // 保存位置零点 + void CommandSetZero(); + + // 清除错误标志 + void CommandClearErrorFlag(); + + // 下面的三个命令方式用的是UIML规定的单位制，发送函数封包的时候再转成达妙的弧度或者映射单位 + // 向MIT模式发送命令，目前只用在了扭矩模式 + void CommandMitMode(uint16_t targetAngle, + uint16_t targetSpeed, + uint16_t kp, + uint16_t kd, + uint16_t targetTorque); + + // 向角度模式发送命令 + void CommandAngleMode(float targetAngle, float targetSpeed); + + // 向速度模式发送命令 + void CommandSpeedMode(float targetSpeed); + + // 发送CAN报文 + void CanTransmitFrame(uint16_t txId, uint8_t length, uint8_t *data); +}; + class DcMotor : public BasicMotor { public: @@ -208,7 +289,7 @@ class DcMotor : public BasicMotor virtual void EmergencyStop(); virtual bool SetTarget(float target); - virtual bool SetTotalAngle(float angle); + virtual bool SetData(MotorDataType type, float angle); virtual float GetData(MotorDataType type); // 编码值与角度的换算（减速比*编码器一圈值/360） @@ -262,7 +343,7 @@ class Servo : public BasicMotor virtual void EmergencyStop() {} virtual bool SetTarget(float target); - virtual bool SetTotalAngle(float angle) { return false; } + virtual bool SetData(MotorDataType type, float angle) { return false; } virtual float GetData(MotorDataType type) { return 0.0f; } protected: diff --git a/tools/include/uimlnumeric.h b/tools/include/uimlnumeric.h index b1ce56089ccb6856ad0c78bb4ed5ff0d53e2e088..d7c794e19588730f1f824685ba767838cca3076a 100644 --- a/tools/include/uimlnumeric.h +++ b/tools/include/uimlnumeric.h @@ -72,3 +72,36 @@ inline float AccumulatedDegTo180(float deg) else return deg; } + +/** + * @brief 将角度转为弧度 + * + * @param deg 角度值 + * @return float 弧度值 + */ +inline float Degree2Radian(float deg) { return deg * 0.0174532f; } + +/** + * @brief 将弧度转为角度 + * + * @param rad 弧度值 + * @return float 角度值 + */ + +inline float Radian2Degree(float rad) { return rad * 57.2958f; } + +/** + * @brief 将RPM转为弧度每秒 + * + * @param rpm RPM值 + * @return float 弧度每秒值 + */ +inline float Rpm2Radps(float rpm) { return rpm * 0.10472f; } + +/** + * @brief 将弧度每秒转为RPM + * + * @param radps 弧度每秒值 + * @return float RPM值 + */ +inline float Radps2Rpm(float radps) { return radps * 9.5493f; } diff --git a/tools/motor/motor_can/dji_motor.cpp b/tools/motor/motor_can/dji_motor.cpp index 58716ac1cca59031574511e2a43c5e706f2e1f8e..ead327a3cc0e3c6d16821d22b131d28caf6130f6 100644 --- a/tools/motor/motor_can/dji_motor.cpp +++ b/tools/motor/motor_can/dji_motor.cpp @@ -8,7 +8,7 @@ void DjiCanMotor::Init(ConfItem *dict) { U_C(dict); // 公共初始化部分 - // 注意：必须设定好减速比与CAN信息 + // 注意：必须已经由子类设定好了减速比与CAN信息才能到达这里 // 默认模式为扭矩模式 m_mode = MOTOR_TORQUE_MODE; @@ -86,12 +86,18 @@ bool DjiCanMotor::SetTarget(float target) } // 开始统计电机累计角度 -bool DjiCanMotor::SetTotalAngle(float angle) +bool DjiCanMotor::SetData(MotorDataType type, float data) { - m_totalAngle = DegToCode(angle, m_reductionRatio); - m_lastAngle = m_motorAngle; + switch (type) + { + case TotalAngle: + m_totalAngle = DegToCode(data, m_reductionRatio); + m_lastAngle = m_motorAngle; + return true; - return true; + default: + return false; + } } // 获取电机数据 @@ -164,7 +170,7 @@ void DjiCanMotor::EmergencyStopCallback(const char *endpoint, SoftBusFrame *fram void DjiCanMotor::UpdateAngle() { int32_t angleDiff = 0; - // FIXME: 无注释，逻辑存疑 + // 计算角度增量（加过零检测） if (m_motorAngle - m_lastAngle < -4000) angleDiff = m_motorAngle + (8191 - m_lastAngle); else if (m_motorAngle - m_lastAngle > 4000) diff --git a/tools/motor/motor_can/dm_j6006.cpp b/tools/motor/motor_can/dm_j6006.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ba00f2a4fca30d31f28862d49528c343f687d4ec --- /dev/null +++ b/tools/motor/motor_can/dm_j6006.cpp @@ -0,0 +1,300 @@ +#include "config.h" +#include "motor.h" +#include "pid.h" +#include "softbus.h" +#include "string.h" +#include "uimlnumeric.h" +#include + +void DamiaoJ6006::Init(ConfItem *dict) +{ + U_C(dict); + + // 读取CAN信息 + // 由于达妙电机不像大疆的RM电机一样在一个ID里塞多个电机的控制量， + // 所以不使用循环缓冲区。另外，ID也是由调参软件设置的，要直接从配置文件读 + m_canInfo.rxID = Conf["rx-id"].get(0); + m_canInfo.txID = Conf["tx-id"].get(0); + m_canInfo.canX = Conf["can-x"].get(0); + + // 默认模式为扭矩模式 + m_mode = MOTOR_TORQUE_MODE; + + // 读取电机方向，为1正向为-1反向 + m_direction = Conf["direction"].get(1); + if (m_direction != -1 && m_direction != 1) + m_direction = 1; + + // 订阅can信息 + char name[] = "/can_/recv"; + name[4] = char(m_canInfo.canX) + '0'; + Bus_SubscribeTopic(this, DamiaoJ6006::CanRxCallback, name); + + m_target = 0.0f; + m_stallTime = 0; + m_totalAngle = 0; +} + +// 切换电机模式 +bool DamiaoJ6006::SetMode(MotorCtrlMode mode) +{ + if (m_mode == MOTOR_STOP_MODE) // 急停模式下不允许切换模式 + return false; + + // J6006可以支持UIML定义的全部三种工作模式，所以不用判断其他的东西 + // 同样因为没有在单片机上进行PID计算，所以也不用像大疆的一样清空PID + m_mode = mode; + m_target = 0.0f; + + return true; +} + +// 急停函数 +void DamiaoJ6006::EmergencyStop() +{ + m_target = 0; + m_mode = MOTOR_STOP_MODE; + CanTransmit(0); +} + +// 设置电机期望值 +bool DamiaoJ6006::SetTarget(float target) +{ + switch (m_mode) + { + case MOTOR_SPEED_MODE: + m_target = target; + CommandSpeedMode(target); + break; + case MOTOR_ANGLE_MODE: + m_target = target; + CommandAngleMode(target, m_autonomousAngleModeSpeedLimit); + break; + case MOTOR_TORQUE_MODE: + m_target = target; + CommandMitMode(0, 0, 0, 0, target); + break; + default: + return false; + } + + return true; +} + +// 开始统计电机累计角度 +bool DamiaoJ6006::SetData(MotorDataType type, float data) +{ + switch (type) + { + case TotalAngle: + m_totalAngle = data; + return true; + + case SpeedLimit: + m_autonomousAngleModeSpeedLimit = data; + return true; + + default: + return false; + } +} + +// 获取电机数据 +float DamiaoJ6006::GetData(MotorDataType type) +{ + switch (type) + { + case CurrentAngle: + // 转子当前角度 + return CodeToDeg(m_motorAngle); + + case TotalAngle: + // 输出轴总转过角度 + return m_totalAngle; + + case RawAngle: + // 转子当前角度，原始值 + return m_motorAngle; + + case SpeedReduced: + // 减速后的转速（RPM） + case Speed: + // 输出轴转速（RPM） + return Radps2Rpm(m_motorSpeed); + + case SpeedLimit: + // 电机自主进行角度环控制时的速度上限（RPM） + return m_autonomousAngleModeSpeedLimit; + + default: + return CanMotor::GetData(type); + } +} + +void DamiaoJ6006::CanRxCallback(const char *endpoint, SoftBusFrame *frame, void *bindData) +{ + // 校验绑定电机对象 + if (!bindData) + return; + + U_S(DamiaoJ6006); + + // 校验电机ID + if (self->m_canInfo.rxID != Bus_GetListValue(frame, 0).U16) + return; + + auto data = reinterpret_cast(Bus_GetListValue(frame, 1).Ptr); + if (data) // 更新电机数据 + { + struct __packed + { + uint8_t ErrorBits : 4; + uint8_t IdLSByte : 4; + uint16_t Angle; + uint8_t SpeedTorque[3]; // 必须手动移位 + uint8_t DriverTemperature; + uint8_t CoilTemperature; + } *J6006Feedback; + + self->m_motorAngle = J6006Feedback->Angle; + self->m_motorSpeed = + uint16_t(J6006Feedback->SpeedTorque[0] << 4) | (J6006Feedback->SpeedTorque[1] >> 4); + self->m_motorTorque = + uint16_t((J6006Feedback->SpeedTorque[1] & 0x0F) << 8) | J6006Feedback->SpeedTorque[2]; + self->m_motorCoilTemp = J6006Feedback->CoilTemperature; + self->m_motorDriverTemp = J6006Feedback->DriverTemperature; + } +} + +// 发送使能/失能控制帧 +void DamiaoJ6006::CommandEnable(bool enable) +{ + uint8_t frame[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC}; + frame[7] = enable ? 0xFC : 0xFD; + CanTransmitFrame(m_canInfo.txID, 8, frame); +} + +// 发送保存位置零点控制帧 +void DamiaoJ6006::CommandSetZero() +{ + uint8_t frame[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE}; + CanTransmitFrame(m_canInfo.txID, 8, frame); +} + +// 发送清除错误消息控制帧 +void DamiaoJ6006::CommandClearErrorFlag() +{ + uint8_t frame[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB}; + CanTransmitFrame(m_canInfo.txID, 8, frame); +} + +// 向MIT模式发送指令 +// FIXME: MIT模式的速度需要映射到上位机设置的范围,这个东西需要先写个配置文件, +// 现在先用个扭矩控制,后面再改 +void DamiaoJ6006::CommandMitMode( + uint16_t targetAngle, uint16_t targetSpeed, uint16_t kp, uint16_t kd, uint16_t targetTorque) +{ + // TODO: + + // CanTransmitFrame(m_canInfo.rxID, 8, data); +} + +// 向角度模式发送命令 +void DamiaoJ6006::CommandAngleMode(float targetAngle, float targetSpeed) +{ + // 达妙用的是rad和rad/s + targetAngle = Degree2Radian(targetAngle); + targetSpeed = Rpm2Radps(targetSpeed); + + float data[2] = {targetAngle, targetSpeed}; + CanTransmitFrame(m_canInfo.rxID + 0x100, 8, (uint8_t *)data); +} + +// 向速度模式发送命令 +void DamiaoJ6006::CommandSpeedMode(float targetSpeed) +{ + // 达妙用的是rad/s + targetSpeed = Rpm2Radps(targetSpeed); + + CanTransmitFrame(m_canInfo.rxID + 0x200, 4, (uint8_t *)&targetSpeed); +} + +inline void DamiaoJ6006::CanTransmitFrame(uint16_t txId, uint8_t length, uint8_t *data) +{ + Bus_RemoteFuncCall("/can/send", + {{"can-x", {.U8 = m_canInfo.canX}}, + {"id", {.U16 = txId}}, + {"len", {.U8 = length}}, + {"data", {data}}}); +} + +// void DamiaoJ6006::TimerCallback(const void *argument) +// { +// auto self = reinterpret_cast(pvTimerGetTimerID((TimerHandle_t)argument)); + +// // 计算前一帧至今转过的角度 +// self->UpdateAngle(); +// // 调用控制器计算电机下一步行动 +// self->ControllerUpdate(self->m_target); + +// // 判断堵转情况 +// } + +// 电机急停回调函数 +// void DamiaoJ6006::EmergencyStopCallback(const char *endpoint, SoftBusFrame *frame, void +// *bindData) +// { +// U_S(DamiaoJ6006); +// self->EmergencyStop(); +// } + +// 统计电机累计转过的圈数 +// FIXME: +// void DamiaoJ6006::UpdateAngle() +// { +// int32_t angleDiff = 0; +// // 计算角度增量（加过零检测） +// if (m_motorAngle - m_lastAngle < -8000) +// angleDiff = m_motorAngle + (16383 - m_lastAngle); +// else if (m_motorAngle - m_lastAngle > 8000) +// angleDiff = -m_lastAngle - (16383 - m_motorAngle); +// else +// angleDiff = m_motorAngle - m_lastAngle; +// // 将角度增量加入计数器 +// m_totalAngle += angleDiff; +// // 记录角度 +// m_lastAngle = m_motorAngle; +// } + +// 控制器根据模式计算输出 +// void DamiaoJ6006::ControllerUpdate(float target) +// { +// int16_t output = 0; +// switch (m_mode) +// { +// case MOTOR_SPEED_MODE: +// output = m_speedPid.SingleCalc(target, m_motorSpeed); +// break; +// case MOTOR_ANGLE_MODE: +// output = m_anglePid.CascadeCalc(target, m_totalAngle, m_motorSpeed); +// break; +// case MOTOR_TORQUE_MODE: +// output = (int16_t)target; +// break; +// case MOTOR_STOP_MODE: +// return; +// } + +// CanTransmit(output); +// } + +// 堵转检测逻辑 +// void DamiaoJ6006::StallDetection() +// { +// m_stallTime = (m_motorSpeed == 0 && abs(m_motorTorque) > 7000) ? m_stallTime + 2 : 0; +// if (m_stallTime > 500) +// { +// Bus_PublishTopicFast(m_stallTopic, {{nullptr}}); +// m_stallTime = 0; +// } +// } diff --git a/tools/motor/motor_pwm/dc_motor.cpp b/tools/motor/motor_pwm/dc_motor.cpp index a1313687012acf737f69dabd9a535a74d2d910c4..c44725b599df57c0476018eb19f61ae4c270441b 100644 --- a/tools/motor/motor_pwm/dc_motor.cpp +++ b/tools/motor/motor_pwm/dc_motor.cpp @@ -1,183 +1,201 @@ -#include "softbus.h" -#include "motor.h" #include "config.h" +#include "motor.h" #include "pid.h" +#include "softbus.h" #include -//软件定时器回调函数 + +// 软件定时器回调函数 void DcMotor::TimerCallback(void const *argument) { - auto self = reinterpret_cast(pvTimerGetTimerID((TimerHandle_t)argument)); - self->UpdateAngle(); - self->ControllerUpdate(self->m_target); + auto self = reinterpret_cast(pvTimerGetTimerID((TimerHandle_t)argument)); + self->UpdateAngle(); + self->ControllerUpdate(self->m_target); } -void DcMotor::Init(ConfItem* dict) +void DcMotor::Init(ConfItem *dict) { - //电机减速比 - m_reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.0f);//电机减速比参数 - m_circleEncode = Conf_GetValue(dict, "max-encode", float, 48.0f); //倍频后编码器转一圈的最大值 - //初始化电机绑定tim信息 - TimerInit(dict); - //设置电机默认模式为速度模式 - SetMode(MOTOR_SPEED_MODE); - //初始化电机pid - m_speedPID.Init(Conf_GetNode(dict, "speed-pid")); - m_anglePID.Init(Conf_GetNode(dict, "angle-pid")); - m_anglePID.outer.maxOutput = m_speedPID.maxOutput/m_reductionRatio; // 将输出轴速度限幅放大到转子上 - - //开启软件定时器 - osTimerDef(DCMOTOR, DcMotor::TimerCallback); - osTimerStart(osTimerCreate(osTimer(DCMOTOR), osTimerPeriodic, this), 2); + // 电机减速比 + m_reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.0f); // 电机减速比参数 + m_circleEncode = Conf_GetValue(dict, "max-encode", float, 48.0f); // 倍频后编码器转一圈的最大值 + // 初始化电机绑定tim信息 + TimerInit(dict); + // 设置电机默认模式为速度模式 + SetMode(MOTOR_SPEED_MODE); + // 初始化电机pid + m_speedPID.Init(Conf_GetNode(dict, "speed-pid")); + m_anglePID.Init(Conf_GetNode(dict, "angle-pid")); + m_anglePID.outer.maxOutput = + m_speedPID.maxOutput / m_reductionRatio; // 将输出轴速度限幅放大到转子上 + + // 开启软件定时器 + osTimerDef(DCMOTOR, DcMotor::TimerCallback); + osTimerStart(osTimerCreate(osTimer(DCMOTOR), osTimerPeriodic, this), 2); } -//初始化tim -void DcMotor::TimerInit(ConfItem* dict) +// 初始化tim +void DcMotor::TimerInit(ConfItem *dict) { - m_posRotateTim.timX = Conf_GetValue(dict,"pos-rotate-tim/tim-x",uint8_t,0); - m_posRotateTim.channelX = Conf_GetValue(dict,"pos-rotate-tim/channel-x",uint8_t,0); - m_negRotateTim.timX = Conf_GetValue(dict,"neg-rotate-tim/tim-x",uint8_t,0); - m_negRotateTim.channelX = Conf_GetValue(dict,"neg-rotate-tim/channel-x",uint8_t,0); - m_encodeTim.channelX = Conf_GetValue(dict,"encode-tim/tim-x",uint8_t,0); + m_posRotateTim.timX = Conf_GetValue(dict, "pos-rotate-tim/tim-x", uint8_t, 0); + m_posRotateTim.channelX = Conf_GetValue(dict, "pos-rotate-tim/channel-x", uint8_t, 0); + m_negRotateTim.timX = Conf_GetValue(dict, "neg-rotate-tim/tim-x", uint8_t, 0); + m_negRotateTim.channelX = Conf_GetValue(dict, "neg-rotate-tim/channel-x", uint8_t, 0); + m_encodeTim.channelX = Conf_GetValue(dict, "encode-tim/tim-x", uint8_t, 0); } -//开始统计电机累计角度 -bool DcMotor::SetTotalAngle(float startAngle) +// 开始统计电机累计角度 +bool DcMotor::SetData(MotorDataType type, float data) { - m_totalAngle = Degree2Code(startAngle, m_reductionRatio, m_circleEncode); - m_lastAngle = startAngle; - - return true; + switch (type) + { + case TotalAngle: + m_totalAngle = Degree2Code(data, m_reductionRatio, m_circleEncode); + m_lastAngle = data; + return true; + + default: + return false; + } + + return true; } -//统计电机累计转过的圈数 +// 统计电机累计转过的圈数 void DcMotor::UpdateAngle() { - int32_t dAngle=0; - uint32_t autoReload=0; - - Bus_RemoteFuncCall("/tim/encode",{{"tim-x", {.U8 = m_encodeTim.timX}}, - {"count", {.U16 = m_angle}}, - {"auto-reload", {&autoReload}}}); - - if (m_angle - m_lastAngle < -(autoReload / 2.0f)) - dAngle = m_angle + (autoReload - m_lastAngle); - else if (m_angle - m_lastAngle > (autoReload / 2.0f)) - dAngle = -m_lastAngle - (autoReload - m_angle); - else - dAngle = m_angle - m_lastAngle; - //将角度增量加入计数器 - m_totalAngle += dAngle; - //计算速度 - m_speed = (float) dAngle / (m_circleEncode * m_reductionRatio) * 500 * 60;//rpm - //记录角度 - m_lastAngle = m_angle; + int32_t dAngle = 0; + uint32_t autoReload = 0; + + Bus_RemoteFuncCall("/tim/encode", + {{"tim-x", {.U8 = m_encodeTim.timX}}, + {"count", {.U16 = m_angle}}, + {"auto-reload", {&autoReload}}}); + + if (m_angle - m_lastAngle < -(autoReload / 2.0f)) + dAngle = m_angle + (autoReload - m_lastAngle); + else if (m_angle - m_lastAngle > (autoReload / 2.0f)) + dAngle = -m_lastAngle - (autoReload - m_angle); + else + dAngle = m_angle - m_lastAngle; + // 将角度增量加入计数器 + m_totalAngle += dAngle; + // 计算速度 + m_speed = (float)dAngle / (m_circleEncode * m_reductionRatio) * 500 * 60; // rpm + // 记录角度 + m_lastAngle = m_angle; } -//控制器根据模式计算输出 +// 控制器根据模式计算输出 void DcMotor::ControllerUpdate(float reference) { - float output=0; - if(m_mode == MOTOR_SPEED_MODE) - { - m_speedPID.SingleCalc(reference, m_speed); - output = m_speedPID.output; - } - else if(m_mode == MOTOR_ANGLE_MODE) - { - m_anglePID.CascadeCalc(reference, m_totalAngle, m_speed); - output = m_anglePID.output; - } - else if(m_mode == MOTOR_TORQUE_MODE) - { - output = reference; - } - - if(output>0) - { - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_posRotateTim.channelX}}, - {"duty", {.F32 = output}}}); - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_negRotateTim.channelX}}, - {"duty", {.F32 = 0.0f}}}); - } - else - { - output = ABS(output); - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_posRotateTim.channelX}}, - {"duty", {.F32 = 0.0f}}}); - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_negRotateTim.channelX}}, - {"duty", {.F32 = output}}}); - } - + float output = 0; + if (m_mode == MOTOR_SPEED_MODE) + { + m_speedPID.SingleCalc(reference, m_speed); + output = m_speedPID.output; + } + else if (m_mode == MOTOR_ANGLE_MODE) + { + m_anglePID.CascadeCalc(reference, m_totalAngle, m_speed); + output = m_anglePID.output; + } + else if (m_mode == MOTOR_TORQUE_MODE) + { + output = reference; + } + + if (output > 0) + { + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_posRotateTim.channelX}}, + {"duty", {.F32 = output}}}); + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_negRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); + } + else + { + output = ABS(output); + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_posRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_negRotateTim.channelX}}, + {"duty", {.F32 = output}}}); + } } -//设置电机期望值 +// 设置电机期望值 bool DcMotor::SetTarget(float targetValue) { - // FIXME: Else分支真的可以直接设置吗 - if(m_mode == MOTOR_ANGLE_MODE) - { - m_target = Degree2Code(targetValue, m_reductionRatio,m_circleEncode); - } - else if(m_mode == MOTOR_SPEED_MODE) - { - m_target = targetValue * m_reductionRatio; - } - else - { - m_target = targetValue; - } - - return true; + // FIXME: Else分支真的可以直接设置吗 + if (m_mode == MOTOR_ANGLE_MODE) + { + m_target = Degree2Code(targetValue, m_reductionRatio, m_circleEncode); + } + else if (m_mode == MOTOR_SPEED_MODE) + { + m_target = targetValue * m_reductionRatio; + } + else + { + m_target = targetValue; + } + + return true; } -//切换电机模式 +// 切换电机模式 bool DcMotor::SetMode(MotorCtrlMode mode) { - // 检测模式是否合法并设置 - if (mode != MOTOR_SPEED_MODE && mode != MOTOR_ANGLE_MODE) return false; - m_mode = mode; - - // 清 PID - switch (mode) - { - case MOTOR_SPEED_MODE: - m_speedPID.Clear(); - case MOTOR_ANGLE_MODE: - m_anglePID.Clear(); - default: break; - } - - return true; + // 检测模式是否合法并设置 + if (mode != MOTOR_SPEED_MODE && mode != MOTOR_ANGLE_MODE) + return false; + m_mode = mode; + + // 清 PID + switch (mode) + { + case MOTOR_SPEED_MODE: + m_speedPID.Clear(); + case MOTOR_ANGLE_MODE: + m_anglePID.Clear(); + default: + break; + } + + return true; } void DcMotor::EmergencyStop() { - m_mode = MOTOR_STOP_MODE; - - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_posRotateTim.channelX}}, - {"duty", {.F32 = 0.0f}}}); - Bus_RemoteFuncCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = m_posRotateTim.timX}}, - {"channel-x", {.U8 = m_negRotateTim.channelX}}, - {"duty", {.F32 = 0.0f}}}); + m_mode = MOTOR_STOP_MODE; + + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_posRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); + Bus_RemoteFuncCall("/tim/pwm/set-duty", + {{"tim-x", {.U8 = m_posRotateTim.timX}}, + {"channel-x", {.U8 = m_negRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); } float DcMotor::GetData(MotorDataType type) { - switch (type) - { - case CurrentAngle: - return Code2Degree(m_totalAngle, m_reductionRatio, m_circleEncode); - case TotalAngle: - return Code2Degree(m_totalAngle, m_reductionRatio, m_circleEncode); - case Speed: - return m_speed; - default: - return 0; - } + switch (type) + { + case CurrentAngle: + return Code2Degree(m_totalAngle, m_reductionRatio, m_circleEncode); + case TotalAngle: + return Code2Degree(m_totalAngle, m_reductionRatio, m_circleEncode); + case Speed: + return m_speed; + default: + return 0; + } }