瓴控MG4005电机

警告
该电机的数据手册非常抽象，存在若干与实际不符的情况

e.g.
该数据手册说电机的回复报文标识符为0x180 + id
但是实测发现电机的回复报文标识符为0x140 + id

如出现bug可能需要调整代码

电机简介

瓴控MG4005电机是一种总线驱动电机。同DJI提供的电机（例如3508，6020）所使用的直接发送电流/电压数据的控制方式不同，瓴控总线电机需要按规范向其发送控制指令才能正常运行。因此该电机无法和已有的电机驱动代码兼容。

电机驱动代码

定义电机对象

查阅数据手册可以看到该电机运行过程中的各种数据，选择其中可能被用到的数据封装为MG4005Data 过于不常用的变量选择直接舍弃
此处需要注意数据手册中对电机编码器值，电机转速的描述都是描述转子的。转子和转轴有1:10的减速比，读取出的数据必须经过转换才是实际输出的角度和速度

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
// MG4005.h
typedef struct struct_motor_data_
{
    uint16_t Ecd;               // 转子编码器返回值
    int16_t SpeedDPS;           // 转子每秒所转角度
    int16_t TorqueCurrent;      // 转矩电流
    uint8_t Temperature;        // 电机温度

    uint8_t MotorState;         // 电机状态， 0x00开启，0x10关闭

    struct ControlParam_
    {
        struct PID_Angle_
        {
            uint16_t Kp;
            uint16_t Ki;
            uint16_t Kd;
        }PID_Angle;

        struct PID_Speed_
        {
            uint16_t Kp;
            uint16_t Ki;
            uint16_t Kd;
        }PID_Speed;

        struct PID_Current_
        {
            uint16_t Kp;
            uint16_t Ki;
            uint16_t Kd;
        }PID_Current;

        int16_t TorqurLimit;
        int32_t SpeedLimit;
        int32_t AngleLimit;
        int32_t CurrentRamp;
        int32_t SpeedRamp;

    }ControlParam;
    

    /*
    ErrorState各位对应的电机错误状态如下:
    |ErrorState位|状态说明|0|1|
    |-|-|-|-|
    |0|低电压状态|正常|低压保护|
    |1|高电压状态|正常|高压保护|
    |2|驱动温度状态|正常|驱动过温|
    |3|电机温度状态|正常|电机过温|
    |4|电机电流状态|正常|电机过流|
    |5|电机短路状态|正常|电机短路|
    |6|堵转状态|正常|电机堵转|
    |7|输入信号状态|正常|输入信号丢失超时|
    */
    uint8_t ErrorState;

    uint8_t StopHard; // 抱闸器状态 0x00：抱闸器断电，刹车启动 0x01：抱闸器通电，刹车释放 

}MG4005Data;

MG4005支持485串口通信和CAN通信两种通信方式，由于串口可能已经没有空闲，因此此处使用CAN通信
将电机进行通信需要的数据和初始化数据共同打包为电机参数MG4005Param 

1
2
3
4
5
6
7
// MG4005.h
typedef struct struct_motor_param_{
    uint8_t CanNumbeer;
    uint16_t CanId;
    uint16_t EcdOffset;
    uint16_t EcdFullRange;
}MG4005Param;

最后加上电机在线判定，数据发送接受数组，指令错误状态，封装为电机对象MG4005

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
// MG4005.h
typedef struct struct_mg4005_{
    MG4005Data Data;
    MG4005Param Param;
    
    uint8_t FPS;
    uint8_t isOnline;
    // 1为正在等待回复
    uint8_t waitForRX;

    uint8_t TxBuffer[8];
    uint8_t RxBuffer[8];

    /*
    记录执行指令发送函数时发生的错误
    若为0x00则无错误发生
    |CmdSendError位|错误类型|
    |-|-|
    |0|填入了错误的电机指令|
    |1|过少的参数|
    |2|过多的参数|
    |3|读写控制参数时给定的ParamID有误|
    |4|发送频率过快，通道阻塞|
    */
    uint8_t CmdSendError;
}MG4005;

指令发送函数

控制MG4005电机的指令数量多，而且参数数量，参数类型各不相同。
在B写的这份代码中，指令发送函数MG4005_SendCmd能够只用一个函数完成所有发送指令的任务。
下面简要介绍如何使用：

1
2
3
4
5
6
// Step 1. 打出指令发送函数，第二参数写上cmd4005后从编辑器的自动补全中选择需要的指令
MG4005_SendCmd(&motor, cmd4005
->
MG4005_SendCmd(&motor, CMD4005_SetCurrent, 
// Step 2. 鼠标悬浮到指令CMD4005_SetCurrent上可查看该指令对应的注释信息，从而得知应该填入什么参数
MG4005_SendCmd(&motor, CMD4005_SetCurrent, iqControl);

指令发送函数会是一个比较复杂的不定参数的函数，为了保证使用的安全性，添加了参数计数宏。
并且为了使用的美观，用#define MG4005_SendCmd(motor, cmd, ...)隐藏了参数计数宏的使用。

1
2
3
4
5
6
7
8
9
10
11
// MG4005.h
#ifndef NUM_ARGS
#ifndef _GET_NTH_ARG
// 实现参数计数的宏魔法 (支持最多传 6 个参数，可扩展)
#define NUM_ARGS(...) _GET_NTH_ARG(__VA_ARGS__, 6, 5, 4, 3, 2, 1)
#define _GET_NTH_ARG(_1, _2, _3, _4, _5, _6, N, ...) N
// 该功能用于不定参数的防呆设计，能够检测传入的参数个数
#endif // _GET_NTH_ARG
#endif // NUM_ARGS

#define MG4005_SendCmd(motor, cmd, ...) MG4005_SendCmd__(motor, cmd, NUM_ARGS(__VA_ARGS__), ##__VA_ARGS__)

MG4005_SendCmd__才是实际实现功能的函数，它比外层的MG4005_SendCmd多接受一个参数数量的变量，用来校验参数数量是否正确。

指令发送函数的第二个参数cmd实际上是指令对应的十六进制码，用一个枚举类实现功能和十六进制码的映射。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
// MG4005.h
typedef enum enum_mg4005_CMD4005_{
    CMD4005_Read_1 = 0x9A,              // 该命令读取当前电机的温度、电压和错误状态标志
    CMD4005_EraseErrorState = 0x9B,     // 该命令清除当前电机的错误状态，电机收到后返回
    CMD4005_Read_2 = 0x9C,              // 该命令读取当前电机的温度、电机转矩电流MG、转速、编码器位置。

    CMD4005_Disable = 0x80,             // 关闭电机（不使能）
    CMD4005_Enable = 0x88,              // 开启电机（使能）
    CMD4005_Stop = 0x81,                // 电机停止旋转
    /**
     * @brief 控制抱闸器的开合，或者读取当前抱闸器的状态
     * 
     * @param cmd 0x00:刹车启动 0x01:刹车释放 0x10:读取刹车状态
     */
    CMD4005_Stop_Hard = 0x8C,           

    /**
     * @brief 主机发送该命令以控制电机的转矩电流输出
     * 
     * @param iqControl int16_t, 数值范围±2048
     */
    CMD4005_SetCurrent = 0xA1, 
    /**
     * @brief 主机发送该命令以控制电机的速度， 同时带有力矩限制
     * 
     * @param iqControl int16_t, 数值范围±2048
     * @param speedControl int32_t, 实际转速0.01dps/LSB
     */
    CMD4005_SetSpeed = 0xA2,
    /**
     * @brief 主机发送该命令以控制电机的位置（多圈角度）
     * 
     * @param maxSpeed uint16_t, 1dps/LSB, 设置为0则取消速度限制
     * @param angleControl int32_t, 0.01degree/LSB
     */
    CMD4005_SpinTo = 0xA3,
    /**
     * @brief 主机发送该命令以控制电机的位置（单圈角度）
     * 
     * @param maxSpeed uint16_t, 1dps/LSB, 设置为0则取消速度限制
     * @param spinDirection uint8_t，0x00代表顺时针，0x01代表逆时针 
     * @param angleControl uint32_t，对应实际位置为0.01degree/LSB，即36000代表360°
     */
    CMD4005_SpinTo_SingleRound = 0xA5,
    /**
     * @brief 主机发送该命令以控制电机的位置增量
     * 
     * @param maxSpeed uint16_t, 1dps/LSB, 设置为0则取消速度限制
     * @param angleIncrement int32_t，对应实际位置为0.01degree/LSB
     */
    CMD4005_SetIncrement = 0xA7,
    /**
     * 主机发送该命令读取当前电机的控制参数，读取的参数由序号controlParamID确定
     * 
     * #### 参数:
     * `ParamID`— 控制参数序号
     * 
     * |ParamID|Introduction|数据类型|
     * |-------|------------|-------|
     * |0x0A|角度环PID|uint16_t|
     * |0x0B|速度环PID|uint16_t|
     * |0x0C|电流环PID|uint16_t|
     * |0x1E|最大力矩电流|int16_t|
     * |0x20|最大速度|int32_t|
     * |0x22|角度限制|int32_t|
     * |0x24|电流斜率|int32_t|
     * |0x26|速度斜率|int32_t|
     * 
     */
    CMD4005_Read_ControlParam = 0xC0,
    /**
     * 主机发送该命令写入控制参数到RAM中，即时生效，断电后失效。
     * 
     * #### 参数:
     * `ParamID`— 控制参数序号
     * `Params`— 若干控制参数的值，控制参数的数量和种类见下表
     * 
     * |ParamID|Brief|Param1|Param2|Param3|
     * |-------|-----|------|------|------|
     * |0x0A|角度环PID|`uint16_t`Kp|`uint16_t`Ki|`uint16_t`Kd|
     * |0x0B|速度环PID|`uint16_t`Kp|`uint16_t`Ki|`uint16_t`Kd|
     * |0x0C|电流环PID|`uint16_t`Kp|`uint16_t`Ki|`uint16_t`Kd|
     * |0x1E|最大力矩电流|`int16_t`inputTorqueLimit|------NULL------|------NULL------|
     * |0x20|最大速度|`int32_t`inputSpeedLimit|------NULL------|------NULL------|
     * |0x22|角度限制|`int32_t`inputAngleLimit|------NULL------|------NULL------|
     * |0x24|电流斜率|`int32_t`inputCurrentRamp|------NULL------|------NULL------|
     * |0x26|速度斜率|`int32_t`inputSpeedRamp|------NULL------|------NULL------|
     * 
     */
    CMD4005_Write_ControlParam = 0xC1,
    /**
     * @brief 设置电机当前位置的编码器原始值作为电机上电后的初始零点
     * 
     * @note 该命令需要重新上电后才能生效
     * @warning  该命令会将零点写入驱动的ROM，多次写入将会影响芯片寿命，不建议频繁使用 
     */
    CMD4005_SetZeroPoint = 0x19,
    /**
     * @brief 主机发送该命令以设置电机的当前位置作为任意角度
     * 
     * @param motorAngle int32_t, 0.01°/LSB
     */
    CMD4005_SetAnyAngle = 0x95
}MG4005CMD;

在执行发送指令的函数中，函数先根据接受到的命令进行情况枚举，进而需要接受的不定参数的个数。通过不定参数数量校验后会执行命令
比较特殊的是，读写控制参数相关的指令还会多接受一个控制参数ID的十六进制标识符。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
// MG4005.c
// 实际执行的函数，多接收一个参数个数
uint8_t MG4005_SendCmd__(MG4005* motor, MG4005CMD cmd, int actual_arg_count, ... ){
    if (motor->waitForRX == 1){
        motor->CmdSendError |= 0x10;
        return 0x10;
    }
    motor->TxBuffer[0] = cmd;
    motor->waitForRX = 1;
    
    for (int i = 1; i <= 7; i++){
        motor->TxBuffer[i] = 0x00;
    }
    switch ( cmd )
    {
    case CMD4005_Read_1:
    case CMD4005_EraseErrorState:
    case CMD4005_Read_2:
    case CMD4005_Disable:
    case CMD4005_Enable:
    case CMD4005_Stop:
    case CMD4005_SetZeroPoint:{
        // 由于计数宏会把0未定参数数成1，所以此处无法判断是否多传了参数
        // 不过本来就是空指令，其实不影响
        break;
    }
    case CMD4005_Stop_Hard:{
        if (actual_arg_count < 1){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 1){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        uint8_t CMD;
        va_list ap;
        va_start(ap, actual_arg_count);
        CMD = (uint8_t)va_arg(ap, int);
        motor->TxBuffer[1] = CMD;
        va_end(ap);
        break;
    }
    case CMD4005_SetCurrent:{
        if (actual_arg_count < 1){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 1){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        int16_t iqControl;
        va_list ap;
        va_start(ap, actual_arg_count);
        iqControl = (int16_t)va_arg(ap, int);
        va_end(ap);
        motor->TxBuffer[4] = (uint8_t)( iqControl );
        motor->TxBuffer[5] = (uint8_t)( iqControl>>8 );
        break;
    }
    case CMD4005_SetSpeed:{
        if (actual_arg_count < 2){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 2){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        int16_t iqControl;
        int32_t speedControl;
        va_list ap;
        va_start(ap, actual_arg_count);
        iqControl = (int16_t)va_arg(ap, int);
        speedControl = va_arg(ap, int32_t);
        va_end(ap);
        motor->TxBuffer[2] = (uint8_t)( iqControl );
        motor->TxBuffer[3] = (uint8_t)( iqControl>>8 );
        motor->TxBuffer[4] = (uint8_t)( speedControl );
        motor->TxBuffer[5] = (uint8_t)( speedControl>>8 );
        motor->TxBuffer[6] = (uint8_t)( speedControl>>16 );
        motor->TxBuffer[7] = (uint8_t)( speedControl>>24 );
        break;
    }
    case CMD4005_SpinTo:{
        if (actual_arg_count < 2){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 2){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        uint16_t maxSpeed;
        int32_t angleControl;
        va_list ap;
        va_start(ap, actual_arg_count);
        maxSpeed = (uint16_t)va_arg(ap, int);
        angleControl = va_arg(ap , int32_t);
        va_end(ap);
        if ( maxSpeed != 0 ){
            motor->TxBuffer[0]++;
        }
        motor->TxBuffer[2] = (uint8_t)( maxSpeed );
        motor->TxBuffer[3] = (uint8_t)( maxSpeed>>8 );
        for ( int i = 0; i < 4; i++ ){
            motor->TxBuffer[i+4] = (uint8_t)( angleControl>>(8*i) );
        }
        break;
    }
    case CMD4005_SpinTo_SingleRound:{
        if (actual_arg_count < 3){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 3){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        uint16_t maxSpeed;
        uint8_t spinDirection;
        uint32_t angleControl;
        va_list ap;
        va_start(ap, actual_arg_count);
        maxSpeed = (uint16_t)va_arg(ap, int);
        spinDirection = (uint8_t)va_arg(ap, int);
        angleControl = va_arg(ap, uint32_t);
        va_end(ap);
        if (maxSpeed != 0){
            motor->TxBuffer[0]++;
        }
        motor->TxBuffer[1] = spinDirection;
        motor->TxBuffer[2] = (uint8_t)( maxSpeed );
        motor->TxBuffer[3] = (uint8_t)( maxSpeed>>8 );
        for ( int i = 0; i < 4; i++ ){
            motor->TxBuffer[i+4] = (uint8_t)( angleControl>>(8*i) );
        }
        break;
    }
    case CMD4005_SetIncrement:{
        if (actual_arg_count < 2){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 2){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        uint16_t maxSpeed;
        int32_t angleIncrement;
        va_list ap;
        va_start(ap, actual_arg_count);
        maxSpeed = (uint16_t)va_arg(ap, int);
        angleIncrement = va_arg(ap, int32_t);
        va_end(ap);
        if (maxSpeed != 0){
            motor->TxBuffer[0] ++;
        }
        motor->TxBuffer[1] = 0x00;
        motor->TxBuffer[2] = (uint8_t)( maxSpeed );
        motor->TxBuffer[3] = (uint8_t)( maxSpeed>>8 );
        motor->TxBuffer[4] = (uint8_t)( angleIncrement );
        motor->TxBuffer[5] = (uint8_t)( angleIncrement>>8 );
        motor->TxBuffer[6] = (uint8_t)( angleIncrement>>16 );
        motor->TxBuffer[7] = (uint8_t)( angleIncrement>>24 );
        break;
    }
    case CMD4005_Read_ControlParam:{
        if (actual_arg_count < 1){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 1){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        uint8_t ParamID;
        va_list ap;
        va_start(ap, actual_arg_count);
        ParamID = (uint8_t)va_arg(ap, int);
        va_end(ap);
        motor->TxBuffer[1] = ParamID;
        break;
    }
    case CMD4005_Write_ControlParam:{
        if (actual_arg_count < 1){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        uint8_t ParamID;
        va_list ap;
        va_start(ap, actual_arg_count);
        ParamID = (uint8_t)va_arg(ap, int);
        switch (ParamID)
        {
        case 0x0A:{
            // 角度环PID
            if (actual_arg_count < 4){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 4){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }
            
            uint16_t Kp, Ki, Kd;
            Kp = (uint16_t)va_arg(ap, int);
            Ki = (uint16_t)va_arg(ap, int);
            Kd = (uint16_t)va_arg(ap, int);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[2] = (uint8_t)(Kp);
            motor->TxBuffer[3] = (uint8_t)(Kp>>8);
            motor->TxBuffer[4] = (uint8_t)(Ki);
            motor->TxBuffer[5] = (uint8_t)(Ki>>8);
            motor->TxBuffer[6] = (uint8_t)(Kd);
            motor->TxBuffer[7] = (uint8_t)(Kd>>8);
            break;
        }
        case 0x0B:{
            // 速度环PID
            if (actual_arg_count < 4){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 4){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            uint16_t Kp, Ki, Kd;
            Kp = (uint16_t)va_arg(ap, int);
            Ki = (uint16_t)va_arg(ap, int);
            Kd = (uint16_t)va_arg(ap, int);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[2] = (uint8_t)(Kp);
            motor->TxBuffer[3] = (uint8_t)(Kp>>8);
            motor->TxBuffer[4] = (uint8_t)(Ki);
            motor->TxBuffer[5] = (uint8_t)(Ki>>8);
            motor->TxBuffer[6] = (uint8_t)(Kd);
            motor->TxBuffer[7] = (uint8_t)(Kd>>8);
            break;
        }
        case 0x0C:{
            // 电流环PID
            if (actual_arg_count < 4){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 4){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            uint16_t Kp, Ki, Kd;
            Kp = (uint16_t)va_arg(ap, int);
            Ki = (uint16_t)va_arg(ap, int);
            Kd = (uint16_t)va_arg(ap, int);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[2] = (uint8_t)(Kp);
            motor->TxBuffer[3] = (uint8_t)(Kp>>8);
            motor->TxBuffer[4] = (uint8_t)(Ki);
            motor->TxBuffer[5] = (uint8_t)(Ki>>8);
            motor->TxBuffer[6] = (uint8_t)(Kd);
            motor->TxBuffer[7] = (uint8_t)(Kd>>8);
            break;
        }
        case 0x1E:{
            // 最大力矩电流
            if (actual_arg_count < 2){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 2){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            int16_t inputTorqueLimit ;
            inputTorqueLimit = (int16_t)va_arg(ap, int);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[4] = (uint8_t)(inputTorqueLimit);
            motor->TxBuffer[5] = (uint8_t)(inputTorqueLimit>>8);
            break;
        }
        case 0x20:{
            // 最大速度
            if (actual_arg_count < 2){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 2){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            int32_t inputSpeedLimit;
            inputSpeedLimit = va_arg(ap, int32_t);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[4] = (uint8_t)(inputSpeedLimit);
            motor->TxBuffer[5] = (uint8_t)(inputSpeedLimit>>8);
            motor->TxBuffer[6] = (uint8_t)(inputSpeedLimit>>16);
            motor->TxBuffer[7] = (uint8_t)(inputSpeedLimit>>24);
            break;
        }
        case 0x22:{
            // 角度限制
            if (actual_arg_count < 2){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 2){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            int32_t inputAngleLimit;
            inputAngleLimit = va_arg(ap, int32_t);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[4] = (uint8_t)(inputAngleLimit);
            motor->TxBuffer[5] = (uint8_t)(inputAngleLimit>>8);
            motor->TxBuffer[6] = (uint8_t)(inputAngleLimit>>16);
            motor->TxBuffer[7] = (uint8_t)(inputAngleLimit>>24);
            break;
        }
        case 0x24:{
            // 电流斜率
            if (actual_arg_count < 2){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 2){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }
            
            int32_t inputCurrentRamp;
            inputCurrentRamp = va_arg(ap, int32_t);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[4] = (uint8_t)(inputCurrentRamp);
            motor->TxBuffer[5] = (uint8_t)(inputCurrentRamp>>8);
            motor->TxBuffer[6] = (uint8_t)(inputCurrentRamp>>16);
            motor->TxBuffer[7] = (uint8_t)(inputCurrentRamp>>24);
            break;
        }
        case 0x26:{
            // 速度斜率
            if (actual_arg_count < 2){
                motor->CmdSendError |= 0x02;
                return 0x02;
            }
            if (actual_arg_count > 2){
                motor->CmdSendError |= 0x04;
                return 0x04;
            }

            int32_t inputSpeedRamp;
            inputSpeedRamp = va_arg(ap, int32_t);
            va_end(ap);
            motor->TxBuffer[1] = ParamID;
            motor->TxBuffer[4] = (uint8_t)(inputSpeedRamp);
            motor->TxBuffer[5] = (uint8_t)(inputSpeedRamp>>8);
            motor->TxBuffer[6] = (uint8_t)(inputSpeedRamp>>16);
            motor->TxBuffer[7] = (uint8_t)(inputSpeedRamp>>24);
            break;
        }
        default:{
            motor->CmdSendError |= 0x08;
            return 0x08; // 读写控制参数时给定的ParamID有误
        }
        }
    }
    case CMD4005_SetAnyAngle:{
        if (actual_arg_count < 1){
            motor->CmdSendError |= 0x02;
            return 0x02;
        }
        if (actual_arg_count > 1){
            motor->CmdSendError |= 0x04;
            return 0x04;
        }
        int32_t motorAngle;
        va_list ap;
        va_start(ap, actual_arg_count);
        motorAngle = va_arg(ap, int32_t);
        va_end(ap);
        for (int i = 0; i < 4; i++ ){
            motor->TxBuffer[i+4] = (uint8_t)( motorAngle>>(8*i) );
        }
        break;
    }
    default:{
        for (int i = 1; i <= 7; i++){
            motor->TxBuffer[i] = 0x00;
        }
        motor->CmdSendError |= 0x01;
        return 0x01; // 命令错误
    }
    }
    
    CAN_TxBuffer bufferTemp;
    bufferTemp.Header.Identifier = 0x140 + motor->Param.CanId;
    for (int i = 0; i < 8; i ++){
        bufferTemp.Data[i] = motor->TxBuffer[i];
    }
    CAN_Send(motor->Param.CanNumbeer == CAN1 ? &can1 : &can2, &bufferTemp);
    return 0x00; // 正常运行
}

指令发送函数被执行后，会将电机标记为等待回复的状态（MG4005::waitForRX == 1），未收到回复前会阻塞接下来的指令发送动作

电机回复接收

要使电机能够接收到返回的原数据，需要将电机中调回调函数MG4005RxCallback放入正确的CAN总线回调函数中 

1
2
3
4
5
6
7
8
9
10
11
// MG4005.c
void MG4005RxCallback(MG4005* motor, CAN_Object* canx, CAN_RxBuffer* rxBuffer){
    if ( (motor->Param.CanNumbeer == CAN1 ? &can1 : &can2) == canx &&               // can口校对
        (motor->Param.CanId + 0x140) == rxBuffer->Header.Identifier ){              // canID校对,虽然说明书上写反馈报文是0x180+id，但是实际测出来是0x140+id
            for (int i = 0; i < 8; i ++ ){
                motor->RxBuffer[i] = rxBuffer->Data[i];
            }
            MG4005_Update(motor);
        }

}

电机中调回调函数MG4005RxCallback会检测收到报文的CAN ID，如果正确则调用数据解算函数MG4005_Update将得到的数据存入电机对象中 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
// MG4005.c
void MG4005_Read_Data_1(MG4005* motor){
    motor->Data.Temperature = motor->RxBuffer[1];
    motor->Data.MotorState = motor->RxBuffer[6];
    motor->Data.ErrorState = motor->RxBuffer[7];
    motor->isOnline = 1;
    motor->FPS = 0;
}

void MG4005_Read_Data_2(MG4005* motor){
    motor->Data.Temperature = motor->RxBuffer[1];
    motor->Data.TorqueCurrent = (int16_t)
        (((uint32_t) motor->RxBuffer[2] + 
        (((uint32_t)motor->RxBuffer[3])<<8)) * (66.0f/4096.0f));
    motor->Data.SpeedDPS = (int16_t)
        ((uint32_t)motor->RxBuffer[4] + 
        (((uint32_t)motor->RxBuffer[5])<<8));
    motor->Data.Ecd = (uint16_t)
        ((uint32_t)motor->RxBuffer[6] + 
        (((uint32_t)motor->RxBuffer[7])<<8));
    motor->isOnline = 1;
    motor->FPS = 0;
}

void MG4005_Read_ControlParam(MG4005* motor){
    switch (motor->RxBuffer[1])
    {
    case 0x0A:{
        motor->Data.ControlParam.PID_Angle.Kp = (int16_t)
            ((uint16_t)motor->RxBuffer[2] +
            (((uint16_t)motor->RxBuffer[3])<<8));
        motor->Data.ControlParam.PID_Angle.Ki = (int16_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8));
        motor->Data.ControlParam.PID_Angle.Kd = (int16_t)
            ((uint16_t)motor->RxBuffer[6] +
            (((uint16_t)motor->RxBuffer[7])<<8));
        break;
    }
    case 0x0B:{
        motor->Data.ControlParam.PID_Speed.Kp = (int16_t)
            ((uint16_t)motor->RxBuffer[2] +
            (((uint16_t)motor->RxBuffer[3])<<8));
        motor->Data.ControlParam.PID_Speed.Ki = (int16_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8));
        motor->Data.ControlParam.PID_Speed.Kd = (int16_t)
            ((uint16_t)motor->RxBuffer[6] +
            (((uint16_t)motor->RxBuffer[7])<<8));
    }
    case 0x0C:{
        motor->Data.ControlParam.PID_Current.Kp = (int16_t)
            ((uint16_t)motor->RxBuffer[2] +
            (((uint16_t)motor->RxBuffer[3])<<8));
        motor->Data.ControlParam.PID_Current.Ki = (int16_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8));
        motor->Data.ControlParam.PID_Current.Kd = (int16_t)
            ((uint16_t)motor->RxBuffer[6] +
            (((uint16_t)motor->RxBuffer[7])<<8));
    }
    case 0x1E:{
        motor->Data.ControlParam.TorqurLimit = (int16_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8));
        break;
    }
    case 0x20:{
        motor->Data.ControlParam.SpeedLimit = (int32_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8) + 
            (((uint16_t)motor->RxBuffer[5])<<16) + 
            (((uint16_t)motor->RxBuffer[5])<<24) );
    }
    case 0x22:{
        motor->Data.ControlParam.AngleLimit = (int32_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8) + 
            (((uint16_t)motor->RxBuffer[5])<<16) + 
            (((uint16_t)motor->RxBuffer[5])<<24) );
    }
    case 0x24:{
        motor->Data.ControlParam.CurrentRamp = (int32_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8) + 
            (((uint16_t)motor->RxBuffer[5])<<16) + 
            (((uint16_t)motor->RxBuffer[5])<<24) );
    }
    case 0x26:{
        motor->Data.ControlParam.SpeedRamp = (int32_t)
            ((uint16_t)motor->RxBuffer[4] +
            (((uint16_t)motor->RxBuffer[5])<<8) + 
            (((uint16_t)motor->RxBuffer[5])<<16) + 
            (((uint16_t)motor->RxBuffer[5])<<24) );
    }
    default:{
        break;
    }
    }
}

void MG4005_Update(MG4005* motor){
    if (motor->waitForRX == 0){
        return ;
    }
    switch ( motor->RxBuffer[0] ) // cmd
    {
    case CMD4005_Read_1:
    case CMD4005_EraseErrorState:{
        MG4005_Read_Data_1(motor);
        break;
    }
    case CMD4005_Read_2:
    case CMD4005_SetSpeed:
    case CMD4005_SetCurrent:
    case CMD4005_SpinTo:
    case CMD4005_SpinTo_SingleRound:
    case CMD4005_SetIncrement:{
        MG4005_Read_Data_2(motor);
        break;
    }
    case CMD4005_Read_ControlParam:{
        MG4005_Read_ControlParam(motor);
        break;
    }
    case CMD4005_Write_ControlParam:{
        MG4005_Read_ControlParam(motor);
        break;
    }
    case CMD4005_Stop_Hard:{
        motor->Data.StopHard = motor->RxBuffer[1];
        motor->isOnline = 1;
        motor->FPS = 0;
    }
    case CMD4005_SetZeroPoint:{
        motor->isOnline = 1;
        motor->FPS = 0;
        break;
    }
    case CMD4005_SetAnyAngle:{
        motor->isOnline = 1;
        for (int i = 0; i < 8; i++ ){
            if ( motor->TxBuffer[i] != motor->RxBuffer[i] ){
                motor->isOnline = 0;
                break;
            }
        }
        motor->isOnline = 1;
        motor->FPS = 0;
        break;
    }
    default:{
        motor->isOnline = motor->RxBuffer[0] == motor->TxBuffer[0] ? 1 : 0;
        for (int i = 1; i < 8 && motor->isOnline; i++ ){
            if (motor->RxBuffer[i] != 0x00){
                motor->isOnline = 0;
                break;
            }
        }
        motor->FPS = motor->isOnline == 1 ? 0 : motor->FPS;
        break;
    }
    
    }
    motor->waitForRX = 0;
}

END

附件:
瓴控MG(40-50)电机连接说明.pdf
瓴控电机CAN通信协议.pdf
瓴控电机串口通信协议.pdf

笔记
#电机
瓴控MG4005电机
http://chose-b-log.netlify.app/瓴控MG4005/
作者
B
发布于
2026年1月10日
更新于
2026年2月20日
许可协议