久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

<cite id="424kw"></cite>

<code id="424kw"></code>

<button id="424kw"><dl id="424kw"></dl></button>

標題: KEA128電磁直立例程 [打印本頁]

---

作者: captains    時間: 2018-5-17 16:27
標題: KEA128電磁直立例程
這是我上周做的智能車電磁直立例程
單片機源程序如下:

1. /*!
   
2. *
   
3. * @file       main.c
   
4. * @brief      山外KEA 平臺主程序
   
5. * @author     山外科技
   
6. * @version    v6.0
   
7. * @date       2017-12-10
   
8. */
   
9. 
   
10. #include "common.h"
    
11. #include "include.h"
    
12. 
    
13. /*!   
    
14. *  @brief      main函數   
    
15. *  @since      v6.0   
    
16. *  @note  **********直立3隊***********************
    
17.            **********2018年3月4日***********************
    
18.            **********電磁直立程序************************  
    
19. */
    
20. 
    
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"
   
2. 
   
3. /******************編程思想*****************************************************
   
4. *******************起跑前尋找最大值和最小值**************************************
   
5. *******************電感采集（存入Collect_ADi[9]）*********************************
   
6. *******************中值濾波（存入AD_filter[2]）***********************************
   
7. *******************歸一化值（存入ADf_returnone[2]和AD_returnone[2]）**************
   
8. *******************差除和計算***************************************************
   
9. *******************各電感配合使用，求偏差，轉向PWM*******************************
   
10. *******************丟線處理，彎道重疊處理(?????)，圓環處理******************************/
    
11. 
    
12. 
    
13. uint16 Collect_AD1[5],Collect_AD2[5];//首次采集存入數組
    
14. 
    
15. //uint16 Collect_AD[2];
    
16. uint16 left_electro,right_electro;
    
17. uint16 left_one,right_one;
    
18. 
    
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電感采集最大值
    
25. 
    
26. // uint16 elcmin,elcmax;
    
27. 
    
28. //uint16 frist_Collect_AD_min[2]={0};
    
29. // uint16 frist_Collect_AD_max[2]={0};
    
30. 
    
31. uint8 left_flag,right_flag;
    
32. 
    
33. 
    
34. float difference_division_sum_data=0;
    
35. float last_difference_division_sum_data=0;
    
36. 
    
37. 
    
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. }
    
78. 
    
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. }
     
118. 
     
119. void sensor_to_one()//電感濾波值歸一化處理
     
120. {
     
121.   uint8 i;
     
122. 
     
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. */
     
141. 
     
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.     }
     
155. 
     
156.     if(difference_division_sum_data>1) difference_division_sum_data=1;
     
157.     if(difference_division_sum_data<-1) difference_division_sum_data=-1;
     
158. 
     
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;
     
164. 
     
165. //    last_difference_division_sum_data=difference_division_sum_data;
     
166. }
     
167. 
     
168. void electro_data_handle()
     
169. {
     
170.   Collect_electro_filter();
     
171. //  sensor_to_one();
     
172.   difference_division_sum();
     
173. }
     

復制代碼

1. #include "control.h"
   
2. 
   
3. /*************************************************
   
4. ****************參數定義*****************
   
5. ***********************************************/
   
6. float Angle_Middle=9.6;
   
7. //float Angle_Middle=10;
   
8. 
   
9. float b_kp=1500,b_kd=2.125;
   
10. 
    
11. float v_kp=0.006,v_ki=0.000025;
    
12. //float v_kp=0,v_ki=0;
    
13. 
    
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;
    
16. 
    
17. float angle_deviation=0;
    
18. 
    
19. int16 Set_velocity=-650;//設定速度必須為負值
    
20. 
    
21. int16 Balance_PWM=0,Velocity_PWM=0,Turn_PWM=0;
    
22. 
    
23. int16 encoder_right=0,encoder_left=0;
    
24. 
    
25. int16 encoder_sum=0;
    
26. 
    
27. float velocity_deviation,velocity_proportion,velocity_integral;
    
28. 
    
29. 
    
30. 
    
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. /****************編碼器采集*******************/
    
50. 
    
51. void encoder_collect()
    
52. {
    
53. 
    
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. }
    
86. 
    
87. /******************速度控制*************************/
    
88. 
    
89. float velocity_control(int16 encoder_sum1)//速度閉環控制
    
90. {
    
91.   int16 encoder_deviation;
    
92. 
    
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;
     
110. 
     
111. }
     
112. 
     
113. 
     
114. /******************************************/
     
115. 
     
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. }
     
139. 
     
140. /***********平衡速度串級控制*******************/
     
141. int16 balance_control(float Angle,float Gyro_Y)
     
142. {
     
143.   int16 balance;
     
144. 
     
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. }
     
168. 
     
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. }
     
185. 
     
186. /************輸出*****************/
     
187. 
     
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. }
     
200. 
     
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. */
     
221. 
     
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)

2018-5-18 02:07 上傳

點擊文件名下載附件
下載積分: 黑幣 -5

---

歡迎光臨 (http://m.zg4o1577.cn/bbs/)

Powered by Discuz! X3.1

感谢您访问我们的网站，您可能还对以下资源感兴趣：

久久久久久久999|99精品久久精品一区二区爱城|成人欧美一区二区三区在线播放|国产精品日本一区二区不卡视频|国产午夜视频|欧美精品在线观看免费

主站蜘蛛池模板： 国产精品一区久久久 | 日韩在线视频网址 | 久久99精品久久久久久秒播九色 | 亚洲高清在线 | 黄色片在线观看网址 | 91视视频在线观看入口直接观看 | 欧美黄a | www.99re5.com| 国产乱码精品一品二品 | 中文在线a在线 | 99色在线 | 日韩成人一区 | 久久久久久黄 | 国产精品久久久久久久粉嫩 | 超碰美女在线 | 欧美成人精品一区二区三区 | 国产一区二区三区高清 | 精品久久久久久久久久久久久久久久久 | 日本精品一区 | 国产视频1区2区 | 久久国产精品久久国产精品 | 亚洲一区二区成人 | 亚洲精品二三区 | 狠狠天天| 日韩欧美亚洲 | 黄免费观看视频 | 91资源在线播放 | 午夜精品视频 | 亚洲国产视频一区 | 亚洲性网 | 国产成人网 | 日本国产高清 | 一二区电影| 色爱综合网 | 欧美一级淫片免费视频黄 | 日韩成人在线视频 | 色综合九九 | 成人免费一区二区三区视频网站 | 久久精品久久久久久 | 天天弄天天操 | 国产视频1 |

<button id="kkqk2"><input id="kkqk2"></input></button>

<samp id="kkqk2"><em id="kkqk2"></em></samp><abbr id="kkqk2"><source id="kkqk2"></source></abbr>

<button id="kkqk2"><input id="kkqk2"></input></button><rt id="kkqk2"><tr id="kkqk2"></tr></rt>

<cite id="kkqk2"></cite>

<table id="kkqk2"></table>

<button id="kkqk2"></button>