测试环境

大疆开发板 C型
STM32F407IGH6TR 芯片
大疆M3508直流无刷电机2
大疆C620无刷电机调速器2
大疆M2006直流无刷电机2
大疆C610无刷电机调速器2
蓝牙模块
蓝牙调试助手APP
RT-Thread操作系统
面向对象模块接口设计
双舵轮底盘，四个小全向轮为支撑轮

代码工程

stm32f407_DJIc.rar
工程基础：
LED频闪指示灯模块： https://www.yuque.com/wangxi_chn/qaxke0/mltilp
DJIC610620电机群模块：https://www.yuque.com/wangxi_chn/qaxke0/ulrbrk
蓝牙驱动遥控模块：https://www.yuque.com/wangxi_chn/qaxke0/dmwgwv

工程组成

将电机群和蓝牙进一步封装为底盘控制的模块。命名为

  • App_SteerWheelBase.c

  • App_SteerWheelBase.h

    App_SteerWheelBase.h

    1. #define    SPEEDWHEELID_LEFT    DjiC610620ID_1
    2. #define    SPEEDWHEELID_RIGHT    DjiC610620ID_2
    3. #define    ANGLEWHEELID_LEFT    DjiC610620ID_3
    4. #define    ANGLEWHEELID_RIGHT    DjiC610620ID_4

  • 指定使用的电机对应的电机号

    • SPEEDWHEELID_LEFT：左侧速度轮
    • SPEEDWHEELID_RIGHT：右侧速度轮
    • ANGLEWHEELID_LEFT：左侧转向轮

    • ANGLEWHEELID_RIGHT：右侧转向轮

      1. enum STEERWHEELBASE_STATION
      2. {
      3. STATIC = 0,
      5. GO_AHEAD,
      6. GO_AHEAD_RIGHT,
      7. GO_RIGHT,
      8. GO_RIGHT_BACK,
      9. GO_BACK,
      10. GO_BACK_LEFT,
      11. GO_LEFT,
      12. GO_LEFT_AHEAD,
      14. ROTATE_CLOCKWISE,
      15. ROTATE_UNCLOCKWISE,
      16. };

  • 枚举指示底盘运动的各种状态，包括八个方位和顺时针逆时针转动

    1. struct _APP_STEERWHEELBASE
    2. {
    3.   /* Property */
    4.   rt_uint16_t                SPEEDWHEELRATE;
    5.   rt_uint16_t                ANGLEWHEELRATE;
    7.   MODULE_DjiC610620GROUP     dev_motorGroup;
    8.   MODULE_BLUETOOTHHC06     dev_btControler;

  • 模块结构体中的必要初始化属性

    • SPEEDWHEELRATE：3508电机转速比率（电机群的控制针对的永远是转子得到的原始数据，通过设置这个参数，实现减速比的转换，齿轮的转换等，映射到轮子的转速）
    • ANGLEWHEELRATE：2006电机角度转换比率，（电机群的控制针对的永远是转子得到的原始数据，通过设置这个参数，实现减速比的转换，齿轮的转换等，映射到转向轮转过的角度）
    • dev_motorGroup：使用的电机群模块实体（结构体嵌套）
    • dev_btControler：使用的蓝牙模块实体（结构体嵌套）
      1. /* Value */
      2. enum STEERWHEELBASE_STATION Value_station;
      3. rt_uint16_t                 Value_DataShowPeriod;
      4. rt_int16_t                    Value_Speed;               //mm/s
      5. rt_int16_t                    Value_Angle;               //°

  • 模块结构体中的中间数值

    • Value_station：当前底盘状态
    • Value_DataShowPeriod：蓝牙回传显示数据的周期
    • Value_Speed：底盘运动速度
    • Value_Angle：转向轮转过的角度
      1. /* Method */
      2. void (*Method_Init)(struct _APP_STEERWHEELBASE *app);
      3. void (*Method_OrderUpdate)(struct _APP_STEERWHEELBASE *app);
      4. void (*Method_DataUpdate)(struct _APP_STEERWHEELBASE *app);
      5. void (*Method_DataShow)(struct _APP_STEERWHEELBASE *app);
      6. void (*Method_Control)(struct _APP_STEERWHEELBASE *app);
      7. };
      8. typedef struct _APP_STEERWHEELBASE APP_STEERWHEELBASE;

  • 模块结构体提供的方法函数（函数指针成员）

    • Method_Init：模块初始化函数，不为用户调用，在调用Config全局函数时自动进行
    • Method_OrderUpdate：模块命令更新函数，在某一线程中被用户调用，用来更新蓝牙遥控下达的命令
    • Method_DataUpdate：模块数据更新函数，在某一线程中被用户调用，用来更新电机回传数据
    • Method_DataShow：模块数据显示函数，在某一线程中被用户调用，用来回传到手机中参数，比如当前车速等
    • Method_Control：模块控制函数，在某一线程中被用户调用，用来发送电机的控制报文
      1. /* Glodal Method */
      2. rt_err_t App_SteerWheelBase_Config(APP_STEERWHEELBASE *App_SteerWheelBase);

  • 使能模块的全局函数

    • 在main函数中被用户调用，实现结构体中函数指针成员的绑定，初始化设备等
    • 只有调用这个函数，结构体配置才会作用，否则报错（RTT-shell显示线程错误）

      App_SteerWheelBase.c

  • 这里重点讲述底盘控制的方法函数

    1. static void App_SteerWheelBaseControl(APP_STEERWHEELBASE *App_SteerWheelBase)
    2. {
    3.   rt_int8_t ClockWiseFlag_L = 1;
    4.   rt_int8_t ClockWiseFlag_R = 1;

  • 指示底盘顺时针逆时针旋转的电机控制系数

    1. switch(App_SteerWheelBase->dev_btControler.Value_keyMask)
    2.   {
    3.       case (0x0001<<0):      
    4.           App_SteerWheelBase->Value_station = GO_LEFT_AHEAD;
    5.           App_SteerWheelBase->Value_Speed += 5;
    6.           App_SteerWheelBase->Value_Angle = 45;
    7.           App_SteerWheelBase->dev_btControler.Value_keyMask = 0;
    8.           break;
    9. ...
    10. ...
    11. ...
    12.       case (0x0001<<11):
    13.           App_SteerWheelBase->Value_station = ROTATE_CLOCKWISE;
    14.           App_SteerWheelBase->Value_Speed += 5;
    15.           App_SteerWheelBase->Value_Angle = 0;
    16.           App_SteerWheelBase->dev_btControler.Value_keyMask = 0;
    17.           break;            
    18.   }

  • 读取蓝牙模块下的按键标识掩码（在模块的命令更新方法中被标记哪个按键被按下）

  • 根据标识，修改底盘模块状态、控制参数（速度，角度）

  • 清空标识，放置反复作用（这里为什么不放在整个switch后清空，避免按键还未发生作用就没清空，影响控制效率，确保按键生效，修改控制参数后再清空）

    1. if(App_SteerWheelBase->Value_station == ROTATE_CLOCKWISE)
    2.   {
    3.       ClockWiseFlag_L = 1;
    4.       ClockWiseFlag_R = -1;
    5.   }
    6.   else if(App_SteerWheelBase->Value_station == ROTATE_UNCLOCKWISE)
    7.   {
    8.       ClockWiseFlag_L = -1;
    9.       ClockWiseFlag_R = 1;
    10.   }
    11.   else
    12.   {
    13.       ClockWiseFlag_L = 1;
    14.       ClockWiseFlag_R = 1;
    15.   }

  • 根据底盘状态（顺时针/逆时针），修改参数值

    1.       /* If open the blue tooth connect check, and now is disconnected, stop all movement */
    2.   if(    App_SteerWheelBase->dev_btControler.ConnectCheckSwitch == 1 &&
    3.       App_SteerWheelBase->dev_btControler.ConnectStatus == DISCONNECTED)
    4.   {
    5.       App_SteerWheelBase->Value_Angle = 0;
    6.       App_SteerWheelBase->Value_Speed = 0;
    7.   }

  • 蓝牙掉线保护，蓝牙是否发生掉线在蓝牙数据发送中已判断，这里直接读取状态

    • 当蓝牙保护已开启，而且发生掉线时，强制制动，角度归零 ```c App_SteerWheelBase->dev_motorGroup.Value_module_DjiC610620[ANGLEWHEELID_LEFT].Value_motor_AimAngle = App_SteerWheelBase->Value_Angle App_SteerWheelBase->ANGLEWHEELRATE; App_SteerWheelBase->dev_motorGroup.Value_module_DjiC610620[ANGLEWHEELID_RIGHT].Value_motor_AimAngle = App_SteerWheelBase->Value_Angle App_SteerWheelBase->ANGLEWHEELRATE;

      App_SteerWheelBase->dev_motorGroup.Value_module_DjiC610620[SPEEDWHEELID_LEFT].Value_motor_AimRPM = ClockWiseFlag_L App_SteerWheelBase->Value_Speed App_SteerWheelBase->SPEEDWHEELRATE; App_SteerWheelBase->dev_motorGroup.Value_module_DjiC610620[SPEEDWHEELID_RIGHT].Value_motor_AimRPM = ClockWiseFlag_R App_SteerWheelBase->Value_Speed App_SteerWheelBase->SPEEDWHEELRATE;

      App_SteerWheelBase->dev_motorGroup.Method_Send(&(App_SteerWheelBase->dev_motorGroup));

  2. - 将底盘参数作用于电机群上，发送电机控制报文
  3. <a name="fhP7i"></a>
  4. ## main
  6. - 初始化配置
  7. ```c
  8. MODULE_LED dev_led_state = 
  9.     {    
  10.         .pin                 = GET_PIN(H, 10),
  11.         .Switch             = 1,
  12.         .LED_TIME_CYCLE     = 4000,
  13.         .LED_TIME_OUTPUT     = 200
  14.     };
  16. MODULE_LED dev_led_alarm = 
  17.     {    
  18.         .pin                 = GET_PIN(H, 12),
  19.         .Switch             = 1,
  20.         .LED_TIME_CYCLE     = 4000,
  21.         .LED_TIME_OUTPUT     = 200
  22.     };

  • 频闪指示灯（运行灯和警示灯）

    1. APP_STEERWHEELBASE app_SteerWheelBase = {
    2.   .SPEEDWHEELRATE = 189  ,//45*23/11*3591/187*2*3.1416/60
    3.   .ANGLEWHEELRATE = 148  ,//66/16*36
    4.   .Value_DataShowPeriod = 500,
    5.   /* motor device */
    6.   .dev_motorGroup = {
    7.       .Property_CanDevName = "can1",
    9.       /* Left Speed Wheel */
    10.       .Value_module_DjiC610620[SPEEDWHEELID_LEFT] = {
    11.           .Enable = 1,
    12.           .Mode    = DjiC610620MODE_SPEED,
    13.           .PID_Speed = {
    14.               .Property_Kp = 5,    .Property_Ki = 0,    .Property_Kd = 0,    .Property_dt = 0.005,
    15.               .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300},},
    17.       /* Right Speed Wheel */
    18.       .Value_module_DjiC610620[SPEEDWHEELID_RIGHT] = {
    19.           .Enable = 1,
    20.           .Mode    = DjiC610620MODE_SPEED,
    21.           .PID_Speed = {
    22.               .Property_Kp = 5,    .Property_Ki = 0,    .Property_Kd = 0,    .Property_dt = 0.005,
    23.               .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300},},
    25.       /* Left Direction Wheel */
    26.       .Value_module_DjiC610620[ANGLEWHEELID_LEFT] = {
    27.           .Enable = 1,
    28.           .Mode    = DjiC610620MODE_ANGLE,
    29.           .PID_Speed = {
    30.               .Property_Kp = 3,    .Property_Ki = 1,        .Property_Kd = 0,    .Property_dt = 0.005,
    31.               .Property_integralMax = 500,    .Property_AimMax = 14000,    .Property_OutputMax =10000},
    32.           .PID_Angle = {
    33.               .Property_Kp = 1,    .Property_Ki = 0.2,    .Property_Kd = 0,    .Property_dt = 0.005,
    34.               .Property_integralMax = 500,    .Property_AimMax = 60000,    .Property_OutputMax =14000},},
    36.       /* Right Direction Wheel */
    37.       .Value_module_DjiC610620[ANGLEWHEELID_RIGHT] = {
    38.           .Enable = 1,
    39.           .Mode    = DjiC610620MODE_ANGLE,
    40.           .PID_Speed = {
    41.               .Property_Kp = 3,    .Property_Ki = 1,        .Property_Kd = 0,    .Property_dt = 0.005,
    42.               .Property_integralMax = 500,    .Property_AimMax = 14000,    .Property_OutputMax =10000},
    43.           .PID_Angle = {
    44.               .Property_Kp = 1,    .Property_Ki = 0.2,    .Property_Kd = 0,    .Property_dt = 0.005,
    45.               .Property_integralMax = 500,    .Property_AimMax = 60000,    .Property_OutputMax =14000},},},
    47.   /* bluetooth controler device */
    48.   .dev_btControler = {
    49.       .Property_UartDevName    = "uart6",
    50.       .ConnectStatusPin = GET_PIN(I, 7),
    51.       .ConnectCheckSwitch = 1,
    52.   },
    53. };

  • 对模块的各个成员赋值，这里仅对Property属性赋值即可，模块作用的必要参数

    • 换算率
    • 显示周期
    • 电机群模块
      • 电机群can设备
      • 电机1
        • 电机使能
        • 电机模式（速度控制/角度控制）
        • 电机速度PID
        • 电机角度PID
      • 电机2
        • …
        • …
      • 电..
    • 蓝牙模块
      • 串口设备
      • 掉线指示引脚
      • 掉线保护开启

        初始化

        1. Module_Led_Config(&dev_led_state);    
        2. dev_led_state.Set(&dev_led_state,9);//频闪9次每周期
        3. Module_Led_Config(&dev_led_alarm);    
        4. dev_led_state.Set(&dev_led_alarm,9);//频闪9次每周期
        5. App_SteerWheelBase_Config(&app_SteerWheelBase);

  • 使能各个模块

    1.   rt_thread_t led_thread = rt_thread_create("ledshine", led_shine_entry, RT_NULL,
    2.                                 512, RT_THREAD_PRIORITY_MAX - 3, 20);
    3.   if (led_thread != RT_NULL){
    4.       rt_thread_startup(led_thread);
    5.   }    
    7.   rt_thread_t OrderUpdate_thread = rt_thread_create("OrderUpdate_thread", OrderUpdate_entry, RT_NULL,
    8.                                 512, RT_THREAD_PRIORITY_MAX - 9, 20);
    9.   if (OrderUpdate_thread != RT_NULL){
    10.       rt_thread_startup(OrderUpdate_thread);
    11.   }
    13.   rt_thread_t DataUpdate_thread = rt_thread_create("DataUpdate_thread", DataUpdate_entry, RT_NULL,
    14.                                 512, RT_THREAD_PRIORITY_MAX - 8, 20);
    15.   if (DataUpdate_thread != RT_NULL){
    16.       rt_thread_startup(DataUpdate_thread);
    17.   }    
    19.   rt_thread_t Control_thread = rt_thread_create("Control_thread", Control_entry, RT_NULL,
    20.                                 512, RT_THREAD_PRIORITY_MAX - 10, 20);
    21.   if (Control_thread != RT_NULL){
    22.       rt_thread_startup(Control_thread);
    23.   }
    25.   rt_thread_t DataShow_thread = rt_thread_create("DataShow_thread", DataShow_entry, RT_NULL,
    26.                                 512, RT_THREAD_PRIORITY_MAX - 4, 20);
    27.   if (DataShow_thread != RT_NULL){
    28.       rt_thread_startup(DataShow_thread);
    29.   }

  • 分配线程

    控制效果

    角度响应很快，但是否准确还需再测试，原因可见链接https://www.yuque.com/wangxi_chn/qaxke0/ulrbrk#ZzYzP
    还未配合舵轮上的磁编码器使用，手动定转向轮初始位置
    现有双舵轮+四支撑轮结构对地面平整要求较高，如果不平整会导致两个舵轮被架空
    两轮转速可能会受地面影响而不一致导致自然偏斜，需要配合陀螺仪角度闭环 90b3b00de5746efcad58b045e20d53a2.gif (52.98MB)