标题: K60_CCD直立小车源码(二等奖程序) [打印本页]

---

作者: zoulichuan    时间: 2018-5-10 21:07
标题: K60_CCD直立小车源码(二等奖程序)
k60 ccd二等奖程序

本程序配套4按键直立一体板

按键功能：
E0： 参数加
E1： 参数减
E2： 切换显示及功能
E3： 回到最初显示及功能


调试直立

A、修改直立静态偏移量： calcultion.c   s32 GRAVITY_OFFSET=485;//230  MPU6550为平衡时Z轴角度，放大10倍
B、main.c里 run()函数中：  修改  PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//电机输出
                            为： PWMout=MotorSpeedOut(AAngPWM,0,0);//关闭速度环和转向环分量，直立后再加上两个分量



系统连线说明 ：


单片机源程序如下:

1. #include "include.h"
   
2. #include "calculation.h"
   
3. #include"VisualScope.h"
   
4. #include "CCD.h"
   
5. #include "LQ12864.h"
   
6. #include "MPU6050.h"
   
7. 
   
8. int16 angletmp,angle_g,angle_w;  //16位
   
9. int16 angle6550;  //MPU6550计算
   
10. 
    
11. extern byte F6x8[][6];
    
12. extern s16 g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma;
    
13. extern float GYROSCOPE_ANGLE_RATIO;
    
14. s32 TurnPosition;//实际位置
    
15. s32 TurnPositionOld; //上一次位置
    
16. int dline; //左右边界差
    
17. 
    
18. float  ANGLE_CONTROL_P=240;//* 260    //直立P  260
    
19. float  ANGLE_CONTROL_D=30;//1.5;//*  4.8   //直立D  5.8
    
20. 
    
21. int16 xgout=1000;   //MPU6050六轴参数  x轴角速度
    
22. int16 ygout=1000;
    
23. int16 zgout=1000;
    
24. int16 xaout=-1000;   //x轴加速度
    
25. int16 yaout=-1000;
    
26. int16 zaout=-1000;
    
27. 
    
28. uint8 PWMDEADL = 0;  //死区电压左路电机
    
29. uint8 PWMDEADR = 0;  //死区电压右路电机
    
30. 
    
31. uint16 MAXTURNPWM= 200;  //电机转向最大PWM值
    
32. 
    
33. #define   SAVEADRESS   (0X1C)
    
34. 
    
35. extern s32 GRAVITY_OFFSET;
    
36. uint8 disen;  //允许上传CCD图像
    
37. 
    
38. uint16 A_P,A_D,V_P,V_I,T_P,T_D;  //从FLASH中读出的参数
    
39. uint32 G_RATIO;
    
40. uint8 flashreadbuf[6];
    
41. 
    
42. u16 SpeedKP = 24;//4.4;//*              //速度P 放大10倍
    
43. u16 SpeedKI = 6;//*               //速度D 放大10倍
    
44. 
    
45. u16 TurnP1 = 80;  //直道前进时舵机PD值，放大10倍
    
46. u16 TurnD1 = 2;
    
47. 
    
48. //u16 TurnP2 = 34;//70;  //弯道时舵机转角比例系数 放大10倍
    
49. //u16 TurnD2 = 140;//30;  //弯道时舵机转角积分系数  放大100倍
    
50. //
    
51. //u16 TurnP3 = 40;  //大弯时转角比例
    
52. //u16 TurnD3 = 110;   //大弯时转角积分
    
53. //
    
54. //u16 TurnP4 = 48;  //大弯时转角比例
    
55. //u16 TurnD4 = 90;   //大弯时转角积分
    
56. 
    
57. u16 TurnP;     //* 94                 //方向P
    
58. u16 TurnD;   //* 8.4              //方向D
    
59. u16 turnsetmax;  //转向最大PWM值
    
60. u16 turnpwmmax;
    
61. u8  disccd[128][4];
    
62. u8  paraadjflag;   //参数修改组切换
    
63. 
    
64. 
    
65. extern int leftline, rightline; //左右边界值
    
66. extern int leftlineold,rightlineold; //左右边界上一次值
    
67. extern int yuzhizhi;//动态阈值
    
68. extern signed int sumlspeed,sumrspeed;
    
69. 
    
70. s16 SetSpeed= -250;
    
71. s16 AmSpeed;//*   //目标速度 用于外部       设置速度时在这里设置
    
72. s16 g_Speedgoal=0;//用于内部 自加速使用
    
73. //时间标志位
    
74. extern u8 TIME0flag_5ms,TIME0flag_10ms,TIME0flag_20ms;
    
75. extern u8 TIME1flag_100ms,flag_1ms ;
    
76. extern u8 TIME1flag_1s   ;  //PT1口1s定时标志位
    
77. extern s16 whitelength;//91;   //具体看CCD图像，赛道宽度
    
78. 
    
79. 
    
80. 
    
81. s32 ATimeCount=700;//100ms进入标志  由20个5ms构成，速度PID时使用
    
82. s32 TimeCount=0 ;//1ms中断标志
    
83. //角度传感器
    
84. s32 GYROSCOPE_OFFSET,GYROSCOPE_turn_OFFSET;//陀螺仪静止时的零点
    
85. s32 AD_ACC_Z;//加速度计的Z轴
    
86. s32 AD_GYRO;//平衡陀螺仪
    
87. s32 AD_GYRO_turn;//转向陀螺仪
    
88. s32 AAngPWM=0,LastAAngPWM=0,AAngPeriodCount=0,MotorAAngPWM=0 ;
    
89. //速度变量
    
90. float g_SpeedControlOutNew,g_SpeedControlOutOld ;
    
91. s16 SpeedPeriodCount=0;
    
92. s32 MotorSpeedPWM=0 ;
    
93. s32 PWMout ;
    
94. int start_flag=0,stop_jiasu=0;
    
95. //CCD变量
    
96. int ccd_count=0;
    
97. uint8_t Pixel[128];
    
98. int ccd_flag=0;
    
99. s16 TurnPeriodCount=0 ;
    
100. s32 MotorTurnPWM=0 ;
     
101. s16 TurnPWMOUT=0 ;
     
102. s16 LastTurnPWMOUT=0;
     
103. u16 discnt;
     
104. u8  workflag;
     
105. 
     
106. 
     
107. void run();//直立主函数
     
108. void qibu();//起步不能一下把速度加上去，速度要慢慢加
     
109. void GPIO_Init();
     
110. void dispage1();
     
111. void dispage2();
     
112. void dispage3();
     
113. void disdata1();
     
114. void disdata2();
     
115. void disdata3();
     
116. void disccdval();
     
117. extern void Pause(void);
     
118. extern void LCD_CLS(void);
     
119. 
     
120. void main()
     
121. {  int16 ii;
     
122.    discnt=0;
     
123.    paraadjflag=0;
     
124.    workflag=0;
     
125.    AmSpeed = SetSpeed;
     
126.    turnsetmax = 650;
     
127.    s32 xgtemp,ygtemp;
     
128.    DisableInterrupts; //禁止总中断
     
129.    uart_init (UART0,115200);//初始化UART0，输出脚PTA15，输入脚PTA14
     
130.    //AngleAcceleration_init() ;//AD初始化
     
131.    FTM1_QUAD_Iint();//正交解码测速  A相---PTA8  B相---PTA9
     
132.    FTM2_QUAD_Iint();//正交解码测速  A相---PTA10 B相---PTA11
     
133.    oled_init();//oled初始化
     
134.    CCD_init (); //CCD初始化
     
135.    GPIO_Init(); //程序运行灯
     
136.    pit_init_ms(PIT0, 1);  //初始化PIT0，定时时间为： 1ms
     
137.    pit_init_ms(PIT1, 100);//初始化PIT1，定时时间为： 100ms
     
138.    FTM_init() ; //PWM初始化
     
139.    delayms(100);
     
140.    MPU6050_Init();  //MPU6050初始化  , PD8  , PD9
     
141.    delayms(300);
     
142.    xgtemp=0;
     
143.    ygtemp=0;
     
144.    TurnP=TurnP1;
     
145.    TurnD=TurnD1;
     
146.    for(ii=0;ii<100;ii++)
     
147.    {
     
148.      xgtemp += MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//读X轴角速度,直立
     
149.      Pause();
     
150.      ygtemp += MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//读Y轴角速度，转向
     
151.      Pause();
     
152.    }
     
153.    GYROSCOPE_OFFSET = xgtemp/100;        //静态X轴角速度值，直立，理论上为0，但实际可能不为0
     
154.    GYROSCOPE_turn_OFFSET = ygtemp/100;   //静态Y轴角速度值，转向，理论上为0，但实际可能不为0
     
155.    
     
156.    EnableInterrupts;//开总中断  
     
157.    uart_irq_EN(UART0);
     
158.    
     
159.    uart_putchar(UART0,'O');
     
160.    uart_putchar(UART0,'K');
     
161. 
     
162.    A_P = ANGLE_CONTROL_P;
     
163.    A_D = ANGLE_CONTROL_D*10;
     
164.    V_P = SpeedKP;
     
165.    V_I = SpeedKI;
     
166.    T_P = TurnP;
     
167.    T_D = TurnD;
     
168.    G_RATIO = GYROSCOPE_ANGLE_RATIO*100;
     
169.    LCD_CLS();
     
170.    dispage1(); //正常显示
     
171.    while(1)
     
172.    {
     
173.      if(workflag==0)  //正常状态
     
174.      { if(gpio_get(PORTE,3)==0)  //状态切换
     
175.        { delayms(4);
     
176.          if(gpio_get(PORTE,3)==0)
     
177.          {
     
178.            workflag=1;
     
179.            LCD_CLS(); //清屏
     
180.            dispage2();
     
181.            while(gpio_get(PORTE,3)==0); //等待按键松开
     
182.          }
     
183.        }
     
184.        discnt++;
     
185.        if(discnt>=100)
     
186.        { discnt=0;
     
187.          disdata1();
     
188.          delayms(2);
     
189.        }
     
190.        delayms(1);
     
191.      }
     
192.      else if(workflag==1)   //显示和上传CCD图像
     
193.      { if(ccd_flag==1)//采完一副图像
     
194.        {
     
195.          ccd_flag=0;
     
196.          tuxiang();  //红树伟业上位机
     
197.          discnt++;
     
198.          if(discnt>=100)
     
199.          { discnt=0;
     
200.            disdata2();  //显示图像
     
201.            disccdval();
     
202.            delayms(1);
     
203.          }
     
204.        }
     
205.        if(gpio_get(PORTE,3)==0)  //状态切换
     
206.        { delayms(4);
     
207.          if(gpio_get(PORTE,3)==0)
     
208.          {
     
209.            workflag=2;
     
210.            LCD_CLS(); //清屏
     
211.            dispage3();
     
212.            while(gpio_get(PORTE,3)==0); //等待按键松开
     
213.          }
     
214.        }
     
215.        if(gpio_get(PORTE,2)==0)  //回正常状态
     
216.        { delayms(4);
     
217.          if(gpio_get(PORTE,2)==0)
     
218.          {
     
219.            workflag=0;
     
220.            LCD_CLS(); //清屏
     
221.            dispage1();
     
222.            while(gpio_get(PORTE,5)==0); //等待按键松开
     
223.          }
     
224.        }
     
225.      }
     
226.      else if(workflag==2)  //调节参数
     
227.      { if(gpio_get(PORTE,3)==0)  //修改参数变换
     
228.        { delayms(4);
     
229.          if(gpio_get(PORTE,3)==0)
     
230.          {
     
231.            paraadjflag++;
     
232.            if(paraadjflag==5) paraadjflag=0;  // 切换。如果有更多的参数组，自行修改
     
233.            if(paraadjflag==0)
     
234.            { LCD_P6x8Str(0,1,"Speed Para:  <--");
     
235.              LCD_P6x8Str(0,3,"Turn Para:      ");
     
236.              LCD_P6x8Str(0,5,"Speed Set:      ");
     
237.            }
     
238.            else if(paraadjflag==1)
     
239.            { LCD_P6x8Str(0,1,"Speed Para:  -->");
     
240.              LCD_P6x8Str(0,3,"Turn Para:      ");
     
241.              LCD_P6x8Str(0,5,"Speed Set:      ");
     
242.            }  
     
243.            else if(paraadjflag==2)
     
244.            { LCD_P6x8Str(0,1,"Speed Para:     ");
     
245.              LCD_P6x8Str(0,3,"Turn Para:   <--");
     
246.              LCD_P6x8Str(0,5,"Speed Set:      ");
     
247.            }
     
248.            else if(paraadjflag==3)
     
249.            { LCD_P6x8Str(0,1,"Speed Para:     ");
     
250.              LCD_P6x8Str(0,3,"Turn Para:   -->");
     
251.              LCD_P6x8Str(0,5,"Speed Set:      ");
     
252.            }
     
253.            else if(paraadjflag==4)
     
254.            { LCD_P6x8Str(0,1,"Speed Para:     ");
     
255.              LCD_P6x8Str(0,3,"Turn Para:      ");
     
256.              LCD_P6x8Str(0,5,"Speed Set:   <--");
     
257.            }
     
258.            while(gpio_get(PORTE,3)==0); //等待按键松开
     
259.          }
     
260.        }
     
261.        if(gpio_get(PORTE,2)==0)  //回正常状态
     
262.        { delayms(4);
     
263.          if(gpio_get(PORTE,2)==0)
     
264.          {
     
265.            workflag=0;
     
266.            LCD_CLS(); //清屏
     
267.            dispage1();
     
268.            while(gpio_get(PORTE,2)==0); //等待按键松开
     
269.          }
     
270.        }
     
271.        if(gpio_get(PORTE,0)==0)  //参数调节
     
272.        { delayms(4);
     
273.          if(gpio_get(PORTE,0)==0)
     
274.          { if(paraadjflag==0)   //速度KP 加减
     
275.            { SpeedKP++;
     
276.              Dis_num(24,2,SpeedKP);
     
277.            }
     
278.            else if(paraadjflag==1)
     
279.            { SpeedKI++;
     
280.              Dis_num(90,2,SpeedKI);         
     
281.            }
     
282.            else if(paraadjflag==2)
     
283.            { turnsetmax=turnsetmax+5;
     
284.              Dis_num(24,4,turnsetmax);         
     
285.            }
     
286.            else if(paraadjflag==3)
     
287.            { TurnD++;
     
288.              Dis_num(90,4,TurnD);         
     
289.            }
     
290.            else if(paraadjflag==4)  //速度
     
291.            { SetSpeed = SetSpeed - 5;
     
292.              AmSpeed = SetSpeed;
     
293.              Dis_num(24,6,SetSpeed);
     
294.            }
     
295.            while(gpio_get(PORTE,0)==0); //等待按键松开
     
296.          }
     
297.        }
     
298.        if(gpio_get(PORTE,1)==0)  //参数调节
     
299.        { delayms(4);
     
300.          if(gpio_get(PORTE,1)==0)
     
301.          {
     
302.            if(paraadjflag==0)   //速度KP 加减
     
303.            { SpeedKP--;
     
304.              Dis_num(24,2,SpeedKP);
     
305.            }
     
306.            else if(paraadjflag==1)
     
307.            { SpeedKI--;
     
308.              Dis_num(90,2,SpeedKI);         
     
309.            }
     
310.            else if(paraadjflag==2)
     
311.            { turnsetmax=turnsetmax-5;
     
312.              Dis_num(24,4,turnsetmax);         
     
313.            }
     
314.            else if(paraadjflag==3)
     
315.            { TurnD--;
     
316.              Dis_num(90,4,TurnD);         
     
317.            }
     
318.            else if(paraadjflag==4)  //速度
     
319.            { SetSpeed = SetSpeed + 5;
     
320.              Dis_num(24,6,SetSpeed);
     
321.              AmSpeed = SetSpeed;
     
322.            }
     
323.            while(gpio_get(PORTE,1)==0); //等待按键松开
     
324.          }
     
325.        }
     
326.        delayms(5);
     
327.      }
     
328.    }
     
329. }
     
330. 
     
331. void GPIO_Init()
     
332. {
     
333.    gpio_init (PORTA, 17, GPO, 1u); //初始化PTE0为高电平输出---LED0
     
334.    gpio_set  (PORTA, 19, 0);       //设置PTE0为高电平输出，LED0灭
     
335.    gpio_init (PORTE,0,GPI,0);      //按键输入
     
336.    gpio_init (PORTE,1,GPI,0);     //按键输入
     
337.    gpio_init (PORTE,2,GPI,0);     //按键输入
     
338.    gpio_init (PORTE,3,GPI,0);     //按键输入   
     
339.    
     
340. }
     
341. 
     
342. void run()//直立函数 在isr.c中的1ms中断中调用  PIT0_IRQHandler
     
343. {   
     
344.   TimeCount++ ;
     
345.   SpeedPeriodCount++;
     
346.   TurnPeriodCount ++ ;
     
347.   AAngPeriodCount ++ ;
     
348.   if(AAngPeriodCount>=4) AAngPeriodCount=4;
     
349.   
     
350.   MotorTurnPWM  = TurnPWMOut(TurnPWMOUT,LastTurnPWMOUT,TurnPeriodCount) ;
     
351.   MotorSpeedPWM = SpeedPWMOut(g_SpeedControlOutNew ,g_SpeedControlOutOld,SpeedPeriodCount);  
     
352.   MotorAAngPWM  = AAangPWMOut(AAngPWM ,LastAAngPWM ,AAngPeriodCount);  
     
353.   Checkcarstate();//开启停止判断
     
354.   if(TimeCount>=5)//读速度  5ms
     
355.   {
     
356.     TimeCount=0;
     
357.     GetMotorPulse();//读速度脉冲
     
358.   }  
     
359.   else if(TimeCount == 1)//读取MPU6050
     
360.   {      
     
361.      xaout=MPU6050_GetDoubleData(MPU6050_ACCEL_XOUT);//读X轴加速度
     
362.      yaout=MPU6050_GetDoubleData(MPU6050_ACCEL_YOUT);//读X轴加速度
     
363.      zaout=MPU6050_GetDoubleData(MPU6050_ACCEL_ZOUT);//读X轴加速度
     
364.      xgout=MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);//读X轴角速度,直立角速度
     
365.      ygout=MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);//读Y轴角速度，转向角速度
     
366.      angle6550=MPU6050_Get_Angle(xaout,yaout,zaout,0);
     
367.   }
     
368.   else if(TimeCount == 2)//5ms 直立滤波，控制
     
369.   {
     
370.     AAngPeriodCount  = 0;
     
371.     AngleCalculate();//计算加速值和角度值
     
372.     LastAAngPWM = AAngPWM ;      
     
373.     AAngPWM = AngleControl() ;     //计算平衡电机速度
     
374.     PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//电机输出
     
375.     //PWMout=MotorSpeedOut(0,0,MotorTurnPWM);
     
376.     //PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,0);//电机输出  刚开始用的时候可以把MotorSpeedPWM和MotorTurnPWM都设为0
     
377.      //PWMout=MotorSpeedOut(AAngPWM,0,0);//电机输出  刚开始用的时候可以把MotorSpeedPWM和MotorTurnPWM都设为0
     
378.     //先调直立，再加速度MotorSpeedPWM，最后加方向MotorTurnPWM
     
379.    
     
380.   }
     
381.   else if(TimeCount == 3)//5ms  速度PI调节
     
382.   {
     
383.      ATimeCount ++ ;
     
384.      if(ATimeCount >= 20)//20*5=100ms进行一次速度PID调节
     
385.      {
     
386.        ATimeCount=0;
     
387.        SpeedPID() ;
     
388.        SpeedPeriodCount = 0 ;
     
389.        qibu();//起步  不可以一下就把速度加上，要缓慢加
     
390.      }
     
391.    }
     
392.   else if(TimeCount == 4)// CCD图像采集与处理
     
393.   {
     
394.     ccd_count++;
     
395.     if(ccd_count>=4)//20ms进一次
     
396.     {
     
397.         ccd_count=0;
     
398.         ccd_flag=1;//发送图像到上位机标志
     
399.         ImageCapture(Pixel) ;//将捕捉到的图像放在Pixel数组中
     
400.         //process();//图像处理函数
     
401.         FindLine();
     
402.         dline = rightline - leftline;
     
403.         if((dline>64)||(dline<12)) //十字路或桥上
     
404.         { //TurnP = TurnP1;
     
405.           //TurnD = TurnD1;
     
406.           turnpwmmax=turnsetmax;
     
407.         }
     
408.         else if((dline>(whitelength+16))||(dline<(whitelength-16))) //丢线
     
409.         { turnpwmmax=turnsetmax+50;
     
410.           //TurnD = TurnD4;
     
411.         }
     
412.         else if((dline>(whitelength+9))||(dline<(whitelength-9))) //60度弯道
     
413.         { //TurnP = TurnP4;
     
414.           //TurnD = TurnD4;
     
415.           turnpwmmax=turnsetmax+30;
     
416.         }
     
417.         else if((dline>(whitelength+4))||(dline<(whitelength-4))) //50度弯道
     
418.         { //TurnP = TurnP3;
     
419.           //TurnD = TurnD3;
     
420.           turnpwmmax=turnsetmax+20;
     
421.         }
     
422.          else if((dline>(whitelength+2))||(dline<(whitelength-2))) //亚直道
     
423.         { //TurnP = TurnP2;
     
424.           //TurnD = TurnD2;
     
425.           turnpwmmax=turnsetmax;
     
426.         }
     
427.         else //直道
     
428.         { //TurnP = TurnP1;
     
429.           //TurnD = TurnD1;
     
430.           turnpwmmax=turnsetmax;
     
431.         }
     
432.         LastTurnPWMOUT = TurnPWMOUT ;
     
433.         TurnPWM() ;
     
434.         TurnPeriodCount = 0 ;
     
435.     }
     
436.   }  
     
437.   if(TIME1flag_1s == 1)//程序运行灯
     
438.   {
     
439.     TIME1flag_1s = 0 ;
     
440.     PTA17_OUT = ~PTA17_OUT ;
     
441.   }
     
442. }
     
443. void qibu()//这一部分认真看一下，也可以根据自己的想法改一下
     
444. {
     
445.   if(AmSpeed!=0)//在目标速度不为0时才运行
     
446.   {
     
447.    start_flag++;
     
448.    if(start_flag<30)//静止2S才出发  可根据规则修改
     
449.    {  g_Speedgoal=0;
     
450.       //SpeedKI=0;
     
451.    }
     
452.    else if(start_flag>=30)
     
453.    {
     
454.      if(g_Speedgoal>AmSpeed&&stop_jiasu==0)//然后逐渐加速
     
455.      {
     
456.          g_Speedgoal-=15; //  30  //单次加速值，值大加速能力强，比赛前可设两组值，一组快速加速，用于应对起步时是长
     
457.                          //直道的情况，另一组为慢加速，用于应对起步时就是弯道的情况
     
458.      }
     
459.      else if(g_Speedgoal<=AmSpeed&&stop_jiasu==0) //内部速度高于设置速度，停止大幅度加速，进行小幅度调整加速
     
460.      {
     
461.          g_Speedgoal=AmSpeed;
     
462.          stop_jiasu=1;
     
463.      }                                
     
464.      start_flag=3000;  //终止静止2s计数
     
465.      //SpeedKI=0;
     
466.      if(stop_jiasu==1)        //车模之后的目标速度调整主要在此函数中
     
467.      {
     
468.        if(g_Speedgoal>AmSpeed)//然后逐渐加速
     
469.        {
     
470.          g_Speedgoal-=15;//
     
471.        }
     
472.         else if(g_Speedgoal<=AmSpeed)
     
473.        {
     
474.          g_Speedgoal=AmSpeed;
     
475.        }   
     
476.      }
     
477.    }
     
478.   }
     
479. }
     
480. 
     
481. 
     
482. void dispage1()
     
483. {  Dis_num(0,0,GRAVITY_OFFSET);  //设定值，垂直时加速度AD
     
484.    Dis_num(42,0,GYROSCOPE_OFFSET);//陀螺仪零偏  自检平均值
     
485.    LCD_P6x8Str(0,1,"Ap:");
     
486.    Dis_num(20,1,A_P);//加计零偏 定值
     
487.    LCD_P6x8Str(60,1,"Ad:");
     
488.    Dis_num(80,1,A_D);
     
489.    
     
490.    LCD_P6x8Str(0,2,"Vp:");
     
491.    Dis_num(20,2,V_P);
     
492.    LCD_P6x8Str(60,2,"Vd:");
     
493.    Dis_num(80,2,V_I);
     
494.    
     
495.    LCD_P6x8Str(0,3,"Tp:");
     
496.    Dis_num(20,3,T_P);
     
497.    LCD_P6x8Str(60,3,"Td:");
     
498.    Dis_num(80,3,T_D);
     
499.    
     
500.    LCD_P6x8Str(0,4,"A      S      C");
     
501.    
     
502.    LCD_P6x8Str(0,5,"Spd");
     
503.    LCD_P6x8Str(1,7,"LMB:");
     
504.    LCD_P6x8Str(64,7,"RMB:");
     
505. }
     
506. 
     
507. void disdata1()
     
508. {    Dis_num(6,4,angletmp);
     
509.      Dis_num(48,4,-AmSpeed);
     
510.      Dis_num(90,4,TurnPosition);
     
511.      A_P = ANGLE_CONTROL_P;
     
512.      A_D = ANGLE_CONTROL_D*10;
     
513.      V_P = SpeedKP;
     
514.      V_I = SpeedKI;  
     
515.      T_P = TurnP;
     
516.      T_D = TurnD;  
     
517.      Dis_num(20,1,A_P);//加计零偏 定值
     
518.      //Dis_num(80,1,A_D);
     
519.      Dis_num(80,1,TurnPWMOUT);
     
520.      Dis_num(20,2,V_P);
     
521.      Dis_num(80,2,V_I);
     
522.      Dis_num(20,3,T_P);
     
523.      Dis_num(80,3,T_D);
     
524.      delayms(50);
     
525.      Dis_num(10,6,leftlineold);  //赛道左边界
     
526.      Dis_num(94,6,rightlineold); //赛道右边界
     
527.      Dis_num(52,6,dline); //赛道右-左
     
528.      Dis_num(28,7,sumlspeed);
     
529.      Dis_num(96,7,sumrspeed);
     
530.      Dis_num(20,5,AmSpeed);
     
531.      Dis_num(80,5,turnsetmax);
     
532. }
     
533. 
     
534. void dispage2()
     
535. {  LCD_P6x8Str(0,0,"CCD DATA UP&DIS");
     
536.    LCD_P6x8Str(0,1,"L:        R:");
     
537.    Dis_num(20,1,leftlineold);  //赛道左边界
     
538.    Dis_num(94,1,rightlineold); //赛道右边界
     
539. }
     
540. 
     
541. void disdata2()
     
542. {  Dis_num(20,1,leftlineold);  //赛道左边界
     
543.    Dis_num(94,1,rightlineold); //赛道右边界
     
544. }
     
545. ……………………
     
546. 
     
547. …………限于本文篇幅 余下代码请从51黑下载附件…………
     

复制代码

所有资料51hei提供下载:
K60_CCD直立.rar (13.35 MB, 下载次数: 55)

2018-5-10 21:07 上传

点击文件名下载附件
下载积分: 黑币 -5

---

作者: admin    时间: 2018-5-12 06:08
好资料，51黑有你更精彩!!!

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1