标题: MC9S12XS128智能小车测试源程序 [打印本页]

---

作者: lxw15385989805    时间: 2018-4-12 15:58
标题: MC9S12XS128智能小车测试源程序
s128智能小车测试程序源码


单片机源程序如下:

1. #include <hidef.h>      /* common defines and macros */
   
2. #include <MC9S12XS128.h>     /* derivative information */
   
3. #include "math.h"
   
4. #pragma LINK_INFO DERIVATIVE "mc9s12xs128"
   
5. 
   
6. 
   
7. /*********************DEFINE************************/
   
8. #define  WHITE              50
   
9. #define  BLACK              40        
   
10. #define  KD                  0                  
    
11. #define  EnableSCIReInt     SCI0CR2 |= 0x20     
    
12. #define  DisableSCIReInt    SCI0CR2 &= 0xDF
    
13. #define  RTI_SWITCH_ON      CRGINT_RTIE=1
    
14. #define  RTI_SWITCH_OFF     CRGINT_RTIE=0
    
15. #define  IC_SWITCH_ON       TSCR1_TEN=1                                
    
16. #define  IC_SWITCH_OFF      TSCR1_TEN=0
    
17. //-----------------define key---------------------//
    
18. #define  KEY_RUN_OR_LCD     PORTK_PK0
    
19. #define  KEY_START_RUN      PORTA_PA1
    
20. #define  KEY_UP             PORTA_PA2
    
21. #define  KEY_DOWN           PORTA_PA3
    
22. #define  KEY_ENTER          PORTA_PA4
    
23. //-----------------part of lcd---------------------//
    
24. #define         LINE1                                              0
    
25. #define         LINE2                                              1
    
26. #define         LINE1_HEAD                                  0x80
    
27. #define         LINE2_HEAD                                  0xC0
    
28. #define         DATA_MODE                                    0x38        // 8 bit data transmission mode, double line
    
29. #define         CLR                                                      0x01
    
30. #define                BUSY                                               0x80                     // LCD Busy Tag
    
31. #define   CURSOR_RESET        0x02
    
32. #define   INPUTMODE_CUR_R     0x06
    
33. #define   INPUTMODE_CUR_L     0x04
    
34. #define   INPUTMODE_ALL_E     0x05
    
35. #define   INPUTMODE_ALL_D     0x04
    
36. #define         SCREEN_OPEN                                 0x0C
    
37. #define         SCREEN_OPEN_CUR                        0x02
    
38. #define         SCREEN_OPEN_FLASH                0x01
    
39. #define   SCSHIFT_CUR_L       0x10        // Screen or Cursor shift
    
40. #define   SCSHIFT_CUR_R       0x14
    
41. #define   SCSHIFT_SCR_L       0x18
    
42. #define   SCSHIFT_SCR_R       0x1C
    
43. 
    
44. //-----------------define lcd pins--------------------//
    
45. #define   RS                  PORTA_PA5         //Data Command Pin                1 data                0 command
    
46. #define   RW                  PORTA_PA6         //Read Write Pin                  1 read                 0 write
    
47. #define   EN                  PORTA_PA7         //LCD Enable Signal
    
48. #define   LCD_DATA            PTH
    
49. #define   LCD_DATA_DIR        DDRH
    
50. #define   LCD_DIR_IN          0x00
    
51. #define   LCD_DIR_OUT         0xff
    
52. /*********************END************************/
    
53. 
    
54. 
    
55. 
    
56. /***************HARDWARE INIT FUN DECLARE******************/
    
57. void uart_init(void);
    
58. extern void uart_PutChar(unsigned char);
    
59. unsigned char TERMIO_GetChar(void);
    
60. void printp( char * , ...);  
    
61. void CPU_Init(void);
    
62. void IO_Init(void);
    
63. void SciTx(unsigned char text);
    
64. char SciRx(void);                              
    
65. void RTI_Init(void);
    
66. void IC_Init(void);
    
67. void OC7_Init(void);
    
68. void PTI0_Init(void);
    
69. void PWM_Init(void);
    
70. void AD_Init(void);
    
71. //-----------------part of lcd---------------------//
    
72. void LcdCommand(unsigned char command,unsigned char BusyC);
    
73. void LcdData(unsigned char temp,unsigned char BusyC);               
    
74. void LcdInit(void);                                                                                          
    
75. unsigned char ReadStatus(void);                        
    
76. void GotoXY( unsigned char x, unsigned char y );                       
    
77. void Print(unsigned char *str);
    
78. void Lcd_Print_Str(unsigned char x,unsigned char y,char *s);   
    
79. void Lcd_Print_Int( unsigned char x,unsigned char y,unsigned int data);
    
80. void Lcd_Print_Char( unsigned char x,unsigned char y,unsigned char data);
    
81. void Lcd_Print_uChar( unsigned char x,unsigned char y,char data);
    
82. /*********************END************************/
    
83.             
    
84.             
    
85.                              
    
86. /***************SOFTWARE FUN DECLARE******************/
    
87. void light_flip(void);
    
88. void AD_DATA_HIGHT(void);
    
89. void AD_DATA_LOW(void);
    
90. void VOL_Normaliz(void);
    
91. void err_calculate_hight(void);
    
92. void err_calculate_low(void);
    
93. signed int PID(void);
    
94. void key_scan_deal(void);
    
95. void LCD_DISPLAY(void);
    
96. uint err_Std(void);
    
97. void check_line(void);
    
98. void LcdDelay(byte k);
    
99. void Dly_ms(int ms);
    
100. void delay(unsigned int ref);
     
101. /*********************END************************/
     
102. 
     
103. 
     
104. 
     
105. /*********************DEFINE VAR************************/
     
106. //---------------sensor data var-----------------//                              
     
107. uchar THRESHOLD_black=40;   
     
108. uchar max_vol[9],min_vol[9]={255,255,255,255,255,255,255,255,255};
     
109. uchar WHITE_MIN=240,WHITE_MAX=250;
     
110. uchar percent[9],data[9],det_vol[9],flip_data[9];
     
111. schar err[100],err_e[3];
     
112. uchar L_times=130;
     
113. byte  flip=0,max_num_p=0;
     
114. 
     
115. //---------------sensor_2 data var-----------------//                              
     
116. uchar data_2[5],THRESHOLD_2=200,flip_data_2[5],err_e2[3];
     
117. 
     
118. //-----------------speed var---------------------//                             
     
119. byte  load_bit=0;
     
120. uchar pluse_1=0,pluse_2=0,get_speed=0,set_speed=30,now_speed=0;
     
121. uchar MAX_SPEED=30;
     
122. 
     
123. //-----------------check var-------------------- //
     
124. uint  check_pluse=0;
     
125. uchar start_line=0,stop=0,stop_delay=0,slope_time=0,slope_timing=0;
     
126. byte  check=0,check_bit=0,line_num=0,triangle_status=0,slope_bit=0,black_bit=0;
     
127. 
     
128. 
     
129. //------------------key&lcd var----------------------//
     
130. byte key_r_o_l=0,lcd_print=1;
     
131. uchar menu=1,menu_option=1,bits=0;
     
132. 
     
133. //----------------- calc var---------------------//
     
134. uchar KP=8,period_1=0,period_2=0;
     
135. char  std_err[10];
     
136. int  temp_std;
     
137. 
     
138. /*********************END************************/
     
139. 
     
140. 
     
141. 
     
142. void main()        
     
143. {                  
     
144.     DisableInterrupts;
     
145.     CPU_Init();
     
146.     IO_Init();                                       
     
147.     uart_init();
     
148.     //EnableSCIReInt;
     
149.     LcdInit();         
     
150.     PWM_Init();                        
     
151.     AD_Init();
     
152.     VOL_Normaliz();
     
153.     Dly_ms(3000);
     
154.     Dly_ms(2000);
     
155.     RTI_Init();
     
156.     IC_Init();  
     
157.     for(;;)
     
158.     {
     
159.       EnableInterrupts;
     
160.       //err_calculate_hight();
     
161.       key_scan_deal();
     
162.     }
     
163. }
     
164. 
     
165. 
     
166. //--------------------------------------------------------------
     
167. //                    HARDWARE FUN START                        
     
168. //--------------------------------------------------------------
     
169. 
     
170. void CPU_Init()
     
171. {
     
172.   CLKSEL=0X00;
     
173.   PLLCTL_PLLON=1;
     
174.   REFDV=0x01;   //BUS_CLOCK = 40M
     
175.   SYNR=0x04;
     
176.   _asm(nop);
     
177.   _asm(nop);
     
178.   while(!(CRGFLG_LOCK==1));
     
179.   CLKSEL_PLLSEL=0x01;  
     
180.   FCLKDIV=0x0F;//Flash Clock Divide Factor 40M/16=?M
     
181. }
     
182. 
     
183. void SciTx(unsigned char text)
     
184. {
     
185.         unsigned char temp;
     
186.         temp=SCI0SR1;      /*clear flag*/
     
187.         while (!(SCI0SR1&0x80));  /* wait for output buffer empty */
     
188.         SCI0DRH=0;
     
189.         SCI0DRL=text;
     
190. }
     
191. 
     
192. char SciRx(void)
     
193. {
     
194.         unsigned char result,temp;
     
195.         temp=SCI0SR1;      /*clear flag*/
     
196.         while(!(SCI0SR1&0x20));
     
197.         result=SCI0DRL;
     
198.         return result;
     
199. }
     
200. 
     
201. void RTI_Init()
     
202. {              
     
203.           RTICTL=0b10010011;     //0010000=2*10^3,10010011=8*10^3=0.5ms,10001111=16*10^3=1ms,11000111=160x10^3=10ms ,11101111=1.6*10^6=100ms
     
204.           //CRGINT_RTIE=1;  
     
205. }
     
206. 
     
207. void IC_Init(){
     
208.   DisableInterrupts;
     
209.   TIOS_IOS0=0;     //channel 0 input capture
     
210.   TCTL4_EDG0B=1;
     
211.   TCTL4_EDG0A=1;
     
212.   TIE_C0I=1;      //channel 0 interrupt enable
     
213.   TSCR2=0x03;     //TIMEclock = 40M / 8 =0.2us
     
214.   TFLG1        =        0xFF;
     
215.   TFLG2        =        0x80;
     
216.   //TSCR1_TEN=1;   //Timer enable
     
217.   EnableInterrupts;
     
218. }
     
219. 
     
220. 
     
221. void OC7_Init()
     
222. {
     
223.    TSCR2_TCRE=1;
     
224.    TSCR2_PR=0;  
     
225.    TIOS_IOS7=1;
     
226.    TC7=0x5dc0;  //24000,1ms
     
227.    TCTL1=0x00;                    
     
228.    TIE_C7I=1;  
     
229.    TSCR1_TEN  = 1;     
     
230. }
     
231. 
     
232. /*
     
233. void PTI0_Init()   
     
234. {
     
235.   PITCFLMT=0x00;
     
236.   PITCE_PCE0=1;
     
237.   PITMUX_PMUX0=0;
     
238.   PITMTLD0=0x01;
     
239.   PITLD0=0x0001;    //2x2x25ns=1us;
     
240.   PITINTE_PINTE0=1;
     
241.   PITCFLMT_PITE=1;
     
242. }
     
243. */
     
244. 
     
245. void PWM_Init()
     
246. {
     
247.   PWME=0xaa;        //enable c_1,c_3,c_5,c_7
     
248.   PWMPOL=0xaa;      
     
249.   PWMCAE=0X00;
     
250.   PWMCTL=0xf0;      //com 0&1 2&3 4&5
     
251.   PWMPRCLK=0x00;
     
252.   PWMCLK_PCLK1=1;      //0&1---SA
     
253.   PWMSCLA=0x04;     //SA = 5M
     
254.   PWMPER01=50000;   //舵机: 5000000/50000=100Hz ;
     
255.   PWMDTY01=7100;
     
256.   
     
257.   PWMPER23=5000;    //电机: 24000000/12000=2000Hz ;24000000/2400=10000Hz ;40000000/5000=8000Hz
     
258.   PWMDTY23=2500;
     
259.   PWMPER45=5000;   
     
260.   PWMDTY45=2500;
     
261. }
     
262. 
     
263. void AD_Init()
     
264. {
     
265.   //[3:0]---WRAP  
     
266.   //ATD0CTL0=0x00;
     
267.   
     
268.   //7---ETRIGSEL, [6:5]---Resolution Select, 4---Discharge, [3:0]---External Trigger Channel Select
     
269.   ATD0CTL1=0x00;
     
270.    
     
271.   //6---AFFC;5---ICLKSTP;  [4:3]---External Trigger Edge/Polarity;  2---ETRIGE;
     
272.   //1---ASCIE,Sequence Complete Interrupt Enable;  0---ACMPIE,Compare Interrupt Enable
     
273.   ATD0CTL2=0x40;
     
274.   
     
275.   //7---DJM,Data Justification; [6:3]---S8C,S4C,S2C,S1C,Sequence Length;2---FIFO;
     
276.   //[1:0]---Background Debug Freeze Enable;   
     
277.   ATD0CTL3=0x98;
     
278.         
     
279.   //[7:5]---SMP,Sample Time Select; [4:0]---PRS,fATDCLK=fBUS/2/(PRS + 1)
     
280.   ATD0CTL4=0x01;  
     
281.   
     
282.   //6---SC5,Special Channel Conversion; 5---SCAN,Continuous Conversion Sequence Mode
     
283.   //4---MULT,Multi-Channel Sample Mode; [3:0]---Analog Input Channel Select Code CD,CC,CB,CA
     
284.   //ATD0CTL5=0x01;
     
285.   
     
286.   //ATD0CMPE_CMPE0=1;
     
287.   //ATD0CMPHT_CMPHT0=0; //>AD0
     
288.   //ATD0DR0L=0x000f;   
     
289.   ATD0DIEN=0x00;  
     
290. }
     
291. 
     
292. void IO_Init(){
     
293.    // SENSOR
     
294.    DDRB=0xff;
     
295.    PORTB=0x00;
     
296.    DDRA=0x01;
     
297.    PORTA_PA0=0;
     
298.    // KEY DEFINE
     
299.    DDRK_DDRK0 = 0;
     
300.    DDRA_DDRA1 = 0;
     
301.    DDRA_DDRA2 = 0;
     
302.    DDRA_DDRA3 = 0;
     
303.    DDRA_DDRA4 = 0;
     
304.    DDRK_DDRK1 = 0;
     
305.    DDRK_DDRK2 = 0;
     
306.    DDRK_DDRK3 = 0;
     
307.    // LCD CONTORL
     
308.    DDRA_DDRA5 = 1;
     
309.    DDRA_DDRA6 = 1;
     
310.    DDRA_DDRA7 = 1;
     
311.    //LED
     
312.    DDRK_DDRK4 = 1;
     
313.    DDRK_DDRK5 = 1;
     
314.    PORTK_PK4 = 0;
     
315.    PORTK_PK5 = 0;
     
316.    //sensor_2
     
317.    DDRK_DDRK7 = 1;
     
318.    PORTK_PK7 = 0;
     
319. }
     
320. 
     
321. void Dly_ms(int ms)
     
322. {
     
323.    int ii,jj;
     
324.    if (ms<1) ms=1;
     
325.    for(ii=0;ii<ms;ii++)
     
326.      for(jj=0;jj<2670;jj++);    //busclk:16MHz--1ms
     
327. }
     
328.                                     
     
329. void delay(unsigned int ref){
     
330.   uint kk;
     
331.   for(kk=0;kk<ref;kk++){
     
332.     asm nop;
     
333.   }
     
334. }
     
335. 
     
336. /************************LCD drive start*************************/
     
337. void LcdInit(){   
     
338.     LCD_DATA_DIR = LCD_DIR_OUT;   
     
339.     LcdDelay(21);
     
340.     LcdCommand(DATA_MODE,1);    //set 8 bit data transmission mode
     
341.     LcdDelay(7);
     
342.     LcdCommand(DATA_MODE,1);    //set 8 bit data transmission mode
     
343.     LcdDelay(7);
     
344.     LcdCommand(DATA_MODE,1);    //set 8 bit data transmission mode
     
345.            
     
346.     LcdCommand(CLR,0);          //clear screen   
     
347.     LcdCommand(DATA_MODE,1);    //set 8 bit data transmission mode   
     
348.     LcdCommand(INPUTMODE_CUR_R | INPUTMODE_ALL_D, 1);   // cursor right, disable moving   
     
349.     LcdCommand(SCREEN_OPEN, 1);                     //open display (enable lcd display)   
     
350.     LcdCommand(LINE1_HEAD,1);   //set lcd first display address   
     
351.     LcdCommand(CLR,1);          //clear screen   
     
352. }
     
353.    
     
354. unsigned char ReadStatus(void)   
     
355. {   
     
356.     unsigned char cRtn;   
     
357.     LCD_DATA_DIR = LCD_DIR_IN;   
     
358.     RS = 0;   
     
359.     RW = 1;   
     
360.     EN = 0;
     
361.     delay(500);   
     
362.     EN = 1;   
     
363.     while (LCD_DATA & BUSY); //Test Busy State   
     
364.     cRtn = LCD_DATA_DIR;     // if Not save the port value, it should be change   
     
365.     LCD_DATA_DIR = LCD_DIR_OUT;   
     
366.     return(cRtn);   
     
367. }   
     
368.      
     
369. void LcdCommand(unsigned char command,unsigned char BusyC){   
     
370.     if (BusyC)
     
371.     ReadStatus(); //Test it busy or not   
     
372.     LCD_DATA=command;   
     
373.     RS=0;      
     
374.     RW=0;   
     
375.     EN=0;   
     
376.     delay(500);   
     
377.     EN=1;   
     
378. }
     
379. 
     
380. void LcdData(unsigned char dat,unsigned char BusyC)   
     
381. {   
     
382.     if (BusyC)
     
383.     ReadStatus(); //Test it busy or not   
     
384.     LCD_DATA=dat;   
     
385.     RS=1;   
     
386.     RW=0;   
     
387.     EN=0;   
     
388.     delay(500);   
     
389.     EN=1;   
     
390. }  
     
391.    
     
392. void LcdDelay(byte k){      //k=7=5ms   
     
393.     uint i, j;   
     
394.     for (i = 0; i < k; i++)   
     
395.       for(j = 0; j < 4780; j++);   
     
396. }      
     
397. 
     
398. void GotoXY( unsigned char x, unsigned char y ){   
     
399.     unsigned char address;   
     
400.     if (y == LINE1)   
     
401.     address = LINE1_HEAD + x;   
     
402.     else   
     
403.     address = LINE2_HEAD + x;   
     
404.     LcdCommand(address,1);   
     
405. }
     
406. 
     
407. void Lcd_Print_Int( unsigned char x,unsigned char y,unsigned int data){   
     
408.     char i=0;
     
409.           unsigned char a[5];
     
410.           a[0]=(data/10000)%10+'0';                
     
411.           a[1]=(data/1000)%10+'0';                                            
     
412.           a[2]=(data/100)%10+'0';                                             
     
413.           a[3]=(data/10)%10+'0';                                              
     
414.           a[4]=data%10+'0';
     
415.           for(i=0; i<5; i++)                     
     
416.           {
     
417.             GotoXY(i+x, y);
     
418.             LcdData(a[i],1);   
     
419.           }      
     
420. }
     
421. 
     
422. void Lcd_Print_Char( unsigned char x,unsigned char y,unsigned char data)
     
423. {   
     
424.     char j=0;
     
425.           unsigned char a[3];
     
426.           a[0]=(data/100)%10+'0';                
     
427.           a[1]=(data/10)%10+'0';                                            
     
428.           a[2]=data%10+'0';                                             
     
429.           for(j=0; j<3; j++)                     
     
430.           {
     
431.             GotoXY(j+x, y);
     
432.             LcdData(a[j],1);   
     
433.           }      
     
434. }
     
435. 
     
436. void Lcd_Print_uChar( unsigned char x,unsigned char y,char data)
     
437. {   
     
438.     char j=0,k=0;
     
439.     unsigned char a[3];
     
440.     if(data>=0){      
     
441.             a[0]=(data/100)%10+'0';                
     
442.             a[1]=(data/10)%10+'0';                                            
     
443.             a[2]=data%10+'0';                                             
     
444.             for(j=0; j<3; j++)                     
     
445.             {
     
446.               GotoXY(j+x, y);
     
447.               LcdData(a[j],1);   
     
448.             }
     
449.     }
     
450.     else{
     
451.       data=~data;
     
452.       data++;
     
453.       a[0]=(data/100)%10+'0';                
     
454.             a[1]=(data/10)%10+'0';                                            
     
455.             a[2]=data%10+'0';
     
456.             Lcd_Print_Str(x,y,"-");                                             
     
457.             for(k=0; k<3; k++)                     
     
458.             {
     
459.               GotoXY(k+x+1, y);
     
460.               LcdData(a[k],1);   
     
461.             }
     
462.     }
     
463. }
     
464. 
     
465. void Lcd_Print_Str(unsigned char x,unsigned char y,char *s)   
     
466. {   
     
467.     GotoXY(x,y); //set address   
     
468.     while(*s!='\0')  // write character   
     
469.     {   
     
470.       LcdData(*s, 1);      
     
471.       s++;   
     
472.     }   
     
473. }
     
474. /************************LCD drive end***************************/
     
475. 
     
476. //--------------------------------------------------------------
     
477. //                    HARDWARE FUN END                          
     
478. //--------------------------------------------------------------
     
479. 
     
480. 
     
481. //--------------------------------------------------------------
     
482. //                    STARTEGY FUN START                        
     
483. //--------------------------------------------------------------
     
484. void light_flip(){
     
485.       ATD0CTL3=0xc8;
     
486.       ATD0CTL5=0x10;
     
487.       while(!ATD0STAT0_SCF);
     
488.       flip_data[0]=ATD0DR0L;
     
489.       flip_data[1]=ATD0DR1L;
     
490.       flip_data[2]=ATD0DR2L;
     
491.       flip_data[3]=ATD0DR3L;
     
492.       flip_data[4]=ATD0DR4L;
     
493.       flip_data[5]=ATD0DR5L;
     
494.       flip_data[6]=ATD0DR6L;
     
495.       flip_data[7]=ATD0DR7L;
     
496.       flip_data[8]=ATD0DR8L;  
     
497. }
     
498. 
     
499. void AD_DATA_HIGHT()
     
500. {                     
     
501.       //light_flip();
     
502.             
     
503.       /*
     
504.       ATD0CTL3=0x98;
     
505.       ATD0CTL5=0x13;
     
506.       while(!ATD0STAT0_SCF);
     
507.       flip_data[3]=ATD0DR0L;
     
508.       flip_data[4]=ATD0DR1L;
     
509.       flip_data[5]=ATD0DR2L;
     
510.       */
     
511.       PORTB = 0x08|0x10|0x20;
     
512.       delay(L_times);
     
513.       ATD0CTL3=0x98;   
     
514.       ATD0CTL5=0x13;
     
515.       while(!ATD0STAT0_SCF);
     
516.       data[3]= ATD0DR0L;
     
517.       data[4]= ATD0DR1L;
     
518.       data[5]= ATD0DR2L;
     
519.       PORTB = 0x00;
     
520.       
     
521.       /*
     
522.       ATD0CTL3=0x98;
     
523.       ATD0CTL5=0x10;
     
524.       while(!ATD0STAT0_SCF);
     
525.       flip_data[0]=ATD0DR0L;
     
526.       flip_data[1]=ATD0DR1L;
     
527.       flip_data[2]=ATD0DR2L;
     
528.       */
     
529.       PORTB = 0x01|0x02|0x04;
     
530.       delay(L_times);
     
531.       ATD0CTL3=0x98;         
     
532.       ATD0CTL5=0x10;
     
533.       while(!ATD0STAT0_SCF);
     
534.       data[0]= ATD0DR0L;
     
535.       data[1]= ATD0DR1L;
     
536.       data[2]= ATD0DR2L;
     
537.       PORTB = 0x00;
     
538. 
     
539.       /*
     
540.       ATD0CTL3=0x98;
     
541.       ATD0CTL5=0x16;
     
542.       while(!ATD0STAT0_SCF);
     
543.       flip_data[6]=ATD0DR0L;
     
544.       flip_data[7]=ATD0DR1L;
     
545.       flip_data[8]=ATD0DR2L;
     
546.       */
     
547.       PORTB = 0x40|0x80;
     
548.       PORTA_PA0=1;
     
549.       delay(L_times);
     
550.       ATD0CTL3=0x98;            
     
551.       ATD0CTL5=0x16;
     
552.       while(!ATD0STAT0_SCF);
     
553.       data[6]= ATD0DR0L;
     
554.       data[7]= ATD0DR1L;
     
555.       data[8]= ATD0DR2L;
     
556.       PORTB = 0x00;
     
557.       PORTA_PA0=0;
     
558. 
     
559.       //printp("\n %D,%D,%D,%D,%D,%D,%D,%D,%D \n",flip_data[0],flip_data[1],flip_data[2],flip_data[3],flip_data[4],flip_data[5],flip_data[6],flip_data[7],flip_data[8]);                    
     
560. }
     
561. void AD_DATA_LOW(){
     
562.       ATD0CTL3=0xa8;   
     
563.       ATD0CTL5=0x19;
     
564.       while(!ATD0STAT0_SCF);
     
565.       data_2[0]=ATD0DR0L;
     
566.       data_2[1]=ATD0DR1L;
     
567.       data_2[2]=ATD0DR2L;
     
568.       data_2[3]=ATD0DR3L;
     
569.       data_2[4]=ATD0DR4L;
     
570. }
     
571. 
     
572. //----------------------------压差归一------------------------------//
     
573. 
     
574. void VOL_Normaliz(){  
     
575.   uchar aa;
     
576.   char show_vol=0;
     
577.   if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==1)  line_num=0;
     
578.   else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==0) line_num=6;
     
579.   else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==1) line_num=7;
     
580.   else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==0) line_num=8;
     
581.   else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==1) line_num=9;
     
582.   else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==0) line_num=10;
     
583.   else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==1) line_num=11;
     
584.   else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==0) line_num=12;
     
585.   while(KEY_RUN_OR_LCD==1) {
     
586.     LCD_DISPLAY();
     
587.   }
     
588.   key_r_o_l=0xff;
     
589.   LcdCommand(CLR,1);
     
590.   while(KEY_START_RUN ==1){
     
591.     AD_DATA_HIGHT();
     
592.     for(aa=0;aa<9;aa++){
     
593.       if(data[aa]>max_vol[aa])  max_vol[aa]=data[aa];
     
594.       if(data[aa]<min_vol[aa])  min_vol[aa]=data[aa];
     
595.       if((max_vol[aa]-min_vol[aa])>det_vol[aa]){                //det_vol impossible zero      
     
596.         det_vol[aa]=max_vol[aa]-min_vol[aa];
     
597.       }
     
598.     }
     
599.    
     
600.     if(lcd_print==1) {
     
601.       if(KEY_DOWN==0)
     
602.       {
     
603.         Dly_ms(1);
     
604.         if(KEY_DOWN==0){
     
605.           show_vol++;
     
606.           if(show_vol>10) show_vol=0;
     
607.           while(!KEY_DOWN);  
     
608.         }
     
609.       }
     
610.       if(KEY_UP==0){
     
611.         Dly_ms(1);
     
612.         if(KEY_UP==0){
     
613.           show_vol--;
     
614.           if(show_vol<0) show_vol=10;  
     
615.           while(!KEY_UP);
     
616.         }  
     
617.       }
     
618.       if(show_vol==0){
     
619.         Lcd_Print_Str(0,0,"data[0]-data[2]:");
     
620.         Lcd_Print_Char(0,1,data[0]);
     
621.         Lcd_Print_Char(6,1,data[1]);
     
622.         Lcd_Print_Char(12,1,data[2]);
     
623.       }
     
624.       if(show_vol==1){
     
625.         Lcd_Print_Str(0,0,"data[3]-data[5]:");
     
626.         Lcd_Print_Char(0,1,data[3]);
     
627.         Lcd_Print_Char(6,1,data[4]);
     
628.         Lcd_Print_Char(12,1,data[5]);
     
629.       }
     
630.       if(show_vol==2){
     
631.         Lcd_Print_Str(0,0,"data[6]-data[8]:");
     
632.         Lcd_Print_Char(0,1,data[6]);
     
633.         Lcd_Print_Char(6,1,data[7]);
     
634.         Lcd_Print_Char(12,1,data[8]);
     
635.       }
     
636.       if(show_vol==3){
     
637.         Lcd_Print_Str(0,0," det[0]-det[2]: ");
     
638.         Lcd_Print_Char(0,1,det_vol[0]);
     
639.         Lcd_Print_Char(6,1,det_vol[1]);
     
640.         Lcd_Print_Char(12,1,det_vol[2]);
     
641.       }
     
642.       if(show_vol==4){
     
643.         Lcd_Print_Str(0,0," det[3]-det[5]: ");
     
644.         Lcd_Print_Char(0,1,det_vol[3]);
     
645.         Lcd_Print_Char(6,1,det_vol[4]);
     
646.         Lcd_Print_Char(12,1,det_vol[5]);
     
647.       }
     
648.       if(show_vol==5){
     
649.         Lcd_Print_Str(0,0," det[6]-det[8]: ");
     
650.         Lcd_Print_Char(0,1,det_vol[6]);
     
651.         Lcd_Print_Char(6,1,det_vol[7]);
     
652.         Lcd_Print_Char(12,1,det_vol[8]);
     
653.       }
     
654.       if(show_vol==6){
     
655.         Lcd_Print_Str(0,0," flip[0]---[2]: ");
     
656.         Lcd_Print_Char(0,1,flip_data[0]);
     
657.         Lcd_Print_Char(6,1,flip_data[1]);
     
658.         Lcd_Print_Char(12,1,flip_data[2]);
     
659.       }
     
660.       if(show_vol==7){
     
661.         Lcd_Print_Str(0,0," flip[3]---[5]: ");
     
662.         Lcd_Print_Char(0,1,flip_data[3]);
     
663.         Lcd_Print_Char(6,1,flip_data[4]);
     
664.         Lcd_Print_Char(12,1,flip_data[5]);
     
665.       }
     
666.       if(show_vol==8){
     
667.         Lcd_Print_Str(0,0," flip[6]---[8]: ");
     
668.         Lcd_Print_Char(0,1,flip_data[6]);
     
669.         Lcd_Print_Char(6,1,flip_data[7]);
     
670.         Lcd_Print_Char(12,1,flip_data[8]);
     
671.       }
     
672.       if(show_vol==9)
     
673.                         {
     
674.         Lcd_Print_Str(0,0,"data_2[0--3]");
     
675.                                 Lcd_Print_Char(0,1,data_2[0]);
     
676.                                 Lcd_Print_Char(6,1,data_2[1]);
     
677.                                 Lcd_Print_Char(12,1,data_2[2]);
     
678.                         }
     
679.                         if(show_vol==10)
     
680.                         {
     
681.         Lcd_Print_Str(0,0,"data_2[4--5]");
     
682.                                 Lcd_Print_Char(0,1,data_2[3]);
     
683.                                 Lcd_Print_Char(6,1,data_2[4]);
     
684.                         }
     
685.     }
     
686.     if(lcd_print==2){
     
687.       printp("\n %D %D %D %D %D %D %D %D %D \n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8]);                     
     
688.       printp("\n det  :%D %D %D %D %D %D %D %D %D \n",det_vol[0],det_vol[1],det_vol[2],det_vol[3],det_vol[4],det_vol[5],det_vol[6],det_vol[7],det_vol[8]);
     
689.     }
     
690.   }
     
691.   
     
692. }
     
693. 
     
694. void err_calculate_low(){
     
695.     uchar jj,b_num=0;
     
696.     AD_DATA_LOW();
     
697.      
     
698.     for(jj=0;jj<5;jj++){
     
699.       if(data_2[jj]<THRESHOLD_2)  b_num++;   
     
700.     }
     
701.     if(b_num==1||b_num==2)
     
702.     {
     
703.       if(data_2[0]<THRESHOLD_2&&data_2[1]<THRESHOLD_2) err_e2[0]=37;
     
704.       else if(data_2[1]<THRESHOLD_2&&data_2[2]<THRESHOLD_2) err_e2[0]=15;
     
705.       else if(data_2[2]<THRESHOLD_2&&data_2[3]<THRESHOLD_2) err_e2[0]=-15;
     
706.       else if(data_2[3]<THRESHOLD_2&&data_2[4]<THRESHOLD_2) err_e2[0]=-37;
     
707.       
     
708.       else if(data_2[2]<THRESHOLD_2)  err_e2[0]=0;
     
709.       else if(data_2[0]<THRESHOLD_2)  err_e2[0]=45;
     
710.       else if(data_2[1]<THRESHOLD_2)  err_e2[0]=30;
     
711.       else if(data_2[3]<THRESHOLD_2)  err_e2[0]=-30;
     
712.       else if(data_2[4]<THRESHOLD_2)  err_e2[0]=-45;
     
713.       err[0]=err_e2[1];
     
714.       err_e2[1]=err_e2[0];
     
715.     }
     
716. }
     
717. 
     
718. void err_calculate_hight()
     
719. {
     
720.     byte  black_l=0,black_r=8,slope_num=0,min_slope=0,black=0;
     
721.     uchar ii=0,max_num=0,max_black=100,n=0;
     
722.     schar det_spacing=0,spacing=0;
     
723.     uchar up_edge=0,down_edge=0;
     
724.     byte  black_num=0,white_num=0,zero_num=0;
     
725.     //flip=0;  
     
726.     AD_DATA_HIGHT();            
     
727.     for(ii=0;ii<9;ii++)
     
728.     {
     
729.       if(data[ii]<=min_vol[ii])  percent[ii]=1;
     
730.       else if(data[ii]>=max_vol[ii])  percent[ii]=100;
     
731.       else percent[ii]=(data[ii]-min_vol[ii])*100/det_vol[ii];
     
732.   
     
733.       if(percent[ii]<max_black&&percent[ii]<=40)   
     
734.       {
     
735.         max_black=percent[ii];     
     
736.         max_num=ii;
     
737.       }
     
738.       if(percent[ii]<=40) {
     
739.         black_num++;      
     
740.       }
     
741.       if(data[ii]-max_vol[ii]>25) slope_num++;
     
742.       if(percent[ii]<=20) zero_num++;
     
743.       if(percent[ii]>=50) white_num++;
     
744.     }
     
745.     for(ii=0;ii<9;ii++){
     
746.       if(percent[ii]<40)  {
     
747.         black_l=ii;
     
748.         break;
     
749.       }
     
750.     }
     
751.     for(ii=8;ii>0;ii--){
     
752.       if(percent[ii]<40){
     
753.         black_r=ii;
     
754.         break;
     
755.       }
     
756.     }
     
757.    
     
758. //----------------------flip----------------------//   
     
759.     if(black_r-black_l==0||black_r-black_l==1)  {
     
760.       flip=1;    //黑线连续，个数规定，数据可用?
     
761.       PORTK_PK5=1;
     
762.     }
     
763.     else if((black_r-black_l)==2&&(black_r+black_l)/2<40)   {
     
764.       flip=1;
     
765.       PORTK_PK5=1;
     
766.     }
     
767.     else  {
     
768.       flip=0;
     
769.       PORTK_PK5=0;
     
770.     }                     
     
771. //------------------------slope------------------------//   
     
772.       //前排为上坡
     
773.       if(check==0&&slope_num>=7&&slope_bit==0){   
     
774.         slope_bit=1;     //连续检测到正常时清除标志
     
775.         PORTK_PK7=1;
     
776.         //PORTK_PK4=1;
     
777.         KP=13;
     
778.         check=0;
     
779.       }
     
780. //------------------------start-line------------------------//   
     
781.       else if(slope_bit==0&&zero_num>=2&&black_num>=6&&check==0&&abs(err_e[0]-err_e[1])<=120){
     
782.         slope_bit=0;
     
783.         check=1;
     
784.         PORTK_PK7 = 1; //打开下排
     
785.         //PORTK_PK4=1;
     
786.       }
     
787.            
     
788. //----------------------前排检测到跑道-------------------------//   
     
789.         
     
790.         if(flip==1&&abs(max_num-max_num_p)<=4)  
     
791.         {
     
792.          
     
793.           if(slope_bit==1){           //连续地看到了数值
     
794.             slope_time=1;
     
795.           }
     
796.           else if(slope_bit==0){
     
797.             
     
798.             if(max_num==0)   err_e[0]=120-30*percent[max_num]/(percent[max_num]+percent[max_num+1]);
     
799.             else if(max_num==8)   err_e[0]=-120+30*percent[max_num]/(percent[max_num]+percent[max_num-1]);
     
800.             else
     
801.             {
     
802.                 switch(max_num)
     
803.                 {
     
804.                     case 1:spacing=90;    break;
     
805.                     case 2:spacing=60;    break;
     
806.                     case 3:spacing=30;    break;
     
807.                     case 4:spacing=0;     break;                             
     
808.                     case 5:spacing=-30;   break;
     
809.                     case 6:spacing=-60;   break;
     
810.                     case 7:spacing=-90;   break;
     
811.                     default:
     
812.                     break;
     
813.                 }
     
814.                 if(percent[max_num+1]==percent[max_num-1])  err_e[0]=spacing;  
     
815.                 else if(percent[max_num+1]<percent[max_num-1])
     
816.                 {
     
817.                   err_e[0]=spacing-30*percent[max_num]/(percent[max_num+1]+percent[max_num]);
     
818.                 }
     
819.                 else if(percent[max_num+1]>percent[max_num-1])   
     
820.                 {
     
821.                   err_e[0]=spacing+30*percent[max_num]/(percent[max_num]+percent[max_num-1]);
     
822.                 }
     
823.             }
     
824.               err[0]=err_e[0];
     
825.               err_e[1]=err_e[0];
     
826.               max_num_p=max_num;  
     
827.           }
     
828.         }
     
829. 
     
830.         else if(slope_bit==1){
     
831.           err_calculate_low();  
     
832.         }
     
833.         /*
     
834.         if(slope_bit==0&&white_num==9&&flip==0&&abs(max_num-max_num_p)<=4){
     
835.           if(err_e[0]>=50)  {
     
836.             err_e[0]=120;
     
837.             err[0]=err_e[0];
     
838.           }
     
839.           else if(err_e[0]<=-50){
     
840.             err_e[0]=-120;
     
841.             err[0]=err_e[0];
     
842.           }
     
843.         }
     
844.         */
     
845.         if(check==1)
     
846.         {
     
847.           AD_DATA_LOW();
     
848.           for(ii=0;ii<5;ii++){
     
849.             if(data_2[ii]<THRESHOLD_2)  black++;  
     
850.           }
     
851.           if(black>=3){
     
852.             black_bit=1;
     
853.             //check_pluse=0;
     
854.             //check=0;
     
855.             //PORTK_PK7=0;  
     
856.           }
     
857.            
     
858.          
     
859.         }     
     
860. }
     
861.                   
     
862. //-------------------------------------------------------------------------//
     
863.                                              
     
864. signed int PID(){
     
865.   uchar kk=0,low=0;
     
866.   sint u;
     
867.   
     
868.   if(err[0]<=45&&err[0]>=-45) {
     
869.     KP=5;
     
870.     u=7000+KP*err[0];                 
     
871.   }
     
872.   else if(err[0]>45){
     
873.     KP=9;                                 
     
874.           u=6920+KP*err[0];  
     
875.   }
     
876.   else if(err[0]<-45) {
     
877.     KP=9;                                 
     
878.           u=7180+KP*err[0];  
     
879.   }
     
880.   
     
881.   //u=7000+KP*err[0]+KD*(err[0]-2*err[1]+err[2]);
     
882.         if(u>=8050)  u=8050;
     
883.         if(u<=6100)  u=6100;
     
884.         for(kk=100;kk>0;kk--) {
     
885.           err[kk]=err[kk-1];
     
886.         }
     
887.         return(u);
     
888. }
     
889. void key_scan_deal(void){
     
890.           /*
     
891.           if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==0)  line_num=6;
     
892.           else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==1) line_num=7;
     
893.           else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==0) line_num=8;
     
894.           else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==1) line_num=9;
     
895.           else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==0) line_num=10;
     
896.           else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==1) line_num=11;
     
897.           else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==0) line_num=12;
     
898.           else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==1) line_num=13;
     
899.           */
     
900.           if(KEY_RUN_OR_LCD==0)
     
901.                 {
     
902.                         LcdDelay(14);
     
903.                         if(KEY_RUN_OR_LCD==0)
     
904.                         {   
     
905.                                 key_r_o_l=~key_r_o_l;
     
906.                                 while(!KEY_RUN_OR_LCD);
     
907.                         }
     
908.                 }
     
909.                 if(key_r_o_l==0){
     
910.       //DisableInterrupts;
     
911.       RTI_SWITCH_OFF;
     
912.       IC_SWITCH_OFF;
     
913.       LCD_DISPLAY();
     
914.       PWMDTY45=PWMDTY23;
     
915.     }
     
916.     else{
     
917.       //EnableInterrupts;
     
918.       LcdCommand(CLR,1);
     
919.       RTI_SWITCH_ON;
     
920.       IC_SWITCH_ON;
     
921.     }  
     
922. }
     
923. 
     
924. 
     
925. /*************************LCD show start************************/
     
926. 
     
927. void LCD_DISPLAY(void)
     
928. {               
     
929. /************************KEY_DOWN*************************/               
     
930.                 if(KEY_DOWN==0)
     
931.                 {
     
932.                         LcdDelay(7);
     
933.                         if(KEY_DOWN==0)
     
934.                         {
     
935.                                 //控制主菜单向下   1---9
     
936.                                
     
937. ……………………
     
938. 
     
939. …………限于本文篇幅 余下代码请从51黑下载附件…………
     

复制代码

所有资料51hei提供下载:
7.15(2).rar (583.2 KB, 下载次数: 14)

2018-4-12 15:58 上传

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

---

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

Powered by Discuz! X3.1