21ic电子技术开发论坛 › 单片机与嵌入式系统 › ST MCU › 直流有刷电机控制（电流环，速度环，位置环） ...
返回列表 发新帖我要提问

[STM32F4] 直流有刷电机控制（电流环，速度环，位置环）

[复制链接]
4538|0
 楼主| zero949079783 发表于 2024-4-6 15:45 | 显示全部楼层 |阅读模式
DA, TE, MOTO, Motor, 电机, 直流
本帖最后由 zero949079783 于 2024-4-14 13:40 编辑

上位机使用：正点原子ATK_PID硬件：正点原子电机开发板

直流有刷电机 (电流环):链接：https://pan.baidu.com/s/1cpPnogMKNhTAeAOBJlFAxA?pwd=n8ut
提取码：n8ut
直流有刷电机 (速度环):链接：https://pan.baidu.com/s/1bd6SHPedWeDGvbiKfD88Vg?pwd=0vvt
提取码：0vvt
直流有刷电机 (位置环):  
链接：https://pan.baidu.com/s/1Cy3HBHYqmDLjFAGRBZYWVQ?pwd=20dn
提取码：20dn



电流环：
复制
  1. #include "main.h"
    

  2. #include "math.h"
    


  3. 


  4. 

  5. /*************************************    第二部分    电压电流温度采集    **********************************************/
    

  6. uint16_t g_adc_val[ADC_CH_NUM];                         /*ADC平均值存放数组*/
    


  7. 


  8. 


  9. 

  10. PID_TypeDef  g_current_pid;             /* 电流环PID参数结构体 */
    

  11. /*
    

  12.     Rt = Rp *exp(B*(1/T1-1/T2))
    


  13. 

  14.     Rt 是热敏电阻在T1温度下的阻值；
    

  15.     Rp是热敏电阻在T2常温下的标称阻值；
    

  16.     exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
    

  17.     B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
    

  18.     这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
    

  19.     T1就是所求的温度
    

  20. */
    


  21. 

  22. const float Rp = 10000.0f;          /* 10K */
    

  23. const float T2 = (273.15f + 25.0f); /* T2 */
    

  24. const float Bx = 3380.0f;           /* B */
    

  25. const float Ka = 273.15f;
    


  26. 

  27. Motor_TypeDef g_motor_data;  /*电机参数变量*/
    

  28. ENCODE_TypeDef g_encode;     /*编码器参数变量*/
    


  29. 

  30. /**
    

  31.  * [url=home.php?mod=space&uid=247401]@brief[/url]       计算温度值
    

  32.  * @param       para: 温度采集对应ADC通道的值（已滤波）
    

  33.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        计算温度分为两步：
    

  34.                 1.根据ADC采集到的值计算当前对应的Rt
    

  35.                 2.根据Rt计算对应的温度值
    

  36.  * @retval      温度值
    

  37.  */
    

  38. float get_temp(uint16_t para)
    

  39. {
    

  40.     float Rt;
    

  41.     float temp;
    

  42.     
    

  43.     /* 
    

  44.         第一步：
    

  45.         Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
    

  46.         由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; 
    

  47.     */
    

  48.     
    

  49.     Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;       /* 根据当前ADC值计算出Rt的值 */
    


  50. 

  51.     /* 
    

  52.         第二步：
    

  53.         根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2)) 
    

  54.     */
    

  55.     
    

  56.     temp = Rt / Rp;                 /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
    

  57.     temp = log(temp);               /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
    

  58.     temp /= Bx;                     /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
    

  59.     temp += (1.0f / T2);            /* 解出1/T1 ，即temp = 1/T1 */
    

  60.     temp = 1.0f / (temp);           /* 解出T1 ，即temp = T1 */
    

  61.     temp -= Ka;                     /* 计算T1对应的摄氏度 */
    

  62.     return temp;                    /* 返回温度值 */
    

  63. }
    


  64. 

  65. /**
    

  66.  * [url=home.php?mod=space&uid=247401]@brief[/url]       计算ADC的平均值（滤波）
    

  67.  * @param       * p ：存放ADC值的指针地址
    

  68.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        此函数对电压、温度、电流对应的ADC值进行滤波，
    

  69.  *              p[0]-p[2]对应的分别是电压、温度和电流
    

  70.  * @retval      无
    

  71.  */
    

  72. void calc_adc_val(uint16_t * adcdata)
    

  73. {
    

  74.           uint32_t temp[3] = {0,0,0};
    

  75.                 uint16_t i;
    

  76.     for(i=0;i<ADC_COLL;i++)         /* 循环ADC_COLL次取值，累加 */
    

  77.     {
    

  78.         temp[0] += adcdata[0+i*ADC_CH_NUM];
    

  79.         temp[1] += adcdata[1+i*ADC_CH_NUM];
    

  80.         temp[2] += adcdata[2+i*ADC_CH_NUM];
    

  81.     }
    

  82.     temp[0] /= ADC_COLL;            /* 取平均值 */
    

  83.     temp[1] /= ADC_COLL;
    

  84.     temp[2] /= ADC_COLL;
    

  85.                 
    

  86.     g_adc_val[0] = temp[0];                 /* 存入电压ADC通道平均值 */
    

  87.     g_adc_val[1] = temp[1];                 /* 存入温度ADC通道平均值 */
    

  88.     g_adc_val[2] = temp[2];                 /* 存入电流ADC通道平均值 */                
    

  89. }
    

  90. uint8_t current_flag =0;
    

  91. void get_Motor_value(uint16_t * adcdata)
    

  92. {
    

  93.         #define CURRENT_NUM 4
    

  94.         #define INIT_CURRENT_NUM 5
    

  95.         float temp_current=0.0;
    

  96.         static float adc_current = 0;
    

  97.         static float init_adc_current = 0;
    

  98.         static uint8_t current_count1 =0,current_count2 =0;
    

  99.         
    

  100.         current_count1++;
    

  101.         calc_adc_val(adcdata);
    

  102.         adc_current += g_adc_val[2];        /* 取出电流通道对应的ADC值进行累计 */
    

  103.         
    

  104.         if(current_count1 >= CURRENT_NUM)/* 累计15次 */
    

  105.         {
    

  106.                         adc_current = (float)(adc_current / current_count1);                 /* 取平均值 */
    

  107.                         if(current_count2 <= INIT_CURRENT_NUM)
    

  108.                         {
    

  109.                                 current_count2++;
    

  110.                                 init_adc_current += adc_current;                         /* 对平均值累计求和 */
    

  111.                                 if(current_count2 == INIT_CURRENT_NUM)
    

  112.                                 {
    

  113.                                         init_adc_current = (float)(init_adc_current/current_count2);        /* 存储初始ADC值 */
    

  114.                                         current_count2 = INIT_CURRENT_NUM+1;
    

  115.                                         current_flag =1;
    

  116.                                         
    

  117.                                 }
    

  118.                         }
    

  119.                         if(current_count2 >= (INIT_CURRENT_NUM+1))
    

  120.                         {
    

  121.                                         
    

  122.                                         temp_current= ((adc_current - init_adc_current) *ADC2CURT);
    

  123.                                         // g_motor_data.current = temp_current;
    

  124.                                         FirstOrderRC_LPF(g_motor_data.current,temp_current,0.40);
    

  125.                                         //g_motor_data.current = (float)((g_motor_data.current * (float)0.60) + ((float)0.40 * temp_current));  /* 一阶低通滤波 */
    

  126.                                  
    

  127.                                         if(g_motor_data.current <=20)
    

  128.                                  {
    

  129.                                                 g_motor_data.current=0.0;
    

  130.                                  }
    

  131.                         }
    

  132.                 adc_current=0;
    

  133.                 current_count1=0;
    

  134.         }
    

  135. }
    


  136. 


  137. 


  138. 


  139. 


  140. 

  141. /*************************************    第三部分    编码器测速    ****************************************************/
    

  142. void speed_computer(int32_t encode_now, uint8_t ms)
    

  143. {
    

  144.     uint8_t i = 0, j = 0;
    

  145.     float temp = 0.0;
    

  146.     static uint8_t sp_count = 0, k = 0;
    

  147.     static float speed_arr[10] = {0.0};                     /* 存储速度进行滤波运算 */
    


  148. 

  149.     if (sp_count == ms)                                     /* 计算一次速度 */
    

  150.     {
    

  151.         /* 计算电机转速 
    

  152.            第一步 ：计算ms毫秒内计数变化量
    

  153.            第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
    

  154.            第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
    

  155.            第四步 ：除以减速比即可得出电机转速
    

  156.         */
    

  157.         g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
    

  158.         g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */
    

  159.         
    

  160.         speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO );    /* 保存电机转速 */
    

  161.         
    

  162.         g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */
    


  163. 

  164.         /* 累计10次速度值，后续进行滤波*/
    

  165.         if (k == 10)
    

  166.         {
    

  167.             for (i = 10; i >= 1; i--)                       /* 冒泡排序*/
    

  168.             {
    

  169.                 for (j = 0; j < (i - 1); j++) 
    

  170.                 {
    

  171.                     if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
    

  172.                     { 
    

  173.                         temp = speed_arr[j];                /* 数值换位 */
    

  174.                         speed_arr[j] = speed_arr[j + 1];
    

  175.                         speed_arr[j + 1] = temp;
    

  176.                     }
    

  177.                 }
    

  178.             }
    

  179.             
    

  180.             temp = 0.0;
    

  181.             
    

  182.             for (i = 2; i < 8; i++)                         /* 去除两边高低数据 */
    

  183.             {
    

  184.                 temp += speed_arr[i];                       /* 将中间数值累加 */
    

  185.             }
    

  186.             
    

  187.             temp = (float)(temp / 6);                       /*求速度平均值*/
    

  188.             
    

  189.             /* 一阶低通滤波
    

  190.              * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
    

  191.              * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
    

  192.              * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
    

  193.              */
    

  194.             g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
    

  195.             k = 0;
    

  196.         }
    

  197.         sp_count = 0;
    

  198.     }
    

  199.     sp_count ++;
    

  200. }
    


  201. 

  202. uint8_t g_run_flag = 0;
    


  203. 

  204. int32_t motor_pwm = 0;
    

  205. atk_debug_data_rev get_atk_data;
    


  206. 


  207. 

  208. void DCMotor_App(void)
    

  209. {
    

  210.         static uint8_t key=0;
    

  211.         get_atkdebug(&get_atk_data);
    

  212.         key=key_scan(0);
    

  213.         if(key == KEY0_PRES)
    

  214.         {        
    

  215.                 TurnTogLed(LED1);
    

  216.                 g_current_pid.SetPoint +=50;
    

  217.                 g_run_flag = 1;
    

  218.                 if (g_current_pid.SetPoint == 0) 
    

  219.                 {
    

  220.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  221.                                 g_motor_data.motor_pwm = 0;
    

  222.                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);                                  /* 设置电机转速 */
    

  223.                 } 
    

  224.                 else
    

  225.                 {
    

  226.                         DC_Motor_Start(MOTOR_1);
    

  227.                         if(g_current_pid.SetPoint  >= 250)
    

  228.                         {
    

  229.                                 g_current_pid.SetPoint  =250;
    

  230.                         }
    

  231.                         atk_debug_send_motorstate(RUN_STATE);           /* 上传电机状态（运行） */
    

  232.                         //Motor_PWM_Set(g_current_pid.SetPoint,MOTOR_1);
    

  233.                 }
    

  234.         }
    

  235.         else if(key == KEY1_PRES)        {        
    

  236.                 TurnTogLed(LED1);
    


  237. 

  238.                 if (g_current_pid.SetPoint == 0) 
    

  239.                 {
    

  240.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  241.                                 g_motor_data.motor_pwm = 0;
    

  242.                                 DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */                                 
    

  243.                 } 
    

  244.                 else
    

  245.                 {
    


  246. 


  247. 

  248.                 }
    

  249.         }
    

  250.         else if(key == KEY2_PRES)
    

  251.         {
    


  252. 

  253.                 DC_Motor_Stop(MOTOR_1);
    

  254.                 g_run_flag = 0;                                 /* 标记电机停止 */
    

  255.                 g_motor_data.motor_pwm = 0;
    

  256.                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);          /* 设置电机转向、速度 */
    

  257.                 pid_init();
    

  258.                 atk_debug_send_motorstate(BREAKED_STATE);           /* 上传电机状态（刹车） */
    

  259.                 atk_debug_send_initdata(TYPE_PID1, (float *)(&g_current_pid.SetPoint), KP, KI, KD);           /* 同步数据到上位机 */
    

  260.         }
    

  261. }
    


  262. 

  263. void DCMotor_Value(void)
    

  264. {
    

  265.         static uint16_t g_time =0;
    

  266.         int32_t motor_pwm_temp = 0;                                        
    

  267.         
    

  268.         if(current_flag == 0 )
    

  269.                 return;
    

  270.                                 
    

  271.                                 
    

  272.                                 /* 获取编码器值，用于计算速度 */
    

  273.                                 get_EncodeValue(5);        /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
    

  274.                                 
    

  275.                                 if(g_time  % 499 ==0)
    

  276.                                 {                                                                                 
    

  277.                                         
    

  278.                                         g_motor_data.volatage  = g_adc_val[0]*ADC2VBUS;                
    

  279.                                         atk_debug_send_valtage(g_motor_data.volatage );
    

  280.                                         __NOP();__NOP();__NOP();
    

  281.                                         g_motor_data.board_temp = get_temp(g_adc_val[1]);
    

  282.                                         __NOP();__NOP();__NOP();
    

  283.                                         atk_debug_send_temp(0,get_temp(g_adc_val[1]));
    

  284.                                         __NOP();__NOP();__NOP();
    

  285.                                         //g_motor_data.current          = (g_adc_val[2]*ADC2CURT)*0.001;
    

  286.                                         atk_debug_send_current(g_motor_data.current*0.001,0,0);                        
    

  287.                                         __NOP();__NOP();__NOP();                
    

  288.                                         atk_debug_send_speed(g_motor_data.speed);
    

  289.                                         __NOP();__NOP();__NOP();                
    

  290. //                                        printf("\r\n******************************\r\n");
    

  291. //                                        printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
    

  292. //                                        printf("Valtage:%.2fV \r\n",    g_motor_data.volatage);                   /* 打印电压值 */
    

  293. //                                        printf("Temp:%.2fC \r\n",                    g_motor_data.board_temp   );                  /* 打印温度值 */
    

  294. //                                        printf("Current:%.2fmA \r\n",   g_motor_data.current); /* 打印电流值 */
    

  295. //                                        printf("电机速度：%.1f RPM\r\n\r\n",   g_motor_data.speed);                
    

  296. //                                        printf("\r\n******************************\r\n");
    

  297.                                 }
    

  298.                                 else if(g_time % SMAPLSE_PID_SPEED == 0)                /* 电机速度 50ms进行一次pid计算 */
    

  299.                                 {
    

  300.                                         if(g_run_flag)
    

  301.                                         {
    

  302.                                         
    

  303.                                                  /* PID计算，输出比较值（占空比） */
    

  304.                                                 motor_pwm_temp = increment_pid_ctrl (&g_current_pid,g_motor_data.current);
    

  305.                                                 g_motor_data.motor_pwm = (int32_t)((g_motor_data.motor_pwm * 0.5) + (motor_pwm_temp * 0.5));
    

  306.                                                 if(g_motor_data.motor_pwm >= 8200)
    

  307.                                                 {
    

  308.                                                         g_motor_data.motor_pwm = 8200;
    

  309.                                                 }
    

  310.                                                 else if (g_motor_data.motor_pwm <= 0)
    

  311.                                                 {
    

  312.                                                         g_motor_data.motor_pwm = 0;
    

  313.                                                 }
    

  314.                                         
    

  315.                                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
    

  316.                                         }
    

  317.                                                 atk_debug_send_wave_data( 1 ,g_motor_data.current);                           /* 选择通道1发送实际电流（波形显示）*/
    

  318.                                                 atk_debug_send_wave_data( 2 ,g_current_pid.SetPoint);                         /* 选择通道2，发送目标电流（波形显示）*/
    

  319.                                                 atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400);          /* 选择通道3，发送占空比（波形显示）*/
    

  320.                                 }
    


  321. 

  322.                                 g_time ++;
    

  323.         
    

  324. }
    


  325. 

  326. void get_EncodeValue(uint8_t  ms)
    

  327. {
    

  328.         int encode_now = gtim_get_encode();
    

  329.         speed_computer(encode_now, ms);
    

  330. }
    


  331. 


  332. 

  333. PID_TypeDef  g_speed_pid;           /* 速度环PID参数结构体 */
    


  334. 

  335. /**
    

  336.  * [url=home.php?mod=space&uid=247401]@brief[/url]       pid初始化
    

  337.  * @param       无
    

  338.  * @retval      无
    

  339.  */
    

  340. void pid_init(void)
    

  341. {
    

  342.         
    


  343. 


  344. 

  345.                 memset(&g_motor_data,0,sizeof(g_motor_data));
    

  346.                 memset(&g_current_pid,0,sizeof(g_current_pid));
    

  347.         
    

  348.     g_current_pid.SetPoint = 0;        /* 设定目标值 */
    

  349.     g_current_pid.ActualValue = 0.0;    /* 期望输出值 */
    

  350.     g_current_pid.SumError = 0.0;       /* 积分值 */
    

  351.     g_current_pid.Error = 0.0;          /* Error[1] */
    

  352.     g_current_pid.LastError = 0.0;      /* Error[-1] */
    

  353.     g_current_pid.PrevError = 0.0;      /* Error[-2] */
    

  354.     g_current_pid.Proportion = KP;      /* 比例常数 Proportional Const */
    

  355.     g_current_pid.Integral = KI;        /* 积分常数 Integral Const */
    

  356.     g_current_pid.Derivative = KD;      /* 微分常数 Derivative Const */ 
    

  357. }
    


  358. 

  359. void DCMotorsample_Init(void)
    

  360. {
    

  361.                 pid_init();
    

  362.                 ADC1CbReg(get_Motor_value);
    

  363. }


速度环：
复制
  1. #include "main.h"
    

  2. #include "math.h"
    


  3. 


  4. 

  5. /*************************************    第二部分    电压电流温度采集    **********************************************/
    

  6. uint16_t g_adc_val[ADC_CH_NUM];                         /*ADC平均值存放数组*/
    


  7. 


  8. 

  9. PID_TypeDef  g_speed_pid;     /*速度环PID参数结构体*/
    


  10. 

  11. /*
    

  12.     Rt = Rp *exp(B*(1/T1-1/T2))
    


  13. 

  14.     Rt 是热敏电阻在T1温度下的阻值；
    

  15.     Rp是热敏电阻在T2常温下的标称阻值；
    

  16.     exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
    

  17.     B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
    

  18.     这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
    

  19.     T1就是所求的温度
    

  20. */
    


  21. 

  22. const float Rp = 10000.0f;          /* 10K */
    

  23. const float T2 = (273.15f + 25.0f); /* T2 */
    

  24. const float Bx = 3380.0f;           /* B */
    

  25. const float Ka = 273.15f;
    


  26. 

  27. Motor_TypeDef g_motor_data;  /*电机参数变量*/
    

  28. ENCODE_TypeDef g_encode;     /*编码器参数变量*/
    


  29. 

  30. /**
    

  31.  * [url=home.php?mod=space&uid=247401]@brief[/url]       计算温度值
    

  32.  * @param       para: 温度采集对应ADC通道的值（已滤波）
    

  33.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        计算温度分为两步：
    

  34.                 1.根据ADC采集到的值计算当前对应的Rt
    

  35.                 2.根据Rt计算对应的温度值
    

  36.  * @retval      温度值
    

  37.  */
    

  38. float get_temp(uint16_t para)
    

  39. {
    

  40.     float Rt;
    

  41.     float temp;
    

  42.     
    

  43.     /* 
    

  44.         第一步：
    

  45.         Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
    

  46.         由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; 
    

  47.     */
    

  48.     
    

  49.     Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;       /* 根据当前ADC值计算出Rt的值 */
    


  50. 

  51.     /* 
    

  52.         第二步：
    

  53.         根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2)) 
    

  54.     */
    

  55.     
    

  56.     temp = Rt / Rp;                 /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
    

  57.     temp = log(temp);               /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
    

  58.     temp /= Bx;                     /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
    

  59.     temp += (1.0f / T2);            /* 解出1/T1 ，即temp = 1/T1 */
    

  60.     temp = 1.0f / (temp);           /* 解出T1 ，即temp = T1 */
    

  61.     temp -= Ka;                     /* 计算T1对应的摄氏度 */
    

  62.     return temp;                    /* 返回温度值 */
    

  63. }
    


  64. 

  65. /**
    

  66.  * [url=home.php?mod=space&uid=247401]@brief[/url]       计算ADC的平均值（滤波）
    

  67.  * @param       * p ：存放ADC值的指针地址
    

  68.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        此函数对电压、温度、电流对应的ADC值进行滤波，
    

  69.  *              p[0]-p[2]对应的分别是电压、温度和电流
    

  70.  * @retval      无
    

  71.  */
    

  72. void calc_adc_val(uint16_t * adcdata)
    

  73. {
    

  74.           uint32_t temp[3] = {0,0,0};
    

  75.                 uint16_t i;
    

  76.     for(i=0;i<ADC_COLL;i++)         /* 循环ADC_COLL次取值，累加 */
    

  77.     {
    

  78.         temp[0] += adcdata[0+i*ADC_CH_NUM];
    

  79.         temp[1] += adcdata[1+i*ADC_CH_NUM];
    

  80.         temp[2] += adcdata[2+i*ADC_CH_NUM];
    

  81.     }
    

  82.     temp[0] /= ADC_COLL;            /* 取平均值 */
    

  83.     temp[1] /= ADC_COLL;
    

  84.     temp[2] /= ADC_COLL;
    

  85.                 
    

  86.     g_adc_val[0] = temp[0];                 /* 存入电压ADC通道平均值 */
    

  87.     g_adc_val[1] = temp[1];                 /* 存入温度ADC通道平均值 */
    

  88.     g_adc_val[2] = temp[2];                 /* 存入电流ADC通道平均值 */                
    

  89. }
    


  90. 

  91. void get_Motor_value(uint16_t * adcdata)
    

  92. {
    

  93.         calc_adc_val(adcdata);
    

  94.         
    

  95. }
    


  96. 


  97. 


  98. 


  99. 


  100. 

  101. /*************************************    第三部分    编码器测速    ****************************************************/
    

  102. void speed_computer(int32_t encode_now, uint8_t ms)
    

  103. {
    

  104.     uint8_t i = 0, j = 0;
    

  105.     float temp = 0.0;
    

  106.     static uint8_t sp_count = 0, k = 0;
    

  107.     static float speed_arr[10] = {0.0};                     /* 存储速度进行滤波运算 */
    


  108. 

  109.     if (sp_count == ms)                                     /* 计算一次速度 */
    

  110.     {
    

  111.         /* 计算电机转速 
    

  112.            第一步 ：计算ms毫秒内计数变化量
    

  113.            第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
    

  114.            第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
    

  115.            第四步 ：除以减速比即可得出电机转速
    

  116.         */
    

  117.         g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
    

  118.         g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */
    

  119.         
    

  120.         speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO );    /* 保存电机转速 */
    

  121.         
    

  122.         g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */
    


  123. 

  124.         /* 累计10次速度值，后续进行滤波*/
    

  125.         if (k == 10)
    

  126.         {
    

  127.             for (i = 10; i >= 1; i--)                       /* 冒泡排序*/
    

  128.             {
    

  129.                 for (j = 0; j < (i - 1); j++) 
    

  130.                 {
    

  131.                     if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
    

  132.                     { 
    

  133.                         temp = speed_arr[j];                /* 数值换位 */
    

  134.                         speed_arr[j] = speed_arr[j + 1];
    

  135.                         speed_arr[j + 1] = temp;
    

  136.                     }
    

  137.                 }
    

  138.             }
    

  139.             
    

  140.             temp = 0.0;
    

  141.             
    

  142.             for (i = 2; i < 8; i++)                         /* 去除两边高低数据 */
    

  143.             {
    

  144.                 temp += speed_arr[i];                       /* 将中间数值累加 */
    

  145.             }
    

  146.             
    

  147.             temp = (float)(temp / 6);                       /*求速度平均值*/
    

  148.             
    

  149.             /* 一阶低通滤波
    

  150.              * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
    

  151.              * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
    

  152.              * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
    

  153.              */
    

  154.             g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
    

  155.             k = 0;
    

  156.         }
    

  157.         sp_count = 0;
    

  158.     }
    

  159.     sp_count ++;
    

  160. }
    


  161. 

  162. uint8_t g_run_flag = 0;
    


  163. 

  164. int32_t motor_pwm = 0;
    


  165. 

  166. void DCMotor_App(void)
    

  167. {
    

  168.         static uint8_t key=0;
    

  169.         key=key_scan(0);
    

  170.         if(key == KEY0_PRES)
    

  171.         {        
    

  172.                 TurnTogLed(LED1);
    

  173.                 g_speed_pid.SetPoint +=30;
    

  174.                 g_run_flag = 1;
    

  175.                 if (g_speed_pid.SetPoint == 0) 
    

  176.                 {
    

  177.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  178.                                 g_motor_data.motor_pwm = 0;
    

  179.                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);                                  /* 设置电机转速 */
    

  180.                 } 
    

  181.                 else
    

  182.                 {
    

  183.                         DC_Motor_Start(MOTOR_1);
    

  184.                         if(g_speed_pid.SetPoint  >= 300)
    

  185.                         {
    

  186.                                 g_speed_pid.SetPoint  =300;
    

  187.                         }
    

  188.                         atk_debug_send_motorstate(RUN_STATE);           /* 上传电机状态（运行） */
    

  189.                         DC_Motor_Speed(g_speed_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
    


  190. 

  191.                 }
    

  192.         }
    

  193.         else if(key == KEY1_PRES)        {        
    

  194.                 TurnTogLed(LED1);
    

  195.                 g_speed_pid.SetPoint -= 30;
    


  196. 

  197.                 g_run_flag = 1;
    

  198.                 if (g_speed_pid.SetPoint == 0) 
    

  199.                 {
    

  200.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  201.                                 g_motor_data.motor_pwm = 0;
    

  202.                                 DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */                                 
    

  203.                 } 
    

  204.                 else
    

  205.                 {
    

  206.                         DC_Motor_Start(MOTOR_1);
    

  207.                         if(g_speed_pid.SetPoint  <= -300)
    

  208.                         {
    

  209.                                 g_speed_pid.SetPoint  =-300;
    

  210.                         }
    

  211.                         DC_Motor_Speed(g_speed_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
    


  212. 

  213.                         atk_debug_send_motorstate(RUN_STATE);           /* 上传电机状态（运行） */
    

  214.                 }
    

  215.         }
    

  216.         else if(key == KEY2_PRES)
    

  217.         {
    

  218.                 DC_Motor_Stop(MOTOR_1);
    

  219.                 g_run_flag = 0;                                 /* 标记电机停止 */
    

  220.                 g_motor_data.motor_pwm = 0;
    

  221.                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);          /* 设置电机转向、速度 */
    

  222.                 g_speed_pid.SetPoint = 0;
    

  223.                 pid_init();
    

  224.                 atk_debug_send_motorstate(BREAKED_STATE);           /* 上传电机状态（刹车） */
    

  225.                 atk_debug_send_initdata(TYPE_PID1, (float *)(&g_speed_pid.SetPoint), KP, KI, KD);           /* 同步数据到上位机 */
    

  226.         }
    

  227. }
    


  228. 

  229. void DCMotor_Value(void)
    

  230. {
    

  231.         static uint16_t init_adc_val = 0;
    

  232.         static uint16_t g_time =0;
    

  233.         static uint8_t num = 0;
    

  234.         if(num < 2)
    

  235.         {
    

  236.                 num ++;
    

  237.                 atk_debug_send_motorcode(DC_MOTOR);         /* 上传电机类型（直流有刷电机） */
    

  238.     atk_debug_send_motorstate(IDLE_STATE);      /* 上传电机状态（空闲） */
    

  239.                 init_adc_val = g_adc_val[2];
    

  240.                 init_adc_val += g_adc_val[2];       /* 现在的值和上一次存储的值相加 */
    

  241.     init_adc_val /= 2;  
    

  242.         }else 
    

  243.         {                                                        
    

  244.                                 
    

  245.                                 
    

  246.                                 
    

  247.                                 /* 获取编码器值，用于计算速度 */
    

  248.                                 get_EncodeValue(5);        /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
    

  249.                                 
    

  250.                                 if(g_time  % 1000 ==0)
    

  251.                                 {                                                                                 
    

  252.                                         
    

  253.                                         __NOP();__NOP();__NOP();
    

  254.                                         g_motor_data.board_temp = get_temp(g_adc_val[1]);
    

  255.                                             
    

  256. //                                                        
    

  257.                                         printf("\r\n******************************\r\n");
    

  258.                                         printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
    

  259.                                         printf("Valtage:%.2fV \r\n",    g_motor_data.volatage);                   /* 打印电压值 */
    

  260.                                         printf("Temp:%.2fC \r\n",                    g_motor_data.board_temp   );                  /* 打印温度值 */
    

  261.                                         printf("Current:%.2fmA \r\n",   g_motor_data.current); /* 打印电流值 */
    

  262.                                         printf("电机速度：%.1f RPM\r\n\r\n",   g_motor_data.speed);                
    

  263.                                         printf("\r\n******************************\r\n");
    

  264.                                 }
    

  265.                                 else if(g_time %300 == 0)
    

  266.                                 {
    

  267.                                         atk_debug_send_temp(0,get_temp(g_adc_val[1]));
    

  268.                                 }
    

  269.                                 else if(g_time %200 == 0)
    

  270.                                 {
    

  271.                                         g_motor_data.volatage  = g_adc_val[0]*ADC2VBUS;                
    

  272.                                         atk_debug_send_valtage(g_motor_data.volatage );
    

  273.                                 }
    

  274.                                 else if(g_time % SMAPLSE_PID_SPEED == 0)                /* 电机速度 50ms进行一次pid计算 */
    

  275.                                 {
    

  276.                                         atk_debug_send_speed(g_motor_data.speed); 
    

  277.                                         if(g_run_flag)
    

  278.                                         {
    

  279.                                         
    

  280.                                                  /* PID计算，输出比较值（占空比） */
    

  281.                                                 g_motor_data.motor_pwm = increment_pid_ctrl (&g_speed_pid,g_motor_data.speed);
    

  282.                                                 
    

  283.                                                 if(g_motor_data.motor_pwm >= 8200)
    

  284.                                                 {
    

  285.                                                         g_motor_data.motor_pwm = 8200;
    

  286.                                                 }
    

  287.                                                 else if (g_motor_data.motor_pwm <= -8200)
    

  288.                                                 {
    

  289.                                                         g_motor_data.motor_pwm = -8200;
    

  290.                                                 }
    

  291.                                         
    

  292.                                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */
    

  293.                                         }
    

  294.                                                 atk_debug_send_wave_data( 1 ,g_motor_data.speed);                           /* 选择通道1，发送实际速度（波形显示）*/
    

  295.                                                 atk_debug_send_wave_data( 2 ,g_speed_pid.SetPoint);                         /* 选择通道2，发送目标速度（波形显示）*/
    

  296.                                                 atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400);          /* 选择通道3，发送占空比（波形显示）*/
    

  297.                                 }
    

  298.                                 else if(g_time % 20 == 0)
    

  299.                                 {
    

  300.                                         g_motor_data.current          = (abs(g_adc_val[2]-init_adc_val)*ADC2CURT)*0.001;
    

  301.                                         atk_debug_send_current(g_motor_data.current,0,0);                                        
    

  302.                                 }
    

  303.                                 g_time ++;
    

  304.         }
    

  305. }
    


  306. 

  307. void get_EncodeValue(uint8_t  ms)
    

  308. {
    

  309.         int encode_now = gtim_get_encode();
    

  310.         speed_computer(encode_now, ms);
    

  311. }
    


  312. 


  313. 

  314. PID_TypeDef  g_speed_pid;           /* 速度环PID参数结构体 */
    


  315. 

  316. /**
    

  317.  * [url=home.php?mod=space&uid=247401]@brief[/url]       pid初始化
    

  318.  * @param       无
    

  319.  * @retval      无
    

  320.  */
    

  321. void pid_init(void)
    

  322. {
    

  323.         
    

  324.                 memset(&g_motor_data,0,sizeof(g_motor_data));
    

  325.                 memset(&g_speed_pid,0,sizeof(g_speed_pid));
    

  326.         
    

  327.                 /* 初始化速度环PID参数 */
    

  328.     g_speed_pid.SetPoint = 0;       /* 设定目标值 */
    

  329.     g_speed_pid.ActualValue = 0.0;  /* 期望输出值 */
    

  330.     g_speed_pid.SumError = 0.0;     /* 积分值 */
    

  331.     g_speed_pid.Error = 0.0;        /* Error[1] */
    

  332.     g_speed_pid.LastError = 0.0;    /* Error[-1] */
    

  333.     g_speed_pid.PrevError = 0.0;    /* Error[-2] */
    

  334.     g_speed_pid.Proportion = KP;    /* 比例常数 Proportional Const */
    

  335.     g_speed_pid.Integral = KI;      /* 积分常数 Integral Const */
    

  336.     g_speed_pid.Derivative = KD;    /* 微分常数 Derivative Const */ 
    

  337.         
    

  338.                 atk_debug_send_wave_data( 1 ,g_motor_data.speed);                           /* 选择通道1，发送实际速度（波形显示）*/
    

  339.                 atk_debug_send_wave_data( 2 ,g_speed_pid.SetPoint);                         /* 选择通道2，发送目标速度（波形显示）*/
    

  340.                 atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400);          /* 选择通道3，发送占空比（波形显示）*/
    

  341. }
    


  342. 

  343. void DCMotorsample_Init(void)
    

  344. {
    

  345.                 pid_init();
    

  346.                 ADC1CbReg(get_Motor_value);
    

  347. }

位置环:
复制
  1. #include "main.h"
    

  2. #include "math.h"
    


  3. 


  4. 

  5. /*************************************    第二部分    电压电流温度采集    **********************************************/
    

  6. uint16_t g_adc_val[ADC_CH_NUM];                         /*ADC平均值存放数组*/
    


  7. 


  8. 


  9. 

  10. PID_TypeDef  g_location_pid;             /*位置环PID参数结构体*/
    


  11. 

  12. /*
    

  13.     Rt = Rp *exp(B*(1/T1-1/T2))
    


  14. 

  15.     Rt 是热敏电阻在T1温度下的阻值；
    

  16.     Rp是热敏电阻在T2常温下的标称阻值；
    

  17.     exp是e的n次方，e是自然常数，就是自然对数的底数，近似等于 2.7182818；
    

  18.     B值是热敏电阻的重要参数，教程中用到的热敏电阻B值为3380；
    

  19.     这里T1和T2指的是开尔文温度，T2是常温25℃，即(273.15+25)K
    

  20.     T1就是所求的温度
    

  21. */
    


  22. 

  23. const float Rp = 10000.0f;          /* 10K */
    

  24. const float T2 = (273.15f + 25.0f); /* T2 */
    

  25. const float Bx = 3380.0f;           /* B */
    

  26. const float Ka = 273.15f;
    


  27. 

  28. Motor_TypeDef g_motor_data;  /*电机参数变量*/
    

  29. ENCODE_TypeDef g_encode;     /*编码器参数变量*/
    


  30. 

  31. /**
    

  32.  * [url=home.php?mod=space&uid=247401]@brief[/url]       计算温度值
    

  33.  * @param       para: 温度采集对应ADC通道的值（已滤波）
    

  34.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        计算温度分为两步：
    

  35.                 1.根据ADC采集到的值计算当前对应的Rt
    

  36.                 2.根据Rt计算对应的温度值
    

  37.  * @retval      温度值
    

  38.  */
    

  39. float get_temp(uint16_t para)
    

  40. {
    

  41.     float Rt;
    

  42.     float temp;
    

  43.     
    

  44.     /* 
    

  45.         第一步：
    

  46.         Rt = 3.3 * 4700 / VTEMP - 4700 ,其中VTEMP就是温度检测通道采集回来的电压值,VTEMP = ADC值* 3.3/4096
    

  47.         由此我们可以计算出当前Rt的值：Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f; 
    

  48.     */
    

  49.     
    

  50.     Rt = 3.3f * 4700.0f / (para * 3.3f / 4096.0f ) - 4700.0f;       /* 根据当前ADC值计算出Rt的值 */
    


  51. 

  52.     /* 
    

  53.         第二步：
    

  54.         根据当前Rt的值来计算对应温度值：Rt = Rp *exp(B*(1/T1-1/T2)) 
    

  55.     */
    

  56.     
    

  57.     temp = Rt / Rp;                 /* 解出exp(B*(1/T1-1/T2)) ，即temp = exp(B*(1/T1-1/T2)) */
    

  58.     temp = log(temp);               /* 解出B*(1/T1-1/T2) ，即temp = B*(1/T1-1/T2) */
    

  59.     temp /= Bx;                     /* 解出1/T1-1/T2 ，即temp = 1/T1-1/T2 */
    

  60.     temp += (1.0f / T2);            /* 解出1/T1 ，即temp = 1/T1 */
    

  61.     temp = 1.0f / (temp);           /* 解出T1 ，即temp = T1 */
    

  62.     temp -= Ka;                     /* 计算T1对应的摄氏度 */
    

  63.     return temp;                    /* 返回温度值 */
    

  64. }
    


  65. 

  66. /**
    

  67.  * @brief       计算ADC的平均值（滤波）
    

  68.  * @param       * p ：存放ADC值的指针地址
    

  69.  * [url=home.php?mod=space&uid=536309]@NOTE[/url]        此函数对电压、温度、电流对应的ADC值进行滤波，
    

  70.  *              p[0]-p[2]对应的分别是电压、温度和电流
    

  71.  * @retval      无
    

  72.  */
    

  73. void calc_adc_val(uint16_t * adcdata)
    

  74. {
    

  75.           uint32_t temp[3] = {0,0,0};
    

  76.                 uint16_t i;
    

  77.     for(i=0;i<ADC_COLL;i++)         /* 循环ADC_COLL次取值，累加 */
    

  78.     {
    

  79.         temp[0] += adcdata[0+i*ADC_CH_NUM];
    

  80.         temp[1] += adcdata[1+i*ADC_CH_NUM];
    

  81.         temp[2] += adcdata[2+i*ADC_CH_NUM];
    

  82.     }
    

  83.     temp[0] /= ADC_COLL;            /* 取平均值 */
    

  84.     temp[1] /= ADC_COLL;
    

  85.     temp[2] /= ADC_COLL;
    

  86.                 
    

  87.     g_adc_val[0] = temp[0];                 /* 存入电压ADC通道平均值 */
    

  88.     g_adc_val[1] = temp[1];                 /* 存入温度ADC通道平均值 */
    

  89.     g_adc_val[2] = temp[2];                 /* 存入电流ADC通道平均值 */                
    

  90. }
    


  91. 

  92. void get_Motor_value(uint16_t * adcdata)
    

  93. {
    

  94.         calc_adc_val(adcdata);
    

  95.         
    

  96. }
    


  97. 


  98. 


  99. 


  100. 


  101. 

  102. /*************************************    第三部分    编码器测速    ****************************************************/
    

  103. void speed_computer(int32_t encode_now, uint8_t ms)
    

  104. {
    

  105.     uint8_t i = 0, j = 0;
    

  106.     float temp = 0.0;
    

  107.     static uint8_t sp_count = 0, k = 0;
    

  108.     static float speed_arr[10] = {0.0};                     /* 存储速度进行滤波运算 */
    


  109. 

  110.     if (sp_count == ms)                                     /* 计算一次速度 */
    

  111.     {
    

  112.         /* 计算电机转速 
    

  113.            第一步 ：计算ms毫秒内计数变化量
    

  114.            第二步 ；计算1min内计数变化量：g_encode.speed * ((1000 / ms) * 60 ，
    

  115.            第三步 ：除以编码器旋转一圈的计数次数（倍频倍数 * 编码器分辨率）
    

  116.            第四步 ：除以减速比即可得出电机转速
    

  117.         */
    

  118.         g_encode.encode_now = encode_now;                                /* 取出编码器当前计数值 */
    

  119.         g_encode.speed = (g_encode.encode_now - g_encode.encode_old);    /* 计算编码器计数值的变化量 */
    

  120.         
    

  121.         speed_arr[k++] = (float)(g_encode.speed * ((1000 / ms) * 60.0) / REDUCTION_RATIO / ROTO_RATIO );    /* 保存电机转速 */
    

  122.         
    

  123.         g_encode.encode_old = g_encode.encode_now;          /* 保存当前编码器的值 */
    


  124. 

  125.         /* 累计10次速度值，后续进行滤波*/
    

  126.         if (k == 10)
    

  127.         {
    

  128.             for (i = 10; i >= 1; i--)                       /* 冒泡排序*/
    

  129.             {
    

  130.                 for (j = 0; j < (i - 1); j++) 
    

  131.                 {
    

  132.                     if (speed_arr[j] > speed_arr[j + 1])    /* 数值比较 */
    

  133.                     { 
    

  134.                         temp = speed_arr[j];                /* 数值换位 */
    

  135.                         speed_arr[j] = speed_arr[j + 1];
    

  136.                         speed_arr[j + 1] = temp;
    

  137.                     }
    

  138.                 }
    

  139.             }
    

  140.             
    

  141.             temp = 0.0;
    

  142.             
    

  143.             for (i = 2; i < 8; i++)                         /* 去除两边高低数据 */
    

  144.             {
    

  145.                 temp += speed_arr[i];                       /* 将中间数值累加 */
    

  146.             }
    

  147.             
    

  148.             temp = (float)(temp / 6);                       /*求速度平均值*/
    

  149.             
    

  150.             /* 一阶低通滤波
    

  151.              * 公式为：Y(n)= qX(n) + (1-q)Y(n-1)
    

  152.              * 其中X(n)为本次采样值；Y(n-1)为上次滤波输出值；Y(n)为本次滤波输出值，q为滤波系数
    

  153.              * q值越小则上一次输出对本次输出影响越大，整体曲线越平稳，但是对于速度变化的响应也会越慢
    

  154.              */
    

  155.             g_motor_data.speed = (float)( ((float)0.48 * temp) + (g_motor_data.speed * (float)0.52) );
    

  156.             k = 0;
    

  157.         }
    

  158.         sp_count = 0;
    

  159.     }
    

  160.     sp_count ++;
    

  161. }
    


  162. 

  163. uint8_t g_run_flag = 0;
    


  164. 

  165. int32_t motor_pwm = 0;
    


  166. 

  167. void DCMotor_App(void)
    

  168. {
    

  169.         static uint8_t key=0;
    

  170.         key=key_scan(0);
    

  171.         if(key == KEY0_PRES)
    

  172.         {        
    

  173.                 TurnTogLed(LED1);
    

  174.                 g_location_pid.SetPoint +=1320;        /* 正转一圈，电机旋转圈数 = 计数值变化量 / 44 / 30 */
    

  175.                 g_run_flag = 1;
    

  176.                 if (g_location_pid.SetPoint == 0) 
    

  177.                 {
    

  178.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  179.                                 g_motor_data.motor_pwm = 0;
    

  180.                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);                                  /* 设置电机转速 */
    

  181.                 } 
    

  182.                 else
    

  183.                 {
    

  184.                         DC_Motor_Start(MOTOR_1);
    

  185.                         if(g_location_pid.SetPoint  >= 6600)
    

  186.                         {
    

  187.                                 g_location_pid.SetPoint  =6600;
    

  188.                         }
    

  189.                         atk_debug_send_motorstate(RUN_STATE);           /* 上传电机状态（运行） */
    

  190.                         DC_Motor_Speed(g_location_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
    


  191. 

  192.                 }
    

  193.         }
    

  194.         else if(key == KEY1_PRES)        {        
    

  195.                 TurnTogLed(LED1);
    

  196.                 g_location_pid.SetPoint -= 1320;        /* 正转一圈，电机旋转圈数 = 计数值变化量 / 44 / 30 */
    


  197. 

  198.                 g_run_flag = 1;
    

  199.                 if (g_location_pid.SetPoint == 0) 
    

  200.                 {
    

  201.                                 DC_Motor_Stop(MOTOR_1);           /* 停止则立刻响应 */                        
    

  202.                                 g_motor_data.motor_pwm = 0;
    

  203.                                 DC_Motor_Speed(g_motor_data.motor_pwm,MOTOR_1); /* 设置电机转速 */                                 
    

  204.                 } 
    

  205.                 else
    

  206.                 {
    

  207.                         DC_Motor_Start(MOTOR_1);
    

  208.                         if(g_location_pid.SetPoint  <= -6600)
    

  209.                         {
    

  210.                                 g_location_pid.SetPoint  =-6600;
    

  211.                         }
    

  212.                         DC_Motor_Speed(g_location_pid.SetPoint,MOTOR_1); /* 设置电机转速 */
    


  213. 

  214.                         atk_debug_send_motorstate(RUN_STATE);           /* 上传电机状态（运行） */
    

  215.                 }
    

  216.         }
    

  217.         else if(key == KEY2_PRES)
    

  218.         {
    

  219.                 DC_Motor_Stop(MOTOR_1);
    

  220.                 g_run_flag = 0;                                 /* 标记电机停止 */
    

  221.                 g_motor_data.motor_pwm = 0;
    

  222.                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);          /* 设置电机转向、速度 */
    

  223.                 g_location_pid.SetPoint = 0;
    

  224.                 pid_init();
    

  225.                 atk_debug_send_motorstate(BREAKED_STATE);           /* 上传电机状态（刹车） */
    

  226.                 atk_debug_send_initdata(TYPE_PID1, (float *)(&g_location_pid.SetPoint), KP, KI, KD);           /* 同步数据到上位机 */
    

  227.         }
    

  228. }
    


  229. 

  230. void DCMotor_Value(void)
    

  231. {
    

  232.         int32_t motor_pwm_temp = 0;
    

  233.         static uint16_t init_adc_val = 0;
    

  234.         static uint16_t g_time =0;
    

  235.         static uint8_t num = 0;
    

  236.         if(num < 2)
    

  237.         {
    

  238.                 num ++;
    

  239.                 atk_debug_send_motorcode(DC_MOTOR);         /* 上传电机类型（直流有刷电机） */
    

  240.     atk_debug_send_motorstate(IDLE_STATE);      /* 上传电机状态（空闲） */
    

  241.                 init_adc_val = g_adc_val[2];
    

  242.                 init_adc_val += g_adc_val[2];       /* 现在的值和上一次存储的值相加 */
    

  243.     init_adc_val /= 2;  
    

  244.         }else 
    

  245.         {                                                        
    

  246.                                 
    

  247.                                 
    


  248. 

  249.                                 /* 获取编码器值，用于计算速度 */
    

  250.                                 get_EncodeValue(5);        /* 中位平均值滤除编码器抖动数据，5ms计算一次速度*/
    

  251.                                 
    

  252.                                 if(g_time  % 1000 ==0)
    

  253.                                 {                                                                                 
    

  254.                                         
    

  255.                                         g_motor_data.volatage  = g_adc_val[0]*ADC2VBUS;                
    

  256.                                         atk_debug_send_valtage(g_motor_data.volatage );
    

  257.                                         __NOP();__NOP();__NOP();
    

  258.                                         g_motor_data.board_temp = get_temp(g_adc_val[1]);
    

  259.                                         atk_debug_send_temp(0,g_motor_data.board_temp);  
    

  260.                                         __NOP();__NOP();__NOP();
    

  261.                                         
    

  262.                                         g_motor_data.current          = (abs(g_adc_val[2]-init_adc_val)*ADC2CURT)*0.001;
    

  263.                                         atk_debug_send_current(g_motor_data.current,0,0);                        
    

  264.                                         __NOP();__NOP();__NOP();
    

  265.                                         
    

  266. //                                        printf("\r\n******************************\r\n");
    

  267. //                                        printf("KEY0：增加比较值，KEY1：减小比较值，KEY2：停止电机\r\n\r\n");
    

  268. //                                        printf("Valtage:%.2fV \r\n",    g_motor_data.volatage);                   /* 打印电压值 */
    

  269. //                                        printf("Temp:%.2fC \r\n",                    g_motor_data.board_temp   );                  /* 打印温度值 */
    

  270. //                                        printf("Current:%.2fmA \r\n",   g_motor_data.current); /* 打印电流值 */
    

  271. //                                        printf("电机速度：%.1f RPM\r\n\r\n",   g_motor_data.speed);                
    

  272. //                                        printf("\r\n******************************\r\n");
    

  273.                                 }
    


  274. 

  275.                                 else if(g_time % SMAPLSE_PID_SPEED == 0)                /* 电机速度 50ms进行一次pid计算 */
    

  276.                                 {
    

  277.                                         if(g_run_flag == 1)
    

  278.                                         {
    

  279.                                                 /* PID计算，输出比较值（占空比），再进行一阶低通滤波 */
    

  280.                                                 motor_pwm_temp = increment_pid_ctrl(&g_location_pid,g_motor_data.location);
    

  281.                                                 g_motor_data.motor_pwm = (int32_t)((g_motor_data.motor_pwm * 0.5) + (motor_pwm_temp * 0.5));
    

  282.                                                  if (g_motor_data.motor_pwm >= 3000)                     /* 限制占空比 */
    

  283.                                                 {
    

  284.                                                                 g_motor_data.motor_pwm = 3000;
    

  285.                                                 }
    

  286.                                                 else if (g_motor_data.motor_pwm <= -3000)
    

  287.                                                 {
    

  288.                                                                 g_motor_data.motor_pwm = -3000;
    

  289.                                                 }
    

  290.                                                 atk_debug_send_wave_data( 1 ,g_motor_data.location);                                        /* 选择通道1，发送实际位置（波形显示）*/
    

  291.             atk_debug_send_wave_data( 2 ,g_location_pid.SetPoint);                                      /* 选择通道2，发送目标位置（波形显示）*/
    

  292.                                                 
    

  293.                                                 Motor_PWM_Set(g_motor_data.motor_pwm,MOTOR_1);
    

  294.                                         }
    

  295.                                 }
    


  296. 

  297.                                 g_time ++;
    

  298.         }
    

  299. }
    


  300. 

  301. void get_EncodeValue(uint8_t  ms)
    

  302. {
    

  303.         int encode_now = gtim_get_encode();
    

  304.         
    

  305.   g_motor_data.location = encode_now;                             /* 获取当前计数总值，用于位置闭环控制 */     
    

  306.         speed_computer(encode_now, ms);
    

  307. }
    


  308. 


  309. 

  310. /**
    

  311.  * @brief       pid初始化
    

  312.  * @param       无
    

  313.  * @retval      无
    

  314.  */
    

  315. void pid_init(void)
    

  316. {
    

  317.         
    

  318.                 memset(&g_motor_data,0,sizeof(g_motor_data));
    

  319.                 memset(&g_location_pid,0,sizeof(g_location_pid));
    

  320.         
    

  321.     g_location_pid.SetPoint = 0.0;       /* 设定目标值 */
    

  322.     g_location_pid.ActualValue = 0.0;    /* 期望输出值 */
    

  323.     g_location_pid.SumError = 0.0;       /* 积分值 */
    

  324.     g_location_pid.Error = 0.0;          /* Error[1] */
    

  325.     g_location_pid.LastError = 0.0;      /* Error[-1] */
    

  326.     g_location_pid.PrevError = 0.0;      /* Error[-2] */
    

  327.     g_location_pid.Proportion = KP;      /* 比例常数 Proportional Const */
    

  328.     g_location_pid.Integral = KI;        /* 积分常数 Integral Const */
    

  329.     g_location_pid.Derivative = KD;      /* 微分常数 Derivative Const */ 
    

  330.         
    

  331.                 atk_debug_send_wave_data( 1 ,g_motor_data.speed);                           /* 选择通道1，发送实际速度（波形显示）*/
    

  332.                 atk_debug_send_wave_data( 2 ,g_location_pid.SetPoint);                         /* 选择通道2，发送目标速度（波形显示）*/
    

  333.                 atk_debug_send_wave_data( 3 ,g_motor_data.motor_pwm * 100 / 8400);          /* 选择通道3，发送占空比（波形显示）*/
    

  334. }
    


  335. 

  336. void DCMotorsample_Init(void)
    

  337. {
    

  338.                 pid_init();
    

  339.                 ADC1CbReg(get_Motor_value);
    

  340. }





   
回复

收藏 举报

相关帖子
返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

zero949079783

33

主题

91

帖子

1

粉丝
快速回复 在线客服 返回列表 返回顶部