欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標題: 單片機四驅(qū)車遙控避障尋線演示程序(GP41QS) [打印本頁]
作者: Dong東    時間: 2018-8-29 01:09
標題: 單片機四驅(qū)車遙控避障尋線演示程序(GP41QS)
希望大家一起學(xué)習(xí)學(xué)習(xí)
電路原理圖如下：


單片機源程序如下:
  1. #include <stdio.h>
  2. #include <intrins.h>
  3. #include "STC12C5202AD.H"
  4. #include "sio.h"

  6. typedef struct PID
  7. {

  9.     int p;         //  比例常數(shù) Proportional Const  
  10.     int i;         //  積分常數(shù) Integral Const  
  11.     int d;         //  微分常數(shù) Derivative Const  

  13.     int position;
  14.     int hisPosition;   
  15.     int lastPosition[3];
  16. } PID;

  18. static PID idata pid;
  19. static int GAIN;

  21. #define MIN9MS (XTAL/2L/256L*9L/1000L*9L/10L)       //9ms脈寬*90%
  22. #define MAX9MS (XTAL/2L/256L*9L/1000L*11L/10L)       //9ms脈寬*110%

  24. #define MIN45MS (XTAL/2L/256L*45L/10000L*9L/10L)       //4.5ms脈寬
  25. #define MAX45MS (XTAL/2L/256L*45L/10000L*11L/10L)

  27. #define MIN225MS (XTAL/2L/256L*225L/100000L*9L/10L)    //2.25ms脈寬
  28. #define MAX225MS (XTAL/2L/256L*225L/100000L*11L/10L)

  30. #define MIN056MS (XTAL/2L/256L*56L/100000L*6L/10L)     //0.56ms脈寬*60%,下限
  31. #define MAX056MS (XTAL/2L/256L*56L/100000L*14L/10L)

  33. #define MIN168MS (XTAL/2L/256L*168L/100000L*6L/10L)   //1.68ms脈寬*60%
  34. #define MAX168MS (XTAL/2L/256L*168L/100000L*14L/10L)

  36. sfr ISP_CUNTR = 0xE7;

  38. sbit LED1 = P3^0;
  39. sbit LED2 = P3^1;

  41. sbit AD_LED1 = P2^0;
  42. sbit AD_LED2 = P2^1;
  43. sbit AD_LED3 = P2^7;

  45. sbit IR_FRONT = P3^2;
  46. sbit IR_LEFT = P3^3;
  47. sbit IR_RIGHT = P2^6;
  48. sbit IR_BACK = P3^7;

  50. sbit IR_OUT = P3^5;

  52. sbit MOTO_IN_B1 = P2^5;
  53. sbit MOTO_IN_B2 = P2^4;

  55. sbit MOTO_IN_A1 = P2^3;
  56. sbit MOTO_IN_A2 = P2^2;

  58. static unsigned char idata ad_datas[8];  //8路光電管采樣電壓
  59. static unsigned char idata ad_datas_check[8];  //8路光電管采樣電壓校驗值,軌道的白色背景的采樣值

  61. bit power_stat;
  62. static unsigned char car_stat;  //小車狀態(tài):0,停止;1,前進;2,后退;3,左轉(zhuǎn);4,右轉(zhuǎn);5,尋線模式;ff,自控避障模式
  63. static unsigned int now;

  65. static unsigned char code led_mod_table[3][20] = {
  66.    {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
  67.    {1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0},
  68.    {1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}
  69. };
  70. unsigned char idata led_mod = 0;
  71. static unsigned char idata led_tick = 0;
  72. static unsigned char idata led_ptr = 0;


  75. static bit test;

  77. static unsigned char tick = 0;
  78. static unsigned int pwm38k = 0;
  79. static unsigned char check;

  81. static unsigned char pwm_moto = 0;
  82. static unsigned char pwm_moto_left = 0;
  83. static unsigned char pwm_moto_right = 0;
  84. static bit moto_left_forward = 1;
  85. static bit moto_right_forward = 1;


  88. #define IR_SINGAL_DELAY 11    //接收管輸出延遲載波數(shù)量
  89. #define TEST_PERIOD 1620      //評估周期,這個不同的接收管差別很大.
  90. #define IR_SINGAL_PERIOD 76   //持續(xù)發(fā)射紅外線載波數(shù)量
  91. #define IR_VALID_THROLD 70    //判斷是否前方有障礙的閥值

  93. /*
  94. #define IR_SINGAL_DELAY 1    //接收管輸出延遲載波數(shù)量
  95. #define TEST_PERIOD 200      //評估周期,這個不同的接收管差別很大.
  96. #define IR_SINGAL_PERIOD 10   //持續(xù)發(fā)射紅外線載波數(shù)量
  97. #define IR_VALID_THROLD 8    //判斷是否前方有障礙的閥值
  98. */
  99. static unsigned char idata front_signal = 0;
  100. static unsigned char idata back_signal = 0;
  101. static unsigned char idata left_signal = 0;
  102. static unsigned char idata right_signal = 0;

  104. static bit front_obj = 0, back_obj = 0, left_obj = 0, right_obj = 0;

  106. /*
  107. * PCA中斷計數(shù),根據(jù)位置判斷信號區(qū)域和定義,位置0表示初始,1代表引導(dǎo)碼信號,2表示引導(dǎo)碼間隔,
  108. * 3表示第一個bit的信號,4表示第一個bit的間隔,以次類推...
  109. * 更具體見對應(yīng)的紅外線協(xié)議.
  110. */
  111. static unsigned int idata pca_tick;
  112. static unsigned char idata pca_int_count;
  113. static unsigned char data pca_int_total;    /* 根據(jù)引導(dǎo)頭確定總長度 */
  114. static unsigned int idata period;   /* 紅外信號占或空周期計數(shù) */
  115. static unsigned char idata data_buf[6];  /* 紅外線協(xié)議數(shù)據(jù)緩沖區(qū) */
  116. static unsigned int idata ccap1;        //PCA0上一次的的計數(shù)
  117. static unsigned char idata frame_dog;  //紅外幀看門狗,限定時間未接收完成清除工作

  119. static void delay_ms(unsigned int v) {
  120.   unsigned int wait = pca_tick + v / 7 + 1;
  121.   while (wait != pca_tick) {
  122.       PCON |= 0x01;
  123.   }
  124. }

  126. static void delay_100us(unsigned char v) {
  127.     unsigned char wait = tick + v * 2; //每tick約50us
  128.     while (wait != tick) {
  129.         ;//PCON |= 0x01;
  130.     }
  131. }

  133. void pid_init(PID *pid)
  134. {
  135.     pid->position = 0;
  136.     pid->hisPosition = 0;
  137.     pid->lastPosition[0] = 0;
  138.     pid->lastPosition[1] = 0;
  139.     pid->lastPosition[2] = 0;
  140.     pid->p = 2;
  141.     pid->i = 0;
  142.     pid->d = 8;
  143. }

  145. void read_sensors(unsigned char sensors[]) {  //讀光電傳感器的采樣值
  146.     unsigned char i;

  148.     for (i = 0; i < 8; i++) {  //LED熄滅狀態(tài)測量一次電壓
  149.         ADC_CONTR = 0xE0 | i; //設(shè)置AD通道, 快速AD采樣.
  150.         delay_100us(5);          //延遲等待電壓穩(wěn)定
  151.         ADC_RES = 0;
  152.         ADC_CONTR = 0xE8 | i;;   //啟動AD
  153.         while (!(ADC_CONTR & 0x10)) {

  155.         }
  156.         ADC_CONTR &= 0xE7;
  157.         sensors[i] = ADC_RES;
  158.     }

  160.     AD_LED1 = 0;
  161.     AD_LED2 = 0;
  162.     AD_LED3 = 0;

  164.     for (i = 0; i < 8; i++) {  //LED打開再測量一次電壓,用于消除環(huán)境光的差異.
  165.         ADC_CONTR = 0xE0 | i; //設(shè)置AD通道, 快速AD采樣.

  167.         delay_100us(5);          //延遲等待電壓穩(wěn)定

  169.         ADC_RES = 0;
  170.         ADC_CONTR = 0xE8 | i;;   //啟動AD
  171.         while (!(ADC_CONTR & 0x10)) {

  173.         }
  174.         ADC_CONTR &= 0xE7;

  176.         if (ADC_RES > sensors[i])
  177.             sensors[i] = ADC_RES - sensors[i];
  178.         else
  179.             sensors[i] = ADC_RES;
  180.     }
  181.     AD_LED1 = 1;
  182.     AD_LED2 = 1;
  183.     AD_LED3 = 1;
  184. }

  186. static void read_sensors_check() { //從flash中讀取光電傳感器的校驗值,結(jié)構(gòu)是8byte的值+1字節(jié)的crc
  187.     unsigned char i, crc = 0;
  188.     IAP_ADDRH = 0;
  189.     IAP_CONTR = 0x80;  //允許ISP/IAP操作
  190.     IAP_CMD = 0x01;  //讀flash

  192.     for (i = 0; i < 8; i++) {
  193.         IAP_ADDRL = i;
  194.         IAP_TRIG = 0x5A;
  195.         IAP_TRIG = 0xA5;  //啟動IAP操作
  196.         _nop_();
  197.         ad_datas_check[i] = IAP_DATA;
  198.         crc += IAP_DATA;
  199. //com_putchar(IAP_DATA);
  200.     }
  201.     IAP_ADDRL = 8;
  202.     IAP_TRIG = 0x5A;
  203.     IAP_TRIG = 0xA5;  //啟動IAP操作
  204.     _nop_();

  206.     if (crc != IAP_DATA) {  //crc驗證錯誤,flash數(shù)據(jù)失效
  207.         for (i = 0; i < 8; i++) {
  208.             ad_datas_check[i] = 0xff;
  209.         }
  210.     }

  212.     IAP_CONTR = 0;    //禁止IAP,防止誤操作
  213.     IAP_CMD = 0;
  214.     IAP_TRIG = 0;
  215.     IAP_ADDRH = 0xff;
  216.     IAP_ADDRL = 0xff;
  217. }

  219. static void write_sensors_check() { //將光電傳感器的校驗值寫入flash,結(jié)構(gòu)是8byte的值+1字節(jié)的crc
  220.     unsigned char i, crc = 0;
  221.     IAP_ADDRH = 0;
  222.     IAP_CONTR = 0x80;  //允許ISP/IAP操作
  223.     read_sensors(ad_datas_check);

  225.     IAP_ADDRL = 0;
  226.     IAP_CMD = 0x03;  //擦除flash
  227.     IAP_TRIG = 0x5A;
  228.     IAP_TRIG = 0xA5;  //啟動IAP操作
  229.     _nop_();

  231.     IAP_CMD = 0x10;  //寫flash

  233.     for (i = 0; i < 8; i++) {
  234.         IAP_ADDRL = i;
  235.         IAP_DATA = ad_datas_check[i];
  236. //com_putchar(IAP_DATA);
  237.         crc += ad_datas_check[i];
  238.         IAP_CMD = 0x02;  //寫flash
  239.         IAP_TRIG = 0x5A;
  240.         IAP_TRIG = 0xA5;  //啟動IAP操作
  241.         _nop_();
  242.     }

  244.     IAP_ADDRL = 8;
  245.     IAP_DATA = crc;
  246.     IAP_TRIG = 0x5A;
  247.     IAP_TRIG = 0xA5;  //啟動IAP操作
  248.     _nop_();


  251.     IAP_CONTR = 0;    //禁止IAP,防止誤操作
  252.     IAP_CMD = 0;
  253.     IAP_TRIG = 0;
  254.     IAP_ADDRH = 0xff;
  255.     IAP_ADDRL = 0xff;
  256. }

  258. void time0_isr() interrupt 1
  259. {
  260.     tick++;
  261.     pwm38k++;
  262.     if (pwm38k == 0)
  263.     {
  264.         front_signal = 0;
  265.         back_signal = 0;
  266.         left_signal = 0;
  267.         right_signal = 0;
  268.     } else if (pwm38k == TEST_PERIOD)
  269.     {
  270. //com_putchar(check);
  271.         if (front_signal >= IR_VALID_THROLD)
  272.         {
  273.             front_obj = 1;
  274.         } else
  275.             front_obj = 0;

  277.         if (back_signal >= IR_VALID_THROLD)
  278.         {
  279.             back_obj = 1;
  280.         } else
  281.             back_obj = 0;

  283.         if (left_signal >= IR_VALID_THROLD)
  284.         {
  285.             left_obj = 1;
  286.         } else
  287.             left_obj = 0;

  289.         if (right_signal >= IR_VALID_THROLD)
  290.         {
  291.             right_obj = 1;
  292.         } else
  293.             right_obj = 0;

  295.         pwm38k = 0xFFFF;
  296.         return;
  297.     }

  299.     if (pwm38k == 0)
  300.     {
  301.         IR_OUT = 1;
  302.         test = 0;
  303.     } else if (pwm38k == IR_SINGAL_PERIOD)
  304.     {
  305.         IR_OUT = 0;
  306.     }

  308. //    if (!test && !IR_FRONT)  //調(diào)試接收管的延遲
  309. //    {
  310. //        test = 1;
  311. //        com_putchar(tick);
  312. //    }

  314.     if (pwm38k >= IR_SINGAL_DELAY && pwm38k < IR_SINGAL_DELAY + IR_SINGAL_PERIOD)
  315.     {
  316.         if (!IR_FRONT)
  317.             front_signal++;
  318.         else
  319.         {
  320.             if (front_signal)
  321.                 front_signal--;
  322.         }

  324.         if (!IR_BACK)
  325.             back_signal++;
  326.         else
  327.         {
  328.             if (back_signal)
  329.                 back_signal--;
  330.         }

  332.         if (!IR_LEFT)
  333.             left_signal++;
  334.         else
  335.         {
  336.             if (left_signal)
  337.                 left_signal--;
  338.         }

  340.         if (!IR_RIGHT)
  341.             right_signal++;
  342.         else
  343.         {
  344.             if (right_signal)
  345.                 right_signal--;
  346.         }
  347.     }

  349.         if (pwm_moto == 0)
  350.         {
  351.             if (pwm_moto_left > 0)
  352.             {
  353.                 if (moto_left_forward)
  354.                     MOTO_IN_A2 = 1;
  355.                 else
  356.                     MOTO_IN_A1 = 1;
  357.             } else
  358.             {
  359.                 MOTO_IN_A1 = 0;
  360.                 MOTO_IN_A2 = 0;
  361.             }
  362.             if (pwm_moto_right > 0)
  363.             {
  364.                 if (moto_right_forward)
  365.                     MOTO_IN_B2 = 1;
  366.                 else
  367.                     MOTO_IN_B1 = 1;
  368.             } else
  369.             {
  370.                 MOTO_IN_B1 = 0;
  371.                 MOTO_IN_B2 = 0;
  372.             }
  373.         } else
  374.         {
  375.             if (pwm_moto == pwm_moto_left)
  376.             {
  377.                 MOTO_IN_A1 = 0;
  378.                 MOTO_IN_A2 = 0;
  379.             }
  380.             if (pwm_moto == pwm_moto_right)
  381.             {
  382.                 MOTO_IN_B1 = 0;
  383.                 MOTO_IN_B2 = 0;
  384.             }
  385.         }
  386.         pwm_moto++;
  387. }

  389. void time0_initialize(void)
  390. {
  391.     TMOD &= ~0x0F;      /* clear timer 0 mode bits */
  392.     TMOD |= 0x02;       /* put timer 0 into MODE 2 */
  393.     AUXR |= 0x80;       // timer0工作在1T模式
  394.         TH0 = 256 - XTAL / 2L / 38400L;  // 256 - XTAL/T1_12/f, f=輸出時鐘頻率
  395.         TL0 = 0x0;

  397.     WAKE_CLKO = 0x01;  // T0在P3.4上輸出時鐘.

  399.     PT0 = 1;            /* 時鐘0中斷高優(yōu)先級 */
  400.         TR0 = 1;            //
  401.         ET0 = 1;
  402. }

  404. static void wakeup (void) interrupt 2
  405. {

  407. }


  410. static void pca_isr (void) interrupt 6
  411. {
  412.     unsigned char i, j;

  414.     if (CCF0) {
  415.         CCF0 = 0;   //清PCA1中斷標志
  416.         LED1 = IR_BACK;
  417.         if (!pca_int_count) {   //第一次收到信號
  418.             if (!IR_BACK) {
  419.                 ccap1 = pca_tick * 256 + CCAP0H;
  420.                 pca_int_count++;
  421.             }
  422.         } else {                //已經(jīng)收到一些信號
  423.              period = pca_tick * 256 + CCAP0H - ccap1;
  424.              ccap1 = pca_tick * 256 + CCAP0H;
  425. //com_putchar(period / 256);
  426. //com_putchar(period % 256);

  428.             if (pca_int_count == 1) {
  429.                 if (period < MIN9MS || period > MAX9MS) {  //9ms
  430.                     pca_int_count = 0;
  431.                     frame_dog = 0;
  432.                 } else
  433.                     pca_int_count++;
  434.             } else if (pca_int_count == 2) {
  435.                 if (period > MIN225MS && period < MAX225MS) { //2.25ms
  436.                     pca_int_total = 3;
  437.                     pca_int_count++;
  438.                 } else if (period > MIN45MS && period < MAX45MS) { //4.5ms
  439.                     pca_int_total = 67;
  440.                     pca_int_count++;
  441.                 } else {
  442.                     pca_int_count = 0;
  443.                     frame_dog = 0;
  444.                 }
  445.             } else {
  446.                 if (IR_BACK) {
  447.                     if (period > MIN056MS && period < MAX056MS) {  //0.56ms
  448.                         if (pca_int_count >= pca_int_total) {  //幀接收完畢,下面進行有效性分析.
  449.                             if (pca_int_total == 67) { //完整信號,含有引導(dǎo)信號,設(shè)備碼8bit,設(shè)備反碼8bit,命令字8bit,命令字反碼8bit
  450.                                 if ((data_buf[0] ^ data_buf[1] == 0xff) && (data_buf[2] ^ data_buf[3] == 0xff)) {
  451.                                     com_putchar(data_buf[0]);
  452.                                     com_putchar(data_buf[2]);
  453.                                     if (data_buf[0] == 0x40) {
  454.                                         switch (data_buf[2]) {
  455.                                         case 0x5F: //左
  456.                                             car_stat = 3;
  457.                                             break;
  458.                                         case 0x5B: //右
  459.                                             car_stat = 4;
  460.                                             break;
  461.                                         case 0x5A: //上
  462.                                             car_stat = 1;
  463.                                             break;
  464.                                         case 0x5E: //下
  465.                                             car_stat = 2;
  466.                                             break;
  467.                                         case 0x56: //菜單
  468.                                             car_stat = 0;
  469.                                             break;
  470.                                         case 0x0:  //數(shù)字0
  471.                                             car_stat = 0xff;
  472.                                             now = pca_tick;
  473.                                             break;
  474.                                         case 0x1:  //數(shù)字1
  475.                                             car_stat = 5;
  476.                                             pid_init(&pid);
  477.                                             GAIN = 150;
  478.                                             now = pca_tick;
  479.                                             break;
  480.                                         case 0x3:  //數(shù)字3
  481.                                             write_sensors_check();  //校驗光電傳感器
  482.                                             break;
  483.                                         case 0x1A:  //+號
  484.                                             pid.p++;
  485.                                             break;
  486.                                         case 0x1E:  //-號
  487.                                             if (pid.p)
  488.                                                 pid.p--;
  489.                                             break;
  490.                                         case 0x10:  //靜音鍵
  491.                                             GAIN++;
  492.                                             break;
  493.                                         case 0x1C:  //屏顯
  494.                                             if (GAIN)
  495.                                                 GAIN--;
  496.                                             break;
  497.                                         case 0x1B:  //^
  498.                                             pid.d++;
  499.                                             break;
  500.                                         case 0x1F:  //向下
  501.                                             if (pid.d)
  502.                                                 pid.d--;
  503.                                             break;
  504.                                         case 0x12: //POWER
  505.                                            // power_stat = ~power_stat;
  506.                                             break;
  507.                                         default:
  508.                                             break;
  509.                                         }
  510.                                     }
  511.                                 }
  512.                             } else {                   //重復(fù)信號,僅含有引導(dǎo)信號
  513.                             }
  514.                             pca_int_count = 0;
  515.                             frame_dog = 0;
  516.                         } else {
  517.                             pca_int_count++;
  518.                         }
  519.                     } else {
  520.                         pca_int_count = 0;
  521.                         frame_dog = 0;
  522.                     }
  523.                 } else {
  524.                     j = (pca_int_count - 3) / 2;
  525.                     i = j / 8;
  526.                     j = j % 8;

  528.                     if (period > MIN168MS && period < MAX168MS) {  //1.68ms
  529.                      //   com_putchar(0x01);
  530.                         data_buf[i] |= (0x01 << j);
  531.                         pca_int_count++;
  532.                     } else if (period > MIN056MS && period < MAX056MS) {  //0.56ms
  533.                      //   com_putchar(0x00);
  534.                         data_buf[i] &= ~(0x01 << j);
  535.                         pca_int_count++;
  536.                     } else {
  537.                         pca_int_count = 0;
  538.                         frame_dog = 0;
  539.                     }
  540.                 }
  541.             }
  542.         }
  543.     }

  545.     if (CF) {    //PCA計數(shù)溢出中斷,19.6608MHZ晶體大約6.7ms溢出
  546.         CF = 0;
  547.         pca_tick++;
  548.         if (led_tick++ >= 10) {
  549.             led_tick = 0;

  551.             if (led_mod_table[led_mod][led_ptr++]) {
  552.                 LED1 = 0;
  553.                 LED2 = 0;
  554.             } else {
  555.                 if (left_obj || right_obj || front_obj || back_obj) {
  556.                     LED1 = 0;
  557.                     LED2 = 0;
  558.                 } else {
  559.                     LED1 = 1;
  560.                     LED2 = 1;
  561.                 }
  562.             }

  564.             led_ptr %= 20;
  565.         }

  567.         if (pca_int_count) {
  568.             frame_dog++;
  569.             if (frame_dog >= 15) {  //100ms后重新開始分析新的紅外線數(shù)據(jù)包
  570.                 pca_int_count = 0;
  571.                 frame_dog = 0;
  572.             }
  573.         }
  574.     }
  575. }

  577. int read_black_line() {  //讀黑線位置.
  578.         unsigned char i;
  579.         int grayscale_min;  //最黑的線
  580.         char line_begin, line_end;  //黑線對應(yīng)的傳感器的開始和結(jié)束位置.
  581.         int tmp;

  583.         read_sensors(ad_datas);
  584. /*
  585.         line_begin = -1; line_end= -1;
  586.         for (i = 0; i < 8; i++) {
  587. //com_putchar(ad_datas[i]);
  588. //com_putchar(ad_datas_check[i]);
  589.             tmp = (ad_datas[i] * 100);
  590.             tmp = tmp / ad_datas_check[i];
  591.             ad_datas[i] = tmp;
  592.             if (ad_datas[i] <= 40)  //灰度<=40%認為下面有部分黑線
  593.             {
  594.                 if (line_begin == -1)
  595.                     line_begin = i;
  596.                 else
  597.                     line_end = i;
  598.             }
  599. //com_putchar(ad_datas[i]);
  600.         }
  601.         if (line_begin == -1)
  602.             return -1;
  603.         if (line_end == -1)
  604.             return line_begin * 10;

  606.         return 10*line_begin + (line_end - line_begin) * 10 * ad_datas[line_begin] / (ad_datas[line_begin] + ad_datas[line_end]);
  607. */

  609.         line_begin = 0;
  610. //        ad_datas[0] = ad_datas[0] * 100 / ad_datas_check[0];
  611.         grayscale_min = 500;
  612.         for (i = 1; i < 8; i++) {
  613. //com_putchar(ad_datas[i]);
  614. //com_putchar(ad_datas_check[i]);
  615.             tmp = (ad_datas[i] * 100);
  616.             tmp = tmp / ad_datas_check[i];
  617.             ad_datas[i] = tmp;
  618.             if (ad_datas[i] <= grayscale_min)
  619.             {
  620.                 grayscale_min = ad_datas[i];
  621.                 line_begin = i;
  622.             }
  623. //com_putchar(ad_datas[i]);
  624.         }

  626.         if (grayscale_min < 50)  //灰度<50%認為是有效黑線
  627.         {
  628.             if (line_begin == 0)
  629.                 line_end = 1;
  630.             else if (line_begin == 7)
  631.             {
  632.                 line_begin = 6;
  633.                 line_end = 7;
  634.             } else if (ad_datas[line_begin - 1] < ad_datas[line_begin + 1])
  635.             {
  636.                 line_end = line_begin;
  637.                 line_begin = line_begin - 1;
  638.             } else
  639.                 line_end = line_begin + 1;
  640.             if (ad_datas[line_begin] == 0)
  641.                 ad_datas[line_begin] = 1;
  642.             if (ad_datas[line_end] == 0)
  643.                 ad_datas[line_end] = 1;
  644.             return 10*line_begin + (line_end - line_begin) * 10 * ad_datas[line_begin] / (ad_datas[line_begin] + ad_datas[line_end]);

  646.         } else
  647.             return -1;
  648. }

  650. void track_line(PID *pid) {  //未使用PID算法的版本,四驅(qū)車用PID算法效果差些,需要改進,三輪車用PID效果還可以.
  651.     int line_pos;

  653.     line_pos = read_black_line();
  654.     if (line_pos == -1)
  655.     {
  656.         return;
  657.     }
  658. com_putchar(line_pos);

  660.     if (line_pos > 45)
  661.     {
  662.         pwm_moto_left = 255;
  663.         pwm_moto_right = 255;

  665.         moto_left_forward = 0;
  666.         moto_right_forward = 1;
  667.     } else if (line_pos < 25) {
  668.         pwm_moto_left = 255;
  669.         pwm_moto_right = 255;

  671.         moto_left_forward = 1;
  672.         moto_right_forward = 0;
  673.     } else
  674.     {
  675.         pwm_moto_left = GAIN;
  676.         pwm_moto_right = GAIN;

  678.         moto_left_forward = 1;
  679.         moto_right_forward = 1;
  680.     }
  681. }

  683. /*
  684. void track_line(PID *pid) {  //使用PID算法的版本,三輪小車效果比較不錯.
  685.     int p; //P增益
  686.     int i; //I增益
  687.     int d; //D增益
  688.     int gain; //控制量

  690.     pid->position = read_black_line();
  691.     if (pid->position == -1)
  692.     {
  693. //        pwm_moto_left = 0;
  694. //        pwm_moto_right = 0;
  695. //        pid_init(pid);
  696.         return;
  697.     }
  698. com_putchar(pid->position);

  700.     pid->position -= 30;

  702.     pid->hisPosition += pid->position;                 //記錄偏差之和

  704.     pid->lastPosition[2] = pid->lastPosition[1]; //
  705.     pid->lastPosition[1] = pid->lastPosition[0]; //
  706.     pid->lastPosition[0] = pid->position;            //

  708.     p = (pid->p) * (pid->position); //計算比例分量(P)=比例系數(shù)*本次位置差
  709.     i = (pid->i) * (pid->hisPosition); //計算積分分量(I)=積分系數(shù)*偏差之和
  710.     d = (pid->d) * ((pid->position) - (pid->lastPosition[2])); //計算微分分量(D)=微分系數(shù)*(本次位置差-前3次的位置差)
  711.                                                                // 由于采樣比較快，用本次位置-前3次位置才有足夠大的控制
  712.     gain = p + i + d; //P分量和D分量相加，得到控制量

  714. //    if (gain > GAIN)
  715. //        gain = GAIN;
  716. //    if (gain < -GAIN)
  717. //        gain = -GAIN;



  721.     if (gain > 30)
  722.     {
  723.         pwm_moto_left = GAIN + gain;
  724.         pwm_moto_right = GAIN + gain;

  726.         moto_left_forward = 0;
  727.         moto_right_forward = 1;
  728.     } else if (gain < -30) {
  729.         pwm_moto_left = GAIN - gain;
  730.         pwm_moto_right = GAIN - gain;

  732.         moto_left_forward = 1;
  733.         moto_right_forward = 0;
  734.     } else
  735.     {
  736.         pwm_moto_left = GAIN - gain;
  737.         pwm_moto_right = GAIN -gain;

  739.         moto_left_forward = 1;
  740.         moto_right_forward = 1;
  741.     }
  742. }
  743. */
  744. void main (void)
  745. {
  746.     unsigned int i;

  748.     P2M0 = 0x3C;   //P2.2~P2.5  強推挽輸出
  749.     P3M0 = 0x30;   //P3.4,P3.5強輸出.

  751.     P1ASF = 0xff;   //P1.7~P1.0用做AD

  753.     i = 0;
  754.     MOTO_IN_A1 = 0;
  755.     MOTO_IN_A2 = 0;
  756.     MOTO_IN_B1 = 0;
  757.     MOTO_IN_B2 = 0;

  759.    
  760.     EA = 0;

  762.     power_stat = 0;

  764.     time0_initialize();

  766. //    com_initialize ();    /* initialize interrupt driven serial I/O */
  767. //    com_baudrate (14400); /* setup for 14400 baud */
  768. /*
  769.     CMOD = 0x01;          // #00000001B,PCA空閑計數(shù),PCA計數(shù)源=Fosc/12,PCA溢出中斷(做一個定時器使用)
  770.     CCON = 0x00;          //PCA中斷標志清0,PCA停止計數(shù)
  771.     CL = 0x0;
  772.     CH = 0x0;

  774.     CCAPM1 = 0x31;        //PCA1上升下降沿捕獲
  775. */

  777.     CMOD = 0x03;          // #00000011B,PCA空閑計數(shù),PCA計數(shù)源=fosc/2,PCA溢出中斷
  778.     CCON = 0x00;          //PCA中斷標志清0,PCA停止計數(shù)
  779.     CL = 0x0;
  780.     CH = 0x0;

  782.     CCAPM0 = 0x31;        //PCA0上升下降沿捕獲

  784. /*
  785.     CCAPM0 = 0x42;        //PCA0工作模式:8位pwm
  786.     PCA_PWM0 = 0x00;
  787.     CCAP0L = 0x40;        //25%占空比,可調(diào)節(jié)占空比來調(diào)節(jié)發(fā)射功率.低電平驅(qū)動.
  788.     CCAP0H = 0x40;
  789. */


  792. //    EPCA_LVD = 1;         //允許PCA和低壓檢測中斷  
  793.     ELVD = 1;

  795.     car_stat = 0x00;
  796.     pca_tick = 0;
  797.     pca_int_count = 0;
  798.     frame_dog = 0;


  801.     EA = 1;                         /* Enable Interrupts */
  802.     CR = 1;                         //啟動PCA計數(shù)

  804.     now = pca_tick;

  806.     delay_ms(200);

  808.     read_sensors_check();

  810.     while (1)
  811.     {
  812.         if (power_stat) {
  813.          //   auto_power_down();  //自動關(guān)機
  814.         }
  815.         switch (car_stat) {
  816.         case 0:
  817. /*
  818.             MOTO_IN_A1 = 0;
  819.             MOTO_IN_A2 = 0;
  820.             MOTO_IN_B1 = 0;
  821.             MOTO_IN_B2 = 0;
  822. */
  823.             pwm_moto_left = 0;
  824.             pwm_moto_right = 0;
  825.             break;
  826.         case 1:
  827.             if (!front_obj) {
  828. /*
  829.                 MOTO_IN_A1 = 0;
  830.                 MOTO_IN_A2 = 1;
  831.                 MOTO_IN_B1 = 0;
  832.                 MOTO_IN_B2 = 1;
  833. */
  834.                 moto_left_forward = 1;
  835.                 moto_right_forward = 1;
  836.                 pwm_moto_left = 255;
  837.                 pwm_moto_right = 255;
  838.             } else {
  839. /*
  840.                 MOTO_IN_A1 = 1;
  841.                 MOTO_IN_A2 = 0;
  842.                 MOTO_IN_B1 = 1;
  843.                 MOTO_IN_B2 = 0;

  845.                 delay(1000);
  846. */
  847.                 car_stat = 0;
  848.             }
  849.             break;
  850.         case 2:
  851.             if (!back_obj) {
  852.                 moto_left_forward = 0;
  853.                 moto_right_forward = 0;

  855.                 pwm_moto_left = 255;
  856.                 pwm_moto_right = 255;
  857. /*
  858.                 MOTO_IN_A1 = 1;
  859.                 MOTO_IN_A2 = 0;
  860.                 MOTO_IN_B1 = 1;
  861.                 MOTO_IN_B2 = 0;
  862. */
  863.             } else {
  864. /*
  865.                 MOTO_IN_A1 = 0;
  866.                 MOTO_IN_A2 = 1;
  867.                 MOTO_IN_B1 = 0;
  868. ……………………

  870. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼

全部資料51hei下載地址：
四驅(qū)車遙控避障尋線演示程序(GP41QS).zip (291.78 KB, 下載次數(shù): 7)






歡迎光臨 (http://www.raoushi.com/bbs/) Powered by Discuz! X3.1