十四届恩智浦变形金刚三轮组资料

部分代码
源码太大783056733要源码

1. /**
   
2. * @file                采集电感值
   
3. * @author                电子信息工程学院 李科磊
   
4. * @date                2019 3月22   
   
5. */
   
6. void Get_AD(void)
   
7. {
   
8.     uint8 i=0,j=0,k=0;
   
9.     int L_AD=0,R_AD=0,M_AD=0,B_AD=0;
   
10.     for(i=0;i<5;i++)//每个电感取5次值
    
11.     {
    
12.         B_AD+=adc_once(ADC0_SE11,ADC_10bit);//采集ADC0_SE0电压，精度10位
    
13.     }
    
14.     Standar_AD=B_AD*0.2;
    
15.     for(i=0;i<10;i++)//每个电感取5次值
    
16.     {
    
17.         Left_org[i]=adc_once(ADC0_SE10,ADC_10bit);//采集ADC0_SE0电压，精度10位
    
18.         Left_org[i]=(i>0&&(Left_org[i]-Left_org[i-1])>5?Left_org[i-1]:Left_org[i]);
    
19.         Right_org[i]=adc_once(ADC0_SE17,ADC_10bit);//采集ADC0_SE1电压，精度10位
    
20.         Right_org[i]=(i>0&&(Right_org[i]-Right_org[i-1])>5?Right_org[i-1]:Right_org[i]);
    
21.         Mid_org[i]=adc_once(ADC0_SE18,ADC_12bit);
    
22.         Mid_org[i]=(i>0&&(Mid_org[i]-Mid_org[i-1])>5?Mid_org[i-1]:Mid_org[i]);
    
23.     }
    
24. /*===========================冒泡降序排序=================================*/
    
25.       for(j=0;j<10;j++)  //10次数据
    
26.       {
    
27.         for(k=0;k<10-j;k++)
    
28.         {
    
29.           if(Left_org[k]<Left_org[k+1])
    
30.           {
    
31.             L_AD = Left_org[k+1];
    
32.             Left_org[k+1]=Left_org[k];
    
33.             Left_org[k]=L_AD;
    
34.           }
    
35.           if(Right_org[k]<Right_org[k+1])
    
36.           {
    
37.             R_AD = Right_org[k+1];
    
38.             Right_org[k+1]=Right_org[k];
    
39.             Right_org[k]=R_AD;
    
40.           }
    
41.           if(Mid_org[k]<Mid_org[k+1])
    
42.           {
    
43.             M_AD = Mid_org[k+1];
    
44.             Mid_org[k+1]=Mid_org[k];
    
45.             Mid_org[k]=M_AD;
    
46.           }
    
47. 
    
48.         }
    
49.       }
    
50. /*===========================中值滤波=================================*/
    
51. 
    
52.       g_fLeft_AD=(Left_org[3]+Left_org[4]+Left_org[5]+Left_org[6])*0.25f;
    
53.       g_fRight_AD=(Right_org[3]+Right_org[4]+Right_org[5]+Right_org[6])*0.25f;
    
54.       g_fCenter_AD=(Mid_org[3]+Mid_org[4]+Mid_org[5]+Mid_org[6])*0.25f;
    
55. //**********最小二乘法求斜率*********//
    
56.       for(i=0;i<7;i++)
    
57.       {
    
58.         AD_right_buff[i]=AD_right_buff[i+1];
    
59.         AD_left_buff[i]=AD_left_buff[i+1];
    
60.       }
    
61.       AD_right_buff[7]=g_fRight_AD;
    
62.       AD_left_buff[7]=g_fLeft_AD;
    
63. 
    
64.       float xsum=0.216,ysum=0,xysum=0,x2sum=0.00734;//0.01116
    
65.       for(i=0;i<8;i++)
    
66.       {
    
67.         ysum+=AD_right_buff[i];
    
68.         xysum+=(i+1)*(AD_right_buff[i])*0.006;
    
69.       }
    
70.       AD_right_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;
    
71. 
    
72.       ysum = xysum = 0;
    
73.       for(i=0;i<8;i++)
    
74.       {
    
75.         ysum+=AD_left_buff[i];
    
76.         xysum+=(i+1)*(AD_left_buff[i])*0.006;
    
77.       }
    
78.       AD_left_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;
    
79.       Exp=500/(g_fCenter_AD+0.5*abs((int)(g_fLeft_AD-g_fRight_AD)));
    
80.       Exp=Exp>2?2:Exp;
    
81.       Exp=Exp<1?1:Exp;
    
82.       error_AD=(g_fLeft_AD-g_fRight_AD)*100*Exp/(g_fRight_AD+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
    
83.       if(fall_flag==0)
    
84.       {
    
85.           if(obstacl_flag==1)
    
86.           {
    
87.              error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30   //中弯道中速28 40 23
    
88.           }
    
89.           else  if(obstacl_flag==2)
    
90.           {
    
91.              error_AD=-32*g_fRealSpeed/140;//  三轮中速30//40
    
92.           }
    
93.           else  if(obstacl_flag==3)
    
94.           {
    
95.              error_AD=40*g_fRealSpeed/140;
    
96.           }
    
97.       }
    
98.       else
    
99.       {
    
100.           if(obstacl_flag==1)
     
101.           {
     
102.              error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30   //中弯道中速28 40 23
     
103.           }
     
104.           else  if(obstacl_flag==2)
     
105.           {
     
106.              error_AD=-30*g_fRealSpeed/140;//  三轮中速30//40
     
107.           }
     
108.           else  if(obstacl_flag==3)
     
109.           {
     
110.              error_AD=40*g_fRealSpeed/140;
     
111.           }
     
112.       }
     
113. 
     
114. 
     
115. }
     
116. /**********环岛判断**********/
     
117. void Round_Judge()
     
118. {
     
119.    if(QZAngle<-95&&(fall_flag==0||fall_flag==4))//直立状态下环岛检测
     
120.    {
     
121.      if(Deraction%3==1)
     
122.      {
     
123.        if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1600)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope>60&&g_fCenter_AD>300)//预判断
     
124.         {
     
125.                Round_flag_L=1;
     
126.                if(Deraction%3==1)
     
127.                {
     
128.                   Deraction=2;
     
129.                }
     
130.                else
     
131.                {
     
132.                   Deraction=0;
     
133.                }
     
134.         }
     
135.      }
     
136.      else  if(Deraction%3==0)
     
137.      {
     
138.         if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1800)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope<0&&g_fCenter_AD>350)//预判断
     
139.         {
     
140.                   Round_flag_R=1;
     
141.                   Deraction=1;
     
142.           }
     
143.       }
     
144.       else if(Deraction%3==2)
     
145.       {
     
146.          if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1800)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope==0&&g_fCenter_AD>350&&AD_left_slope>50)//预判断
     
147.          {
     
148.                   Round_flag_L=1;
     
149.                   Deraction=0;
     
150.           }
     
151.       }
     
152.       if(AD_right_slope>40&&Round_flag_R==1)//环岛入口+(QZAngle+91)*10   Standar_AD>220  60 40
     
153.       {
     
154.          Round_flag_R=2;
     
155.          Counter=-1;
     
156.       }
     
157.       if(AD_left_slope>0&&Round_flag_L==1&&AD_right_slope==0&&Deraction%3==2)////环岛入口 200*(130/g_fRealSpeed）
     
158.       {
     
159.            Round_flag_L=2;
     
160.            Counter=-1;
     
161.       }
     
162.       if(AD_left_slope>0&&Round_flag_L==1&&Deraction%3==0)//环岛入口 200*(130/g_fRealSpeed）
     
163.       {
     
164.            Round_flag_L=2;
     
165.            Counter=-1;
     
166.       }
     
167.         if(Round_flag_R==2)//右环打角
     
168.         {
     
169.            error_AD=(g_fLeft_AD*0.24-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.24+0.5*g_fCenter_AD);//差比和获取偏差
     
170.            error_AD=error_AD>55?55:error_AD;//42
     
171.            error_AD=error_AD<-55?-55:error_AD;
     
172. 
     
173.         }
     
174.         if(Round_flag_L==2)//左环打角
     
175.         {
     
176.            error_AD=(g_fLeft_AD-g_fRight_AD*0.24)*300/(g_fRight_AD*0.24+g_fLeft_AD+0.5*g_fCenter_AD);//差比和获取偏差
     
177.            if(Deraction%3==0)
     
178.            {
     
179.               //error_AD=error_AD<-40?-40:error_AD;//42
     
180.               error_AD=25;//error_AD>40?40:error_AD;
     
181.            }
     
182.            else
     
183.            {
     
184.               error_AD=error_AD<-50?-50:error_AD;//42
     
185.               error_AD=error_AD>50?50:error_AD;
     
186.            }
     
187. 
     
188.         }
     
189.         if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
     
190.         {
     
191.             Counter--;
     
192.         }
     
193.        if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理&&Counter>0
     
194.        {
     
195.          error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
     
196.          error_AD=error_AD<-45?-45:error_AD;//42
     
197.          error_AD=error_AD>45?45:error_AD;
     
198.        }
     
199.        else  if(Round_flag_R==0&&Round_L==2)
     
200.        {
     
201.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
     
202.          error_AD=error_AD<-45?-45:error_AD;//42
     
203.          error_AD=error_AD>45?45:error_AD;
     
204.        }
     
205.        if(Deraction%3==0)
     
206.       {
     
207.          if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
     
208.         {
     
209.           g_fLeft_AD=400-Counter*1;
     
210.          error_AD=(g_fLeft_AD-g_fRight_AD)*100/(g_fLeft_AD+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
     
211.          error_AD=error_AD<-45?-45:error_AD;//42
     
212.          error_AD=error_AD>45?45:error_AD;
     
213.          }
     
214.       }
     
215.        else if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
     
216.        {
     
217.          error_AD=(350-g_fRight_AD)*100/(g_fRight_AD+350+1*g_fCenter_AD);//差比和获取偏差
     
218.          error_AD=error_AD<-45?-45:error_AD;//42
     
219.          error_AD=error_AD>45?45:error_AD;
     
220.        }
     
221.        else if(Round_flag_L==0&&Round_R==2)
     
222.        {
     
223.          error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
     
224.          error_AD=error_AD<-45?-45:error_AD;//42
     
225.          error_AD=error_AD>45?45:error_AD;
     
226.        }
     
227.    }
     
228.    if(fall_flag==2)//三轮状态下环岛检测
     
229.    {
     
230.       if(((g_fRight_AD>850&&g_fLeft_AD>850))&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&g_fCenter_AD<700)//预判断||(g_fRight_AD>900||g_fLeft_AD>900)
     
231.         {
     
232.             if(Deraction%3==0)
     
233.             {
     
234.               Round_flag_R=1;
     
235.               Deraction=1;
     
236.             }
     
237.             else  if(Deraction==1)
     
238.             {
     
239.                Round_flag_L=1;
     
240.                Deraction=2;
     
241.             }
     
242.             else if(Deraction==2)
     
243.             {
     
244.                  Round_flag_L=1;
     
245.                  Deraction=0;
     
246.             }
     
247.         }
     
248.         if(Standar_AD>550&&Round_flag_R==1)//环岛入口
     
249.         {
     
250.            Round_flag_R=2;
     
251.            Counter=-1;
     
252.         }
     
253.         if(Standar_AD>535*(150.0/g_fRealSpeed)&&Round_flag_L==1&&Deraction==2)////环岛入口
     
254.         {
     
255.            Round_flag_L=2;
     
256.            Counter=-1;
     
257.         }
     
258.         if(Standar_AD>600&&Round_flag_L==1&&Deraction==0)////环岛入口
     
259.         {
     
260.            Round_flag_L=2;
     
261.            Counter=-1;
     
262. 
     
263.         }
     
264.         if(Round_flag_R==2)//右环打角
     
265.         {
     
266.            error_AD=(g_fLeft_AD*0.25-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.25+0.5*g_fCenter_AD);//差比和获取偏差
     
267.            error_AD=error_AD>(42*g_fRealSpeed/140)?(42*g_fRealSpeed/140):error_AD;//42
     
268.            error_AD=error_AD<(-42*g_fRealSpeed/140)?(-42*g_fRealSpeed/140):error_AD;
     
269.         }
     
270.         if(Round_flag_L==2)//左环打角
     
271.         {
     
272.            //差比和获取偏差
     
273.           if(Deraction%3==0)
     
274.           {
     
275.             error_AD=17*(g_fRealSpeed/135.0);//error_AD>40?40:error_AD;
     
276.           }
     
277.           else
     
278.           {
     
279.             error_AD=(g_fLeft_AD-g_fRight_AD*0.25)*300/(g_fRight_AD*0.25+g_fLeft_AD+0.5*g_fCenter_AD);
     
280.             error_AD=error_AD<-42?-42:error_AD;//42
     
281.             error_AD=error_AD>42?42:error_AD;
     
282.           }
     
283.         }
     
284.         if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
     
285.         {
     
286.            error_AD=(200-g_fRight_AD)*100/(250+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
     
287.            error_AD=error_AD<-42?-42:error_AD;//42
     
288.            error_AD=error_AD>42?42:error_AD;
     
289.        }
     
290.        else if(Round_flag_L==0&&Round_R==2)
     
291.        {
     
292.            error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
     
293.            error_AD=error_AD<-42?-42:error_AD;//42
     
294.            error_AD=error_AD>42?42:error_AD;
     
295.        }
     
296.        if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理//(1+(Counter-300)/500)
     
297.       {  
     
298.         if(Deraction%3==2)
     
299.         {
     
300.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
     
301.            error_AD=error_AD<-45?-45:error_AD;//42
     
302.            error_AD=error_AD>45?45:error_AD;
     
303.         }
     
304.         else
     
305.         {
     
306.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
     
307.            error_AD=error_AD<-45?-45:error_AD;//42
     
308.            error_AD=error_AD>45?45:error_AD;
     
309.         }
     
310.       }
     
311.       if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
     
312.       {
     
313.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
     
314.          error_AD=error_AD<-45?-45:error_AD;//42
     
315.          error_AD=error_AD>45?45:error_AD;
     
316.       }
     
317. //      if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
     
318. //      {
     
319. //       error_AD=(200-g_fRight_AD)*100/(200+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
     
320. //       error_AD=error_AD<-45?-45:error_AD;//42
     
321. //       error_AD=error_AD>45?45:error_AD;
     
322. //       }
     
323. 
     
324. 
     
325.    }
     
326.    if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
     
327.    {
     
328.       Counter--;
     
329.    }
     
330.    if((Round_R==1||Round_L==1||Round_L==2||Round_R==2)&&Counter==0)//变量清零
     
331.    {
     
332.       Round_R=0;
     
333.       Round_L=0;
     
334.    }
     
335. }
     
336. /**
     
337. * @file                方向外环
     
338. * @note                      三电感
     
339. * @brief                山外K60平台
     
340. * @author                电子信息工程学院 李科磊
     
341. * @date                2019年3月22日
     
342. */
     
343. void DerectError_Get(void)  //外环
     
344. {
     
345.   Get_AD();
     
346.   if(obstacl_Delay<300)
     
347.   {
     
348.      Round_Judge();
     
349.   }
     
350. 
     
351. 
     
352.   if(fall_flag==2)
     
353.   {
     
354.      target_vel_z=(5.7)*error_AD+4.5*(error_AD-last_error_AD);
     
355.      target_vel_z=target_vel_z>240?240:target_vel_z;
     
356.      target_vel_z=target_vel_z<-240?-240:target_vel_z;
     
357.   }
     
358.   else
     
359.   {
     
360.      target_vel_z=(5.0)*error_AD+3.0*(error_AD-last_error_AD);
     
361.      target_vel_z=target_vel_z>250?250:target_vel_z;
     
362.      target_vel_z=target_vel_z<-250?-250:target_vel_z;
     
363.   }
     
364.   last_error_AD=error_AD;
     
365. }
     
366. /**
     
367. * @file                方向内环
     
368. * @note                      串级
     
369. * @brief                山外K60
     
370. * @author                沈阳航空航天大学 天牛座
     
371. * @date                2019年3月22日
     
372. */
     
373. void Direction_out(void)
     
374. {
     
375.   float error;
     
376.   static  float last_error;
     
377.   float price_Differention;
     
378.   float price_Proportion;
     
379.   static float Pre1_Error[4];
     
380.   //error = target_vel_z*13*(-89/QZAngle) - GYRO_Speed_Z*0.25*(-100/QZAngle);//转向角度串方向
     
381.   error = target_vel_z*13.0 - GYRO_Speed_Z*0.25;
     
382.   price_Proportion=g_priceControl_P_in * error;
     
383.   price_Differention=g_priceControl_D_in * (error - last_error);
     
384.   if(fall_flag==2)
     
385.   {
     
386.      error = target_vel_z*13.1 - GYRO_Speed_Z*0.245;//转向角度串方向
     
387.      price_Proportion=g_priceControl_P_in * error;
     
388.      price_Differention=g_priceControl_D_in * (error - last_error)*1.1;
     
389.   }
     
390.   Direct_Parameter =price_Proportion  + price_Differention ;//(1+error*error*0.000000001f)
     
391. 
     
392.   last_error =  error;
     
393.   Pre1_Error[3]=Pre1_Error[2];
     
394.   Pre1_Error[2]=Pre1_Error[1];
     
395.   Pre1_Error[1]=Pre1_Error[0];
     
396.   Pre1_Error[0]=Direct_Parameter;
     
397.   Direct_Parameter=Pre1_Error[0]*0.8+Pre1_Error[1]*0.1+Pre1_Error[2]*0.06+Pre1_Error[3]*0.02 ;
     
398.   price_PWM=Direct_Parameter;
     
399.   price_PWM=price_PWM>550?550:price_PWM;
     
400.   price_PWM=price_PWM<-550?-550:price_PWM;
     
401. }
     

复制代码