设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 智能小车/机器人 › KEA128电磁直立例程
返回列表 发新帖
查看: 4089|回复: 0
打印 上一主题 下一主题
收起左侧

KEA128电磁直立例程

[复制链接]
跳转到指定楼层
楼主
ID:288295 发表于 2018-5-17 16:27 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
这是我上周做的智能车电磁直立例程
单片机源程序如下:
  1. /*!
  2. *
  3. * @file       main.c
  4. * @brief      山外KEA 平台主程序
  5. * @author     山外科技
  6. * @version    v6.0
  7. * @date       2017-12-10
  8. */

  10. #include "common.h"
  11. #include "include.h"

  13. /*!   
  14. *  @brief      main函数   
  15. *  @since      v6.0   
  16. *  @note  **********直立3队***********************
  17.            **********2018年3月4日***********************
  18.            **********电磁直立程序************************  
  19. */

  21. void main(void)
  22. {
  23.     summary_init();
  24.     while(1)
  25.     {
  26. //      electro_data_handle();
  27.       key_scan();
  28.       menu_show();
  29.     }
  30. }
复制代码
  1. #include "electro.h"

  3. /******************编程思想*****************************************************
  4. *******************起跑前寻找最大值和最小值**************************************
  5. *******************电感采集（存入Collect_ADi[9]）*********************************
  6. *******************中值滤波（存入AD_filter[2]）***********************************
  7. *******************归一化值（存入ADf_returnone[2]和AD_returnone[2]）**************
  8. *******************差除和计算***************************************************
  9. *******************各电感配合使用，求偏差，转向PWM*******************************
  10. *******************丢线处理，弯道重叠处理(?????)，圆环处理******************************/


  13. uint16 Collect_AD1[5],Collect_AD2[5];//首次采集存入数组

  15. //uint16 Collect_AD[2];
  16. uint16 left_electro,right_electro;
  17. uint16 left_one,right_one;

  19. //归一化参数
  20. uint16 AD_filter[2]={0,0};//电感滤波值
  21. //uint8 AD_returnone[2]={0,0};//归一化值
  22. //float ADf_returnone[2]={0};//归一化值
  23. //uint16 min_v[2]={3000,3000};//起跑前2电感采集最小值
  24. //uint16 max_v[2]={726,726};//起跑前2电感采集最大值

  26. // uint16 elcmin,elcmax;

  28. //uint16 frist_Collect_AD_min[2]={0};
  29. // uint16 frist_Collect_AD_max[2]={0};

  31. uint8 left_flag,right_flag;


  34. float difference_division_sum_data=0;
  35. float last_difference_division_sum_data=0;


  38. void Collect_electro_filter()//电感采集  中值滤波
  39. {
  40.     uint8 i,filter_i,filter_j;
  41.     uint16 temp;
  42.    
  43.     for(i=0;i<5;i++) //L1
  44.     {  
  45.      Collect_AD1[i] =adc_once(ADC0_SE0,ADC_12bit);//A0;
  46.     }
  47.     for(i=0;i<5;i++) //L2
  48.     {  
  49.      Collect_AD2[i] =adc_once(ADC0_SE1,ADC_12bit);//A1;
  50.     }
  51.    
  52.     for(filter_i=0;filter_i<5;filter_i++) //中值为AD[3]
  53.     {
  54.       for(filter_j=0;filter_j<5-filter_i;filter_j++)
  55.       {
  56.         if(Collect_AD1[filter_j]>Collect_AD1[filter_j+1])
  57.            {
  58.               temp=Collect_AD1[filter_j];
  59.               Collect_AD1[filter_j]=Collect_AD1[filter_j+1];
  60.               Collect_AD1[filter_j+1]=temp;
  61.            }
  62.       }
  63.        for(filter_j=0;filter_j<5-filter_i;filter_j++)
  64.       {
  65.         if(Collect_AD2[filter_j]>Collect_AD2[filter_j+1])
  66.            {
  67.                temp=Collect_AD2[filter_j];
  68.                Collect_AD2[filter_j]=Collect_AD2[filter_j+1];
  69.                Collect_AD2[filter_j+1]=temp;
  70.            }
  71.        }            
  72.     }
  73. AD_filter[0]=Collect_AD1[2];
  74. AD_filter[1]=Collect_AD2[2];
  75. left_electro=AD_filter[0];
  76. right_electro=AD_filter[1];
  77. }

  79. /*
  80. void find_L_min()//起跑前找到2电感最小值
  81. {
  82.   uint8 i;
  83.   
  84.   for(i=0;i<10;i++)
  85.   {
  86.     frist_Collect_AD_min[0]=adc_once(ADC0_SE0, ADC_12bit);
  87.     if(frist_Collect_AD_min[0]<min_v[0]) min_v[0]=frist_Collect_AD_min[0];
  88.   }
  89.   
  90.     for(i=0;i<10;i++)
  91.   {
  92.     frist_Collect_AD_min[1]=adc_once(ADC0_SE1, ADC_12bit);
  93.     if(frist_Collect_AD_min[1]<min_v[1]) min_v[1]=frist_Collect_AD_min[1];
  94.   }
  95.   
  96. }
  97. void find_L_max()//起跑前找到2电感最大值
  98. {
  99.   uint32 i;
  100.   DELAY_MS(25);
  101.     for(i=0;i<200000;i++)
  102.   {
  103.     frist_Collect_AD_max[0]=adc_once(ADC0_SE0, ADC_12bit);
  104.     if(frist_Collect_AD_max[0]>max_v[0])max_v[0]=frist_Collect_AD_max[0];
  105.     frist_Collect_AD_max[1]=adc_once(ADC0_SE1, ADC_12bit);
  106.     if(frist_Collect_AD_max[1]>max_v[1])max_v[1]=frist_Collect_AD_max[1];
  107.     //DELAY_MS(3);
  108.   }
  109. //    for(i=0;i<600;i++)
  110. //  {
  111. //    frist_Collect_AD_max[1]=adc_once(ADC0_SE1, ADC_12bit);
  112. //    if(frist_Collect_AD_max[1]>max_v[1])max_v[1]=frist_Collect_AD_max[1];
  113. //    DELAY_MS(3);
  114. //  }
  115.   elcmin=max_v[0];
  116.   elcmax=max_v[1];
  117. }

  119. void sensor_to_one()//电感滤波值归一化处理
  120. {
  121.   uint8 i;

  123.   float sensor_to_one[2];
  124.   for(i=0;i<2;i++)
  125.   {
  126.    sensor_to_one[i]=(float)(AD_filter[i]-min_v[i])/(float)(max_v[i]-min_v[i]);
  127.    if(sensor_to_one[i]<=0.0)
  128.      sensor_to_one[i]=0.001;
  129.    if(sensor_to_one[i]>1.0)
  130.      sensor_to_one[i]=1.0;
  131.    
  132.    ADf_returnone[i]=(100*sensor_to_one[i]);// AD[i]归一后的值。0~100之间
  133.    AD_returnone[i]=(uint8)ADf_returnone[i];
  134.    left_flag=AD_returnone[0];
  135.    right_flag=AD_returnone[1];
  136.    left_one=AD_returnone[0];
  137.    right_one=AD_returnone[1];
  138.   }
  139. }
  140. */

  142. void difference_division_sum()//差除和计算
  143. {
  144.    
  145.     difference_division_sum_data=(float)((AD_filter[0]-AD_filter[1])*(AD_filter[0]-AD_filter[1]))/(float)((AD_filter[0]+AD_filter[1])*(AD_filter[0]+AD_filter[1]));
  146.    
  147.     if(AD_filter[0]>AD_filter[1])
  148.     {
  149.       difference_division_sum_data=-difference_division_sum_data;
  150.     }
  151.     if(AD_filter[0]<AD_filter[1])
  152.     {
  153.       difference_division_sum_data=difference_division_sum_data;
  154.     }

  156.     if(difference_division_sum_data>1) difference_division_sum_data=1;
  157.     if(difference_division_sum_data<-1) difference_division_sum_data=-1;

  159.     if(AD_filter[0]<260)
  160.     difference_division_sum_data=0.34;
  161.    
  162.     if(AD_filter[1]<260)
  163.     difference_division_sum_data=-0.34;

  165. //    last_difference_division_sum_data=difference_division_sum_data;
  166. }

  168. void electro_data_handle()
  169. {
  170.   Collect_electro_filter();
  171. //  sensor_to_one();
  172.   difference_division_sum();
  173. }
复制代码
  1. #include "control.h"

  3. /*************************************************
  4. ****************参数定义*****************
  5. ***********************************************/
  6. float Angle_Middle=9.6;
  7. //float Angle_Middle=10;

  9. float b_kp=1500,b_kd=2.125;

  11. float v_kp=0.006,v_ki=0.000025;
  12. //float v_kp=0,v_ki=0;

  14.   //float t_kp=600.0,t_kd=1.0,t_kg=0.06;
  15. float t_kp=1150,t_kd=7,t_kg=0;

  17. float angle_deviation=0;

  19. int16 Set_velocity=-650;//设定速度必须为负值

  21. int16 Balance_PWM=0,Velocity_PWM=0,Turn_PWM=0;

  23. int16 encoder_right=0,encoder_left=0;

  25. int16 encoder_sum=0;

  27. float velocity_deviation,velocity_proportion,velocity_integral;



  31. /*****************PWM输出********************/
  32. void PWM_out(int16 x11,int16 x12,int16 x21,int16 x22)
  33. {
  34.     if(x11<1)x11=0;if(x11>9999)x11=9999;
  35.     if(x12<1)x12=0;if(x12>9999)x12=9999;
  36.     if(x21<1)x21=0;if(x21>9999)x21=9999;
  37.     if(x22<1)x22=0;if(x22>9999)x22=9999;
  38.     if(x11==0||x12==0)
  39.     {
  40.       if(x21==0||x22==0)
  41.       {
  42.          ftm_pwm_init(FTM2, FTM_CH0,10000,x11);
  43.          ftm_pwm_init(FTM2, FTM_CH1,10000,x12);
  44.          ftm_pwm_init(FTM2, FTM_CH2,10000,x22);
  45.          ftm_pwm_init(FTM2, FTM_CH3,10000,x21);        
  46.       }
  47.     }
  48. }
  49. /****************编码器采集*******************/

  51. void encoder_collect()
  52. {

  54.     encoder_left= ftm_pulse_get(FTM0);
  55.     if(PTB2_IN == 1)
  56.     {
  57.         encoder_left=-encoder_left;
  58.     }
  59.     else
  60.     {
  61.         encoder_left=encoder_left;
  62.     }
  63.     ftm_pulse_clean(FTM0);       //清空计数。
  64.     encoder_right= ftm_pulse_get(FTM1);
  65.     if(PTB3_IN == 1)
  66.     {
  67.         encoder_right=encoder_right;
  68.     }
  69.     else
  70.     {
  71.         encoder_right=-encoder_right;
  72.     }
  73.     ftm_pulse_clean(FTM1);       //清空计数。
  74.    
  75.     if(encoder_left>9999) encoder_left=9999;
  76.     if(encoder_left<-9999) encoder_left=-9999;
  77.    
  78.     if(encoder_right>9999) encoder_right=9999;
  79.     if(encoder_right<-9999) encoder_right=-9999;   
  80.    
  81.   if(encoder_left>5000&&encoder_right<5000)encoder_left =encoder_right;
  82.   if(encoder_left<5000&&encoder_right>5000)encoder_right=encoder_left;
  83.    
  84.     encoder_sum=encoder_left+encoder_right;     
  85. }

  87. /******************速度控制*************************/

  89. float velocity_control(int16 encoder_sum1)//速度闭环控制
  90. {
  91.   int16 encoder_deviation;

  93.   static float last_integral=0;
  94.   
  95.   encoder_deviation=Set_velocity+encoder_sum1;
  96.   velocity_proportion=encoder_deviation*v_kp;//比例   
  97.   
  98.   velocity_integral=encoder_deviation*v_ki;//积分
  99.   velocity_integral+=last_integral;
  100.   
  101.   if(velocity_integral<-1.0)velocity_integral=-1.0;//积分限幅
  102.   if(velocity_integral> 1.0)velocity_integral= 1.0;//积分限幅
  103.   last_integral=velocity_integral;
  104.   
  105.   velocity_deviation=velocity_proportion+velocity_integral;
  106. //  velocity_deviation=velocity_proportion;
  107.   if(velocity_deviation>10)velocity_deviation=10;
  108.   if(velocity_deviation<-10)velocity_deviation=-10;
  109.   return velocity_deviation;

  111. }


  114. /******************************************/

  116. int16 velocity_out(uint8 count,float velocity,float last_velocity)
  117. {
  118.   int16 velocity_pwm;
  119.   float velocity_data;
  120.   
  121.   velocity_data=(velocity-last_velocity)/20.0*count+last_velocity;
  122.   
  123.   velocity_pwm=(int16)(velocity_data);
  124.   
  125.   return velocity_pwm;
  126. }
  127. /**********************************************/
  128. int16 turn_out(uint8 count,float new_turn,float last_turn)
  129. {
  130.   int16 turn_pwm;
  131.   float turn_data;
  132.   
  133.   turn_data=(new_turn-last_turn)/20.0*count+last_turn;
  134.   
  135.   turn_pwm=(int16)(turn_data);
  136.   
  137.   return turn_pwm;
  138. }

  140. /***********平衡速度串级控制*******************/
  141. int16 balance_control(float Angle,float Gyro_Y)
  142. {
  143.   int16 balance;

  145.   static uint8 v_count=1;
  146.   static float last_velocity=0.0;
  147.   static float this_velocity=0.0;
  148.    
  149.   angle_deviation=Angle-Angle_Middle;
  150.   
  151.   v_count++;
  152.   if(v_count==21)
  153.   {
  154.      v_count=1;
  155.      encoder_collect();
  156.      last_velocity=this_velocity;
  157.      this_velocity=velocity_control(encoder_sum);
  158.   }
  159.   Velocity_PWM=velocity_out(v_count,this_velocity,last_velocity);
  160.   balance=(int16)(b_kp*(angle_deviation+Velocity_PWM)+b_kd*Gyro_Y);
  161.   
  162.   //balance=(int16)(b_kp*angle_deviation+b_kd*Gyro_Y);
  163.   if(balance> 9999)balance= 9999;
  164.   if(balance<-9999)balance=-9999;
  165.   
  166.   return balance;
  167. }

  169. /**************转向控制*********************/
  170. int16 turn_control(float turn_deviation,float gyro_turn)
  171. {
  172.   int16 turn;
  173.   float turn_proportion,turn_differential;
  174.   static float last_deviation;
  175.   
  176.   turn_proportion=t_kp*turn_deviation;
  177.   
  178.   turn_differential=t_kd*(turn_deviation-last_deviation);
  179.   last_deviation=turn_deviation;
  180.   
  181.   turn=(int16)(turn_proportion+turn_differential+t_kg*gyro_turn);
  182.    
  183.   return turn;
  184. }

  186. /************输出*****************/

  188. void balance_out(int16 balance_pwm,int16 turn_pwm)    //合成
  189. {
  190.   if(balance_pwm>=0)
  191.   {
  192.     PWM_out((int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm-turn_pwm),0);
  193.   }
  194.   else if(balance_pwm<0)
  195.   {
  196.     balance_pwm=-balance_pwm;
  197.     PWM_out(0,(int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm-turn_pwm));
  198.   }
  199. }

  201. /*
  202. void balance_out(int16 balance_pwm,int16 turn_pwm)    //合成
  203. {
  204.   if(balance_pwm>=0)
  205.   {
  206.     if(turn_pwm>=0)
  207.     PWM_out((int16)(balance_pwm-turn_pwm/2),0,(int16)(balance_pwm-turn_pwm),0);
  208.     if(turn_pwm<0)
  209.     PWM_out((int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm+turn_pwm/2),0);      
  210.   }
  211.   else if(balance_pwm<0)
  212.   {
  213.     balance_pwm=-balance_pwm;
  214.     if(turn_pwm>=0)
  215.     PWM_out(0,(int16)(balance_pwm-turn_pwm/2),0,(int16)(balance_pwm-turn_pwm));
  216.     if(turn_pwm<0)
  217.     PWM_out(0,(int16)(balance_pwm+turn_pwm),0,(int16)(balance_pwm+turn_pwm/2));
  218.   }
  219. }
  220. */

  222. void summary_control()
  223. {
  224.     Balance_PWM=balance_control(Angle_Balance,Gyro_Balance);
  225.     Turn_PWM   =turn_control(difference_division_sum_data,Gyro_Turn);
  226.     balance_out(Balance_PWM,Turn_PWM);
  227. //    led_turn(LED1);                     //LED1翻转
  228. }
复制代码

所有资料51hei提供下载:
电磁直立（KEA）.rar (188.69 KB, 下载次数: 69)


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

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

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表