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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › MC9S12XS128智能小车测试源程序
返回列表 发新帖
查看: 2083|回复: 0
打印 上一主题 下一主题
收起左侧

MC9S12XS128智能小车测试源程序

[复制链接]
跳转到指定楼层
楼主
ID:306629 发表于 2018-4-12 15:58 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
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"


  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

  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************************/



  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************************/



  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;

  115. //---------------sensor_2 data var-----------------//                              
  116. uchar data_2[5],THRESHOLD_2=200,flip_data_2[5],err_e2[3];

  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;

  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;


  129. //------------------key&lcd var----------------------//
  130. byte key_r_o_l=0,lcd_print=1;
  131. uchar menu=1,menu_option=1,bits=0;

  133. //----------------- calc var---------------------//
  134. uchar KP=8,period_1=0,period_2=0;
  135. char  std_err[10];
  136. int  temp_std;

  138. /*********************END************************/



  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. }


  166. //--------------------------------------------------------------
  167. //                    HARDWARE FUN START                        
  168. //--------------------------------------------------------------

  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. }

  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. }

  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. }

  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. }

  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. }


  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. }

  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. */

  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. }

  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. }

  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. }

  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. }

  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. }

  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. }      

  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. }

  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. }

  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. }

  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. }

  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***************************/

  476. //--------------------------------------------------------------
  477. //                    HARDWARE FUN END                          
  478. //--------------------------------------------------------------


  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. }

  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;

  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;

  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. }

  572. //----------------------------压差归一------------------------------//

  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. }

  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. }

  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.         }

  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. }


  925. /*************************LCD show start************************/

  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. ……………………

  939. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

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


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

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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