Gitee

https://gitee.com/WangXi_Chn/RttOs_ModuleLib

开发环境

CubeMX
MDK5 IDE
STM32F407IGH6TR 芯片
大疆开发板 C型
大疆M3508直流无刷电机
大疆C620无刷电机调速器
RT-Thread操作系统
面向对象模块接口设计

硬件条件

  • 确保Can线供电电平达到5V（不可仅使用JTAG接口供电3.3V）
  • Can线接口连接正确

  • 接口函数、驱动等同上一节

    核心思想

  • 将一条Can线上的所有3508电机视为一个电机群

  • 最大支持驱动8个不同ID的电机（符合电机电调参数手册）
  • 隐藏固定的ID，减少驱动工作量，使能一个电机仅需打开开关，未打开的电机不使能
  • 通过掩码思想，标记已打开的电机，并仅驱动打开的电机
  • 设计电机工作模式：角度环/速度环，支持切换
  • 每个电机绑定两个不同的PID分别控制角度和速度，根据电机模式分别使能
  • 模块化思想

    源文件

    Module_DjiC610620Group.h

    ```c /*
    • Copyright (c) 2020 - ~, HIT_HERO Team *
    • 2508MOTOR_CAN MODULE HEAD FILE
    • Used in RT-Thread Operate System *
    • Change Logs:
    • Date Author Notes Mail
    • 2020-08-08 WangXi first version WangXi_chn@foxmail.com *
    • Note:
    • DJI 3508 motor and C620 motor controler driver and control framework
    • All paramemters shall be subject to the motor rotor control
    • And the parameters of the shaft shall be converted by yourself if needed
    • The positive direction is clockwise facing the rotor */

ifndef MODULE_DJIC610620GROUP_H

define MODULE_DJIC610620GROUP_H

include

include

include

include “Math_PID.h”

/ Definition —————————————————————————————/ enum DjiC610620ID { DjiC610620ID_1 = 0, DjiC610620ID_2 = 1, DjiC610620ID_3 = 2, DjiC610620ID_4 = 3, DjiC610620ID_5 = 4, DjiC610620ID_6 = 5, DjiC610620ID_7 = 6, DjiC610620ID_8 = 7, };

enum DjiC610620MODE { DjiC610620MODE_SPEED = 0, DjiC610620MODE_ANGLE = 1, };

/ Single motor label —————————————————————————————/ struct _MODULE_DjiC610620 { / Property / rt_uint8_t Enable; rt_uint8_t Mode;

  1. MATH_PID         PID_Speed;
  2. MATH_PID         PID_Angle;
  4. /* Value */        
  5. rt_int16_t Value_motor_AimCurrent;
  6. rt_int16_t Value_motor_AimRPM;
  7. rt_int16_t Value_motor_AimAngle;
  8. rt_int16_t Value_motor_RealCurrent;
  9. rt_int16_t Value_motor_RealRPM;
  10. rt_int16_t Value_motor_RealAngle;
  11. rt_uint8_t Value_motor_Temperature;
  13. rt_int8_t  Value_motor_InitFlag;
  14. rt_int16_t Value_motor_OffsetAngle;
  15. rt_int16_t Value_motor_LastAngle;
  16. rt_int32_t Value_motor_TotalAngle;   /* after the conversion (°) */
  17. rt_int16_t Value_motor_AngleCnt;
  19. /* Method */
  20. void (*Method_Init)(struct _MODULE_DjiC610620 *module);

}; typedef struct _MODULE_DjiC610620 MODULE_DjiC610620;

/ Motor group label —————————————————————————————/ struct _MODULE_DjiC610620GROUP { / Property / char * Property_CanDevName; MODULE_DjiC610620 Value_module_DjiC610620[7];

  1. /* Value */
  2. struct rt_can_msg     Value_can_msgLow;
  3. struct rt_can_msg     Value_can_msgHigh;
  4. struct rt_can_msg     Value_can_mrg;
  5. rt_device_t         Value_can_dev;
  6. rt_uint8_t             Value_motorUsedMask;
  9. /* Method */
  10. void (*Method_Init)            (struct _MODULE_DjiC610620GROUP *module);
  11. void (*Method_Send)            (struct _MODULE_DjiC610620GROUP *module);
  12. void (*Method_Feed)            (struct _MODULE_DjiC610620GROUP *module);
  13. rt_err_t (*Method_Handle)    (rt_device_t dev,rt_size_t size);

}; typedef struct _MODULE_DjiC610620GROUP MODULE_DjiC610620GROUP;

/ Glodal Method / rt_err_t Module_DjiC610620Group_Config(MODULE_DjiC610620GROUP *Dev_DjiC610620Group);

endif

/** (C) COPYRIGHT 2020 WANGXI **END OF FILE**/

  1. <a name="bYc1E"></a>
  2. ## Module_DjiC610620Group.c
  3. ```c
  4. /*
  5.  * Copyright (c) 2020 - ~, HIT_HERO Team
  6.  *
  7.  * 2508MOTOR_CAN MODULE SOUCE FILE
  8.  * Used in RT-Thread Operate System
  9.  *
  10.  * Change Logs:
  11.  * Date           Author       Notes                Mail
  12.  * 2020-08-08     WangXi       first version        WangXi_chn@foxmail.com
  13.  * 
  14.  * Note:
  15.  * DJI 3508 motor and C620 motor controler driver and control framework
  16.  * All paramemters shall be subject to the motor rotor control
  17.  * And the parameters of the shaft shall be converted by yourself if needed
  18.  * The positive direction is clockwise facing the rotor
  19.  */
  21. #include "Module_DjiC610620Group.h"
  23. /* User Code Begin*/
  26. /* User Code End */
  28. /* Static Method */
  30. static void     Module_DjiC610620GroupInit(MODULE_DjiC610620GROUP *module);
  31. static void     Module_DjiC610620GroupSend(MODULE_DjiC610620GROUP *module);
  32. static void     Module_DjiC610620GroupFeed(MODULE_DjiC610620GROUP *module);
  33. static rt_err_t Module_DjiC610620GroupHandle(rt_device_t dev, rt_size_t size);
  35. static rt_err_t Module_DjiC610620_Config(MODULE_DjiC610620 *Dev_DjiC610620);
  36. static void     Module_DjiC610620Init(MODULE_DjiC610620 *module);
  38. static struct rt_semaphore can3508Motor_sem;
  41. /* Global Method */
  42. rt_err_t Module_DjiC610620Group_Config(MODULE_DjiC610620GROUP *Dev_DjiC610620Group)
  43. {
  44.     if( Dev_DjiC610620Group->Method_Init        ==NULL &&
  45.         Dev_DjiC610620Group->Method_Feed        ==NULL &&
  46.         Dev_DjiC610620Group->Method_Send        ==NULL &&
  47.         Dev_DjiC610620Group->Method_Handle    ==NULL
  49.     ){  
  50.         /* Link the Method */
  51.         Dev_DjiC610620Group->Method_Init     = Module_DjiC610620GroupInit;
  52.         Dev_DjiC610620Group->Method_Feed        = Module_DjiC610620GroupFeed;
  53.         Dev_DjiC610620Group->Method_Send        = Module_DjiC610620GroupSend;
  54.         Dev_DjiC610620Group->Method_Handle    = Module_DjiC610620GroupHandle;
  56.     }
  57.     else{
  58.         rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
  59.         return RT_ERROR;
  60.     }
  62.     /* Device Init */
  63.     Dev_DjiC610620Group->Method_Init(Dev_DjiC610620Group);
  65.     /* Motor Config */
  66.     for(int i=0;i<8;i++)
  67.     {
  68.         if(Dev_DjiC610620Group->Value_module_DjiC610620[i].Enable != 0)
  69.         {
  70.             Dev_DjiC610620Group->Value_motorUsedMask |= 0x01<<i; 
  71.             Module_DjiC610620_Config(&(Dev_DjiC610620Group->Value_module_DjiC610620[i]));
  72.         }
  73.     }
  75.     return RT_EOK;
  76. }
  78. static void Module_DjiC610620GroupInit(MODULE_DjiC610620GROUP *module)
  79. {
  80.     rt_err_t res;
  82.     /* link can devices */
  83.     module->Value_can_dev = rt_device_find(module->Property_CanDevName);
  84.     if (!module->Value_can_dev)
  85.         rt_kprintf("find %s failed!\n", module->Property_CanDevName);
  87.     /* init semaphone */
  88.     rt_sem_init(&can3508Motor_sem, "can3508Motor_sem", 0, RT_IPC_FLAG_FIFO);
  90.     module->Value_can_msgLow.id = 0x200;
  91.     module->Value_can_msgLow.ide = RT_CAN_STDID;
  92.     module->Value_can_msgLow.rtr = RT_CAN_DTR;
  93.     module->Value_can_msgLow.len = 8;
  94.     module->Value_can_msgHigh.id = 0x1FF;
  95.     module->Value_can_msgHigh.ide = RT_CAN_STDID;
  96.     module->Value_can_msgHigh.rtr = RT_CAN_DTR;
  97.     module->Value_can_msgHigh.len = 8;
  98.     module->Value_can_mrg.hdr = -1;
  100.     res = rt_device_open(module->Value_can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
  101.     RT_ASSERT(res == RT_EOK);    
  103.     /* set up can baudrate */
  104.     rt_device_control(module->Value_can_dev, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
  106.     /* link call back function */
  107.     rt_device_set_rx_indicate(module->Value_can_dev, module->Method_Handle);
  110. }
  112. rt_err_t Module_DjiC610620_Config(MODULE_DjiC610620 *Dev_DjiC610620)
  113. {
  114.     if( Dev_DjiC610620->Method_Init        ==NULL
  116.     ){  
  117.         /* Link the Method */
  118.         Dev_DjiC610620->Method_Init     = Module_DjiC610620Init;
  119.     }
  120.     else{
  121.         rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
  122.         return RT_ERROR;
  123.     }
  125.     /* Device Init */
  126.     Dev_DjiC610620->Method_Init(Dev_DjiC610620);
  128.     /* Module PID controler Init */
  129.     if(Dev_DjiC610620->Mode == DjiC610620MODE_SPEED)
  130.     {
  131.         Module_PID_Config(&(Dev_DjiC610620->PID_Speed));
  132.     }
  133.     else if(Dev_DjiC610620->Mode == DjiC610620MODE_ANGLE)
  134.     {
  135.         Module_PID_Config(&(Dev_DjiC610620->PID_Speed));
  136.         Module_PID_Config(&(Dev_DjiC610620->PID_Angle));
  137.     }
  139.     return RT_EOK;
  140. }
  142. /* Static Method */
  143. static void Module_DjiC610620Init(MODULE_DjiC610620 *module)
  144. {    
  145.     module->Value_motor_AimAngle = 0;
  146.     module->Value_motor_AimCurrent = 0;
  147.     module->Value_motor_AimRPM = 0;
  149.     if(module->ENCODER == ROTARYENCODER)
  150.     {    
  151.         module->Encoder_dev = rt_device_find(module->Encoder_DevName);
  152.         if (module->Encoder_dev == RT_NULL){
  153.             rt_kprintf("pulse encoder sample run failed! can't find %s device!\n", module->Encoder_DevName);
  154.             return;
  155.         }
  156.         rt_err_t ret = rt_device_open(module->Encoder_dev, RT_DEVICE_OFLAG_RDONLY);
  157.         if (ret != RT_EOK){
  158.             rt_kprintf("open %s device failed!\n", module->Encoder_DevName);
  159.             return;
  160.         }
  162.         rt_pin_mode(GET_PIN(D,12), PIN_MODE_INPUT_PULLUP);
  163.         rt_pin_mode(GET_PIN(D,13), PIN_MODE_INPUT_PULLUP);
  164.     }
  165. }
  167. static void Module_DjiC610620GroupSend(MODULE_DjiC610620GROUP *module)
  168. {
  169.     rt_size_t  size;
  171.     /* Calculate the final current value of motor */    
  172.     for(int i=0;i<8;i++)
  173.     {
  174.         if((module->Value_motorUsedMask & (0x01<<i))!=0x00)
  175.         {
  176.             module->Value_module_DjiC610620[i].Value_motor_AimCurrent = 
  177.                 module->Value_module_DjiC610620[i].PID_Speed.Value_output;                    
  178.         }        
  179.     }
  181.     /* make up the all can fram */
  182.     if((module->Value_motorUsedMask & 0x0F) != 0)
  183.     {    
  184.         for(int i=1;i<=4;i++)
  185.         {
  186.             module->Value_can_msgLow.data[i*2-1] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent & 0xFF;
  187.             module->Value_can_msgLow.data[i*2-2] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent >> 8;
  188.         }
  190.         size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msgLow), sizeof(module->Value_can_msgLow));
  191.         if (size == 0)
  192.             rt_kprintf("can dev write data failed!\n");
  193.         size = 0;
  194.     }
  196.     rt_thread_mdelay(1);
  198.     if((module->Value_motorUsedMask & 0xF0) != 0)
  199.     {    
  200.         for(int i=5;i<=8;i++)
  201.         {
  202.             module->Value_can_msgHigh.data[i*2-9] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent & 0xFF;
  203.             module->Value_can_msgHigh.data[i*2-10] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent >> 8;
  204.         }
  206.         size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msgHigh), sizeof(module->Value_can_msgHigh));
  207.         if (size == 0)
  208.             rt_kprintf("can dev write data failed!\n");
  209.         size = 0;
  210.     }
  211. }
  213. static rt_err_t Module_DjiC610620GroupHandle(rt_device_t dev, rt_size_t size)
  214. {
  215.     rt_sem_release(&can3508Motor_sem);
  217.     return RT_EOK;
  218. }
  220. static void Module_DjiC610620GroupFeed(MODULE_DjiC610620GROUP *module)
  221. {    
  222.     rt_sem_take(&can3508Motor_sem, RT_WAITING_FOREVER);
  224.     /* Get true value of the motor */
  225.     rt_uint8_t index = 1;
  226.     rt_device_read(module->Value_can_dev, 0, &(module->Value_can_mrg), sizeof(module->Value_can_mrg));
  227.     index = (module->Value_can_mrg.id & 0x00F) - 1;
  229.     module->Value_module_DjiC610620[index].Value_motor_RealAngle = 
  230.         ((module->Value_can_mrg.data[0]<<8) | module->Value_can_mrg.data[1]);
  231.     module->Value_module_DjiC610620[index].Value_motor_RealRPM = 
  232.         ((module->Value_can_mrg.data[2]<<8) | module->Value_can_mrg.data[3]);
  233.     module->Value_module_DjiC610620[index].Value_motor_RealCurrent = 
  234.         ((module->Value_can_mrg.data[4]<<8) | module->Value_can_mrg.data[5]);
  235.     module->Value_module_DjiC610620[index].Value_motor_Temperature = 
  236.         module->Value_can_mrg.data[6];
  239.     /* Get the offset angle */
  240.     if(module->Value_module_DjiC610620[index].Value_motor_InitFlag == 0)
  241.     {
  242.         module->Value_module_DjiC610620[index].Value_motor_OffsetAngle = 
  243.             module->Value_module_DjiC610620[index].Value_motor_RealAngle;
  244.         module->Value_module_DjiC610620[index].Value_motor_InitFlag = 1;
  245.         return;
  246.     }
  248.     if(module->Value_module_DjiC610620[index].Mode == DjiC610620MODE_SPEED)
  249.     {
  250.         /* Update PID output */
  251.         module->Value_module_DjiC610620[index].PID_Speed.Method_Update(
  252.             &(module->Value_module_DjiC610620[index].PID_Speed),
  253.             module->Value_module_DjiC610620[index].Value_motor_AimRPM,
  254.             module->Value_module_DjiC610620[index].Value_motor_RealRPM);    
  255.     }
  256.     else if(module->Value_module_DjiC610620[index].Mode == DjiC610620MODE_ANGLE)
  257.     {
  258.         /* Deal with the data of motor's angle */
  260.         if(module->Value_module_DjiC610620[index].ENCODER == DjiC610620SELF)
  261.         {
  262.             /* Use DjiC610/C620 it self encoder */
  263.             module->Value_module_DjiC610620[index].Value_motor_RealAngle -= 
  264.             module->Value_module_DjiC610620[index].Value_motor_OffsetAngle;
  266.             if(module->Value_module_DjiC610620[index].Value_motor_RealAngle - 
  267.                     module->Value_module_DjiC610620[index].Value_motor_LastAngle > 4096)
  268.                         module->Value_module_DjiC610620[index].Value_motor_AngleCnt --;
  269.             else if (module->Value_module_DjiC610620[index].Value_motor_RealAngle - 
  270.                     module->Value_module_DjiC610620[index].Value_motor_LastAngle < -4096)
  271.                         module->Value_module_DjiC610620[index].Value_motor_AngleCnt ++;
  273.             module->Value_module_DjiC610620[index].Value_motor_LastAngle = 
  274.                 module->Value_module_DjiC610620[index].Value_motor_RealAngle;
  276.             module->Value_module_DjiC610620[index].Value_motor_TotalAngle = 
  277.                 (    module->Value_module_DjiC610620[index].Value_motor_AngleCnt * 8192 + 
  278.                     module->Value_module_DjiC610620[index].Value_motor_RealAngle)*360/8191;
  279.         }        
  280.         else if(module->Value_module_DjiC610620[index].ENCODER == ROTARYENCODER)
  281.         {
  282.             /* Not use DjiC610/C620 it self encoder but use extra rotary encoder*/
  284.             //"module->Value_module_DjiC610620[index].Value_motor_TotalAngle" get from the out side 
  286.         }
  288.         /* Update PID output */
  289.         module->Value_module_DjiC610620[index].PID_Angle.Method_Update(
  290.             &(module->Value_module_DjiC610620[index].PID_Angle),
  291.             module->Value_module_DjiC610620[index].Value_motor_AimAngle,
  292.             module->Value_module_DjiC610620[index].Value_motor_TotalAngle);    
  294.         module->Value_module_DjiC610620[index].Value_motor_AimRPM = 
  295.             module->Value_module_DjiC610620[index].PID_Angle.Value_output;
  297.         module->Value_module_DjiC610620[index].PID_Speed.Method_Update(
  298.             &(module->Value_module_DjiC610620[index].PID_Speed),
  299.             module->Value_module_DjiC610620[index].Value_motor_AimRPM,
  300.             module->Value_module_DjiC610620[index].Value_motor_RealRPM);
  301.     }
  303. }
  306. /************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

Math_PID.h

  1. /*
  2.  * Copyright (c) 2020 - ~, HIT_HERO Team
  3.  *
  4.  * PID MATH MODULE HEAD FILE
  5.  * Used in RT-Thread Operate System
  6.  *
  7.  * Change Logs:
  8.  * Date           Author       Notes                Mail
  9.  * 2020-08-05     WangXi       first version        WangXi_chn@foxmail.com
  10.  */
  11. #include <rtthread.h>
  12. #include <rtdevice.h>
  13. #include <board.h>
  15. #ifndef _MATH_PID_H_
  16. #define _MATH_PID_H_
  18. struct _MATH_PID
  19. {
  20.     /* Property */
  21.     float Property_Kp;
  22.     float Property_Ki;
  23.     float Property_Kd;  
  25.     float Property_dt;
  27.     float Property_integralMax;
  28.     float Property_integralErrMax;
  29.     float Property_AimMax;
  30.     float Property_OutputMax;
  33.     /* Value */
  34.     float Value_Aim;            
  35.     float Value_Actual;         
  36.     float Value_err;                 
  37.     float Value_err_last;                            
  38.     float Value_integral;
  40.     float Value_output;
  42.     /* Method */
  43.     void (*Method_Init)(struct _MATH_PID *module);
  44.     void (*Method_Update)(struct _MATH_PID *module,float Aim, float Feedback);
  46. };
  47. typedef struct _MATH_PID MATH_PID;
  49. rt_err_t Module_PID_Config(MATH_PID *module);
  51. #endif
  53. /************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

Math_PID.c

  1. /*
  2.  * Copyright (c) 2020 - ~, HIT_HERO Team
  3.  *
  4.  * PID MATH MODULE SOUCE FILE
  5.  * Used in RT-Thread Operate System
  6.  *
  7.  * Change Logs:
  8.  * Date           Author       Notes                Mail
  9.  * 2020-08-05     WangXi       first version        WangXi_chn@foxmail.com
  10.  */
  11. #include "Math_PID.h"
  12. #include <math.h>
  14. static void Module_PIDInit(MATH_PID *module);
  15. static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback);
  17. /* Global Method */
  18. rt_err_t Module_PID_Config(MATH_PID *module)
  19. {
  20.     if( module->Method_Init        ==NULL &&
  21.         module->Method_Update        ==NULL 
  23.     ){  
  24.         /* Link the Method */
  25.         module->Method_Init         = Module_PIDInit;
  26.         module->Method_Update     = Module_PIDUpdate;
  28.     }
  29.     else{
  30.         rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
  31.         return RT_ERROR;
  32.     }
  34.     /* Device Init */
  35.     module->Method_Init(module);
  37.     return RT_EOK;
  38. }
  40. static void Module_PIDInit(MATH_PID *module)
  41. {
  42.     module->Value_Actual = 0;
  43.     module->Value_Aim = 0;
  44.     module->Value_err = 0;
  45.     module->Value_err_last = 0;
  46.     module->Value_integral = 0;
  47.     module->Value_output = 0;
  48. }
  50. static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback)
  51. {
  52.     float P,I,D;
  54.     module->Value_Aim = Aim;
  55.     module->Value_Actual = Feedback;
  57.     /* Culculate P */    
  58.     if(module->Value_Aim > module->Property_AimMax)
  59.         module->Value_Aim = module->Property_AimMax;
  60.     else if(module->Value_Aim < -module->Property_AimMax)
  61.         module->Value_Aim = -module->Property_AimMax;
  63.     module->Value_err = module->Value_Aim - module->Value_Actual;
  65.     P = module->Value_err * module->Property_Kp;
  67.     /* Culculate I */
  68.     if(fabs(module->Value_err) < module->Property_integralErrMax)
  69.     {
  70.         module->Value_integral += module->Value_err * module->Property_Ki * module->Property_dt;
  72.         if(module->Value_integral > module->Property_integralMax)
  73.             module->Value_integral = module->Property_integralMax;
  74.         else if(module->Value_integral < -module->Property_integralMax)
  75.             module->Value_integral = -module->Property_integralMax;
  77.         I = module->Value_integral;
  78.     }
  80.     /* Culculate D */
  81.     D = module->Property_Kd * (module->Value_err - module->Value_err_last) / module->Property_dt;
  83.     /* Culculate Output */
  84.     module->Value_output = P + I + D;
  86.     if(module->Value_output > module->Property_OutputMax)
  87.         module->Value_output = module->Property_OutputMax;
  88.     else if(module->Value_output < -module->Property_OutputMax)
  89.         module->Value_output = -module->Property_OutputMax;
  90. }
  92. /************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

使用

  1. /*
  2.  * Copyright (c) 2020 - ~, HIT_HERO Team
  3.  *
  4.  * MAIN SOUCE FILE
  5.  * Used in RT-Thread Operate System
  6.  *
  7.  * Change Logs:
  8.  * Date           Author       Notes                Mail
  9.  * 2020-08-08     WangXi       first version        WangXi_chn@foxmail.com
  10.  * 2020-08-13     WangXi       second version        WangXi_chn@foxmail.com
  11.  *
  12.  * Note:
  13.  * Used for friction gear shot Rugby
  14.  *
  15.  */
  17. #include <rtthread.h>
  18. #include <rtdevice.h>
  19. #include <board.h>
  21. #include "Module_LED.h"
  22. #include "Module_Beep.h"
  23. #include "Module_Key.h"
  24. #include "Module_DjiC610620Group.h"
  26. /* Module param list ---------------------------------------------------------------------------------------- */
  27. MODULE_LED dev_led_state = 
  28.     {    
  29.         .pin                 = GET_PIN(H, 10),
  30.         .LED_TIME_CYCLE     = 4000,
  31.         .LED_TIME_OUTPUT     = 200
  32.     };
  34. MODULE_DjiC610620GROUP dev_DjiC610620group = 
  35. {
  36.     .Property_CanDevName = "can1",
  37.     .Value_module_DjiC610620[DjiC610620ID_1] = 
  38.     {
  39.         .Enable = 1,
  40.         .Mode    = DjiC610620MODE_SPEED,
  41.         .ENCODER = DjiC610620SELF,
  42.         .PID_Speed = 
  43.         {
  44.             .Property_Kp = 6,    .Property_Ki = 2,    .Property_Kd = 0,    .Property_dt = 0.005,
  45.             .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
  46.             .Property_integralErrMax = 500,
  47.         }
  48.     },
  49.     .Value_module_DjiC610620[DjiC610620ID_2] = 
  50.     {
  51.         .Enable = 1,
  52.         .Mode    = DjiC610620MODE_SPEED,
  53.         .ENCODER = DjiC610620SELF,
  54.         .PID_Speed = 
  55.         {
  56.             .Property_Kp = 6,    .Property_Ki = 2,    .Property_Kd = 0,    .Property_dt = 0.005,
  57.             .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
  58.             .Property_integralErrMax = 500,
  59.         }
  60.     },
  61.     .Value_module_DjiC610620[DjiC610620ID_3] = 
  62.     {
  63.         .Enable = 1,
  64.         .Mode    = DjiC610620MODE_ANGLE,
  65.         .ENCODER = ROTARYENCODER,
  66.         .Encoder_DevName = "pulse1",
  67.         .PID_Speed = 
  68.         {
  69.             .Property_Kp = 6,    .Property_Ki = 1,    .Property_Kd = 0,    .Property_dt = 0.005,
  70.             .Property_integralMax = 2000,    .Property_AimMax = 9000,    .Property_OutputMax =16300,
  71.             .Property_integralErrMax = 1000,
  72.         },
  73.         .PID_Angle = 
  74.         {
  75.             .Property_Kp = 3,    .Property_Ki = 0.5,    .Property_Kd = 0,    .Property_dt = 0.005,
  76.             .Property_integralMax = 2000,    .Property_AimMax = 60000,    .Property_OutputMax =1000,
  77.             .Property_integralErrMax = 10000,
  78.         },
  79.     },
  80. };
  82. /* thread entry list ---------------------------------------------------------------------------------------- */
  83. static void led_shine_entry(void *parameter)
  84. {
  85.     while (1)
  86.     {
  87.         rt_thread_mdelay(1);        
  88.         dev_led_state.Handle(&dev_led_state);        
  89.     }
  90. }
  92. static void motor_entry(void *parameter)
  93. {
  94.     while (1)
  95.     {    
  96.         /* Communicate with motor group */        
  97.         dev_DjiC610620group.Method_Feed(&dev_DjiC610620group);
  98.         rt_thread_mdelay(1);
  99.         //dev_DjiC610620group.Method_Send(&dev_DjiC610620group);
  100.     }
  101. }
  103. int main(void)
  104. {
  105.     /* Config module ---------------------------------------------------------------------------------------- */
  106.     Module_Led_Config(&dev_led_state);    
  107.     dev_led_state.Set(&dev_led_state,9);
  109.     Module_DjiC610620Group_Config(&dev_DjiC610620group);
  111.     /* Enable task thread ---------------------------------------------------------------------------------------- */
  112.     /* system running shine led thread */
  113.     rt_thread_t led_thread = rt_thread_create("ledshine", led_shine_entry, RT_NULL,
  114.                                   512, RT_THREAD_PRIORITY_MAX - 3, 20);
  115.     if (led_thread != RT_NULL){
  116.         rt_thread_startup(led_thread);
  117.     }    
  119.     /* motor control thread */
  120.     rt_thread_t motor_thread = rt_thread_create("motor", motor_entry, RT_NULL,
  121.                                   1024, RT_THREAD_PRIORITY_MAX - 6, 20);
  122.     if (motor_thread != RT_NULL){
  123.         rt_thread_startup(motor_thread);
  124.     }
  126. }
  130. /************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

测试结果

  • 可以大大减少驱动压力，仅需关注调节PID参数和控制方式
  • 灵活性强，添加一个电机仅需为结构体成员赋值即可

  • PID控制已验证，逻辑正确

    注意事项

  • 读取电机总转角，实现电机位置控制时，在高速运动情况下，会出现距离（圈数）误差

  • 原因是电机返回的是电机的转子的位置（0~360°）是否转过了一圈需要程序确定
  • 当电机转速较快，（减速齿轮约为19：1）转子位置采样会出现丢圈的可能
  • 建议仅使用3508或2006于速度控制，或较低速的位置控制，或对位置不严格要求的场合
  • 如果需要用3508或2006电机实现精准的位置控制，建议配合编码器使用