测试环境

CubeMX
MDK5 IDE
STM32F429IGT6 芯片
正点原子开发板 阿波罗
STM32F103C8T6 芯片
Stm32最小系统板
RT-Thread操作系统
面向对象模块接口设计
LPD3806-400BM-G5-24C 脉冲AB相增量式光电旋转编码器
大疆M3508直流无刷电机3
大疆C620无刷电机调速器3
两个3508电机用来驱动摩擦轮，仅速度控制
一个3508电机用来驱动取球爪子，角度控制
但由于3508电机位置控制的问题配合编码器读取爪子转动角度
原因：https://www.yuque.com/wangxi_chn/qaxke0/ulrbrk#ZzYzP

硬件条件

  • 因为使用的是正点原子开发板，芯片引脚被多处复用，定时器的两个引脚无法正常读取脉冲数（如果使用的是实验室自己绘制的核心板就不会有这个问题）
  • 所以这里借用了一个Stm32最小系统板只用来读取编码器，然后通过串口把数据发送给开发板

  • 两块板通过串口线连接，同时正点原子开发板为最小系统板供电，共地

    代码工程

    stm32f103_MIN.rar
    stm32f429_ALIENTEX.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
    串口板级通信模块：https://www.yuque.com/wangxi_chn/qaxke0/xpkyvm
    ROTARY编码器驱动模块：https://www.yuque.com/wangxi_chn/qaxke0/ketr3p

    工程组成

  • 没有进一步封装，直接调用各个模块的方法在主函数内组合作用

    STM32F429主控 main

  • 初始化配置

    1. MODULE_LED dev_led_state = 
    2.   {    
    3.       .pin                 = GET_PIN(B, 0),
    4.       .LED_TIME_CYCLE     = 4000,
    5.       .LED_TIME_OUTPUT     = 200
    6.   };

  • 频闪系统运行状态显示灯

    1. MODULE_BLUETOOTHHC06 dev_communicate = 
    2.   {
    3.       .Property_UartDevName    = "uart2",
    4.   };

  • 蓝牙模块

    1. MODULE_BSPCOMMUNICATE dev_BspCommunicate = 
    2.   {
    3.       .Property_UartDevName    = "uart3",
    4.   };

  • 串口板级通信模块

  • 和最小系统板通信，读取编码器信息

    1. MODULE_DjiC610620GROUP dev_DjiC610620group = 
    2. {
    3.   .Property_CanDevName = "can1",
    4.   .Value_module_DjiC610620[DjiC610620ID_1] = 
    5.   {
    6.       .Enable = 1,
    7.       .Mode    = DjiC610620MODE_SPEED,
    8.       .ENCODER = DjiC610620SELF,
    9.       .PID_Speed = 
    10.       {
    11.           .Property_Kp = 6,    .Property_Ki = 2,    .Property_Kd = 0,    .Property_dt = 0.005,
    12.           .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
    13.           .Property_integralErrMax = 500,
    14.       }
    15.   },
    16.   .Value_module_DjiC610620[DjiC610620ID_2] = 
    17.   {
    18.       .Enable = 1,
    19.       .Mode    = DjiC610620MODE_SPEED,
    20.       .ENCODER = DjiC610620SELF,
    21.       .PID_Speed = 
    22.       {
    23.           .Property_Kp = 6,    .Property_Ki = 2,    .Property_Kd = 0,    .Property_dt = 0.005,
    24.           .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
    25.           .Property_integralErrMax = 500,
    26.       }
    27.   },
    28.   .Value_module_DjiC610620[DjiC610620ID_3] = 
    29.   {
    30.       .Enable = 1,
    31.       .Mode    = DjiC610620MODE_ANGLE,
    32.       .ENCODER = ROTARYENCODER,
    33.       .PID_Speed = 
    34.       {
    35.           .Property_Kp = 6,    .Property_Ki = 1,    .Property_Kd = 0,    .Property_dt = 0.005,
    36.           .Property_integralMax = 2000,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
    37.           .Property_integralErrMax = 1000,
    38.       },
    39.       .PID_Angle = 
    40.       {
    41.           .Property_Kp = 2,    .Property_Ki = 0.5,    .Property_Kd = 0,    .Property_dt = 0.005,
    42.           .Property_integralMax = 2000,    .Property_AimMax = 60000,    .Property_OutputMax =1000,
    43.           .Property_integralErrMax = 10000,
    44.       },
    45.   },
    46. };

  • 电机群模块

    • 分别使能三个ID为1、2、3的3508电机
    • 1、2为速度模式，3为角度模式
    • 配置1，2电机的速度控制PID，3电机的角度控制PID
    • 注意角度控制PID PID_Angle 的 Property_OutputMax 即为达到目标角度的速度大小
    • 注意设置电机的编码来源：电机1和2使用自身编码器，电机3使用ROTARY编码器

    • 这里ROTARY编码器没有集成在里面，如果 .ENCODER = ROTARYENCODER, 需要自己手动实现对电机角度的赋值，可看后面内容

      1. Module_Led_Config(&dev_led_state);    
      2. dev_led_state.Set(&dev_led_state,9);
      4. Module_DjiC610620Group_Config(&dev_DjiC610620group);
      6. Module_BlueToothHC06_Config(&dev_communicate);
      8. Module_BspCommunicate_Config(&dev_BspCommunicate);

  • 使能配置

    1.   /* system running shine led thread */
    2.   rt_thread_t led_thread = rt_thread_create("ledshine", led_shine_entry, RT_NULL,
    3.                                 512, RT_THREAD_PRIORITY_MAX - 3, 20);
    4.   if (led_thread != RT_NULL){
    5.       rt_thread_startup(led_thread);
    6.   }    
    8.   /* motor control thread */
    9.   rt_thread_t motor_thread = rt_thread_create("motor", motor_entry, RT_NULL,
    10.                                 1024, RT_THREAD_PRIORITY_MAX - 6, 20);
    11.   if (motor_thread != RT_NULL){
    12.       rt_thread_startup(motor_thread);
    13.   }
    15.   /* blue tooth updata thread */
    16.   rt_thread_t bluetupdata_thread = rt_thread_create("bluetupdata", bluetupdata_entry, RT_NULL,
    17.                                 512, RT_THREAD_PRIORITY_MAX - 5, 20);
    18.   if (bluetupdata_thread != RT_NULL){
    19.       rt_thread_startup(bluetupdata_thread);
    20.   }    
    22.   /* blue tooth control thread */
    23.   rt_thread_t bluetcontrol_thread = rt_thread_create("bluetcontrol", bluetcontrol_entry, RT_NULL,
    24.                                 512, RT_THREAD_PRIORITY_MAX - 5, 20);
    25.   if (bluetcontrol_thread != RT_NULL){
    26.       rt_thread_startup(bluetcontrol_thread);
    27.   }
    29.   /* bsp communicate thread */
    30.   rt_thread_t bspCommnicate_thread = rt_thread_create("bspCommnicate", bspCommnicate_entry, RT_NULL,
    31.                                 512, RT_THREAD_PRIORITY_MAX - 6, 20);
    32.   if (bspCommnicate_thread != RT_NULL){
    33.       rt_thread_startup(bspCommnicate_thread);
    34.   }

  • 配置线程

    1. static void motor_entry(void *parameter)
    2. {
    3.   while (1)
    4.   {    
    5.       //get data from outside
    6.       dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].Value_motor_TotalAngle = 
    7.           -(dev_BspCommunicate.Value_RotaryEncoder * 8);
    9.       /* Communicate with motor group */        
    10.       dev_DjiC610620group.Method_Feed(&dev_DjiC610620group);
    11.       rt_thread_mdelay(1);
    12.       dev_DjiC610620group.Method_Send(&dev_DjiC610620group);
    13.   }
    14. }

  • 在电机线程中，实现读取报文，计算PID，发送报文

  • 注意这里电机3的角度从编码器中获得，即从板级通信中获得，角度值已在板级通信的线程中更新
  • 加上负号是为了和电机自带的编码器角度读数方向一致，避免电机PID逻辑错误

  • 乘上了8是为了让编码器获得的脉冲数和电机自带编码器的数量级一致，方便统一PID参数的大小

    1. static void bluetcontrol_entry(void *parameter)
    2. {
    3.   static rt_uint32_t time_1000m = 0;
    4.   while(1)
    5.   {
    6.       rt_thread_mdelay(1);
    7.       time_1000m++;
    8.       switch(dev_communicate.Value_keyMask)
    9.       {
    10.           case (0x0001<<0):      //speed up the friction wheel
    11.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimRPM -= 200;
    12.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM += 200;
    13.               break;
    14.           case (0x0001<<1):    //start up the friction wheel
    15.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimRPM = -2000;
    16.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM = 2000;
    17.               break;
    18.           case (0x0001<<2):    //slow down the friction wheel
    19.               if(dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM >= 200)
    20.               {
    21.                   dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimRPM += 200;
    22.                   dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM -= 200;
    23.               }
    24.               break;
    25.           case (0x0001<<3):    //raise up the paw
    26.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_Kp = 3;
    27.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_Ki = 0.5;
    28.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Speed.Value_integral = 0;
    29.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Value_integral = 0;
    30.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_integralErrMax = 1000;
    31.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_OutputMax = 1500;
    32.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].Value_motor_AimAngle = -2200;
    33.               break;
    34.           case (0x0001<<4):    //stop the friction wheel
    35.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_1].Value_motor_AimRPM = 0;
    36.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM = 0;                
    37.               break;
    38.           case (0x0001<<5):    //put down the paw
    39.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_Kp = 3;
    40.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Speed.Property_Kp = 4;
    41.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_Ki = 0.1;
    42.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Speed.Value_integral = 0;
    43.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Value_integral = 0;
    44.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_integralErrMax = 30;
    45.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].PID_Angle.Property_OutputMax = 300;
    46.               dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].Value_motor_AimAngle = 0;
    47.               break;
    48.           case (0x0001<<6):
    49.               break;
    50.           case (0x0001<<7):
    51.               break;
    52.           case (0x0001<<8):
    53.               break;
    54.           case (0x0001<<9):
    55.               break;
    56.           case (0x0001<<10):
    57.               break;
    58.           case (0x0001<<11):
    59.               break;
    61.       }
    62.       dev_communicate.Value_keyMask = 0x0000;
    64.       if(time_1000m >= 1000)
    65.       {
    66.           time_1000m = 0;
    68.           /* 可以在这里数据回显，打印等，可调整发送频率 */
    69. //            dev_communicate.Method_Send(&dev_communicate,PARAM_SHOW_1,
    70. //                dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM);
    71. //            dev_communicate.Method_Send(&dev_communicate,WAVE_SHOW,
    72. //                dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_2].Value_motor_AimRPM);
    73. //            rt_device_read(    dev_DjiC610620group.Value_module_DjiC610620[DjiC610620ID_3].Encoder_dev, 0, 
    74. //                            &count, 1);    
    75. //            rt_kprintf("get count %d\n",count);
    77.       }
    78.   }
    79. }

  • 通过蓝牙按键实现动作控制

    • 其实不一定非要使用蓝牙功能，可以将这里对应于任何一种遥控器的接口
    • 格外注意在 //raise up the paw 和 //put down the paw 的分支中，修改了PID参数、清除积分后实现电机控制
    • 这主要是因为在爪子上下翻转的过程中，电机的负载是不同的
      • 向上运动时夹持着橄榄球，PID参数较为激进，否则无法抬起爪子
      • 在向下运动时由于橄榄球已经飞出，负载较清，PID参数较缓，否则爪子下落力度很大，损坏齿轮或机构
    • 因此直接通过修改标志上下移动爪子更方便调试参数

      STM32F103读数 main

  • 初始化配置

    1. MODULE_BSPCOMMUNICATE dev_BspCommunicate = 
    2.   {
    3.       .Property_UartDevName    = "uart2",
    4.   };

  • 串口板级通信模块

    1. int main(void)
    2. {
    3.   rt_err_t ret = RT_EOK;
    4.   rt_device_t pulse_encoder_dev = RT_NULL;   /* 脉冲编码器设备句柄 */
    6.   /* 查找脉冲编码器设备 */
    7.   pulse_encoder_dev = rt_device_find(PULSE_ENCODER_DEV_NAME);
    8.   if (pulse_encoder_dev == RT_NULL)
    9.   {
    10.       rt_kprintf("pulse encoder sample run failed! can't find %s device!\n", PULSE_ENCODER_DEV_NAME);
    11.       return RT_ERROR;
    12.   }
    14.   /* 以只读方式打开设备 */
    15.   ret = rt_device_open(pulse_encoder_dev, RT_DEVICE_OFLAG_RDONLY);
    16.   if (ret != RT_EOK)
    17.   {
    18.       rt_kprintf("open %s device failed!\n", PULSE_ENCODER_DEV_NAME);
    19.       return ret;
    20.   }
    22.   rt_pin_mode(GET_PIN(B,  6), PIN_MODE_INPUT_PULLUP);
    23.   rt_pin_mode(GET_PIN(B,  7), PIN_MODE_INPUT_PULLUP);

  • 这里没有将编码器功能封装成模块，直接使用

    1. rt_pin_mode(GET_PIN(C,  13), PIN_MODE_OUTPUT);

  • 发送状态指示灯

    1. Module_BspCommunicate_Config(&dev_BspCommunicate);

  • 使能板级通信模块

    1. while(1)
    2.   {
    3.       rt_thread_mdelay(1);
    4.       /* 读取脉冲编码器计数值 */
    5.       rt_pin_write(GET_PIN(C,  13), PIN_LOW);
    6.       rt_device_read(pulse_encoder_dev, 0, &(dev_BspCommunicate.Value_RotaryEncoder), 1);        
    7.       if(abs(dev_BspCommunicate.Value_RotaryEncoder)>20000)
    8.       {
    9.           /* 清空脉冲编码器计数值 */
    10.           rt_device_control(pulse_encoder_dev, PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
    11.       }
    12.       dev_BspCommunicate.Method_Send(&dev_BspCommunicate,M0_D1,dev_BspCommunicate.Value_RotaryEncoder);
    13.       rt_pin_write(GET_PIN(C,  13), PIN_HIGH);
    14.   }

  • 没有使用多余线程，这里只使用了主线程工作

  • LPD3806-400BM-G5-24C 脉冲AB相增量式光电旋转编码器，不能抗冲击，当落爪子力度较大，会导致读到异常数据
  • 这里判断读数大于20000强制编码器读数清零，

  • 测试效果较好

    存在问题

  • 由于3508电机位置控制缺陷，原因：https://www.yuque.com/wangxi_chn/qaxke0/ulrbrk#ZzYzP

    • 需要额外配置编码器，因此建议角度控制电机选用其他类型（舵机、步进电机、3863这类内部集成脉冲编码器的电机）

  • 取球爪的PID是个较为复杂的过程

    • 在向上抬升爪子的时候，随着角度的变化，负载在变化，但是变化程度较小
    • 在将橄榄球送至发球处时，橄榄球会瞬间被摩擦轮发出，负载突变，此时爪子控制电机会有较大抖动
    • 向下回落爪子的时候，负载较轻，需要修改PID参数，否则损坏机构

      解决途径

  • 换用3863电机，角度控制，但是力量会很大，可以使用速度角度控制（目前机械方案修改方向）

  • 还需仔细研究串级PID结构控制原理