Code666 (代码贴、代码片段)

[Plain Text] 2 →→→→→进入此内容的聊天室

来自 1, 2026-04-27, 写在 Plain Text, 查看 3 次.
URL http://www.code666.cn/view/cf29365e
  1. #include "stm32f10x.h"
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include "tcs.h"
  5. #include "timer.h"
  6. #include"stdbool.h"
  7. #include "delay.h"
  8.  
  9. #define QTI1_Pin   GPIO_Pin_0
  10. #define QTI2_Pin   GPIO_Pin_1
  11. #define QTI3_Pin   GPIO_Pin_2
  12. #define QTI4_Pin   GPIO_Pin_3
  13. #define QTI15_Pin  GPIO_Pin_15
  14.  
  15.  
  16.  
  17. #define leftservo    GPIO_Pin_6
  18. #define rightservo   GPIO_Pin_7
  19.  
  20. /* -------------------------颜色传感器引脚电平控制定义--------------------------------------*/
  21. #define S0_Write_1()     GPIO_SetBits(GPIOE,GPIO_Pin_4) //写1
  22. #define S0_Write_0()     GPIO_ResetBits(GPIOE,GPIO_Pin_4) //写0
  23. #define S1_Write_1()     GPIO_SetBits(GPIOE,GPIO_Pin_5)
  24. #define S1_Write_0()     GPIO_ResetBits(GPIOE,GPIO_Pin_5)
  25. #define S2_Write_1()     GPIO_SetBits(GPIOE,GPIO_Pin_6)
  26. #define S2_Write_0()     GPIO_ResetBits(GPIOE,GPIO_Pin_6)
  27. #define S3_Write_1()     GPIO_SetBits(GPIOE,GPIO_Pin_8)
  28. #define S3_Write_0()     GPIO_ResetBits(GPIOE,GPIO_Pin_8)
  29. #define LED_Write_1()    GPIO_SetBits(GPIOE,GPIO_Pin_10)
  30. #define LED_Write_0()    GPIO_ResetBits(GPIOE,GPIO_Pin_10)
  31. //OUT 接到PD2
  32.  
  33.  
  34.  /*********读取QTI的电平宏定义*******/
  35. #define PE0_ReadBit()   GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_0)//读PE0上的值
  36. #define PE1_ReadBit()   GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_1)//读PE1上的值
  37. #define PE2_ReadBit()   GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2)//读PE2上的值
  38. #define PE3_ReadBit()   GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_3)//读PE3上的值
  39.  
  40. #define Yellow 3
  41. #define White  1
  42. #define Red    4
  43. #define Black  2
  44. #define Blue   5
  45.  
  46.  
  47. extern int count_tcs230;
  48.  
  49.  
  50. /************************************函数声明************************/
  51. void BlueCarry(void);              //搬蓝色色块到分数区
  52. void BlackCarry(void);             //搬黑色色块到分数区
  53. void YellowCarry(void);            //搬黄色色块到分数区
  54. void WhiteCarry(void);             //搬白色色块到分数区
  55. void RedCarry(void);                //搬红色色块到分数区
  56.  
  57. /*---------------------------------结构变量、宏定义、变量、常量定义------------------------------------------------------------------------------*/
  58. ErrorStatus HSEStartUpStatus;
  59. GPIO_InitTypeDef GPIO_InitStructure;
  60. TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  61. TIM_OCInitTypeDef  TIM_OCInitStructure;
  62. TIM_ICInitTypeDef  TIM_ICInitStructure;
  63. EXTI_InitTypeDef EXTI_InitStructure;
  64. NVIC_InitTypeDef NVIC_InitStructure;
  65. USART_InitTypeDef USART_InitStructure;
  66. USART_ClockInitTypeDef USART_ClockInitStructure;
  67.  
  68.  
  69.  
  70.  
  71.  
  72.  
  73. int f_flag=1;
  74. int g_flag=0;
  75. int h_flag=1;
  76. int i_flag=0;
  77.  
  78.  
  79.  
  80. volatile u16 times;
  81. volatile unsigned int time;            
  82.  
  83. char process=0;
  84. char Ahavesome = 1;//A有无色块标志   //比赛时候可以直接将抽到点的标志全部置1
  85. char Bhavesome = 1;//B有无色块标志
  86. char Chavesome = 1;//C有无色块标志
  87. char Dhavesome = 1;//D有无色块标志
  88. char Ehavesome = 1;//E有无色块标志
  89. char Fhavesome = 0;//F有无色块标志
  90. char Ghavesome = 0;//G有无色块标志
  91. char Hhavesome = 0;//H有无色块标志
  92. char Ihavesome = 0;//I有无色块标志
  93.  
  94. char pointsth=0;//在直线上放第几个色块
  95. char findsekuai = 0;// 已经搬运色块的总个数
  96.  
  97. //char pointsth=3;//在直线上放第几个色块
  98. //char findsekuai = 3;// 已经搬运色块的总个数
  99.  
  100.  
  101.  
  102.  
  103. char Idone=0;
  104. char sekuai = 5;//色块的总个数
  105. u16 pcolor[3]={0,0,0};//颜色传感器的比例因子
  106. u16 RGB[3]={0,0,0};
  107. int hadedone = 1;
  108. unsigned char QTIS;//循线的QTI状态
  109.  
  110. unsigned int color =0;//颜色代表值
  111.  
  112. unsigned int distance=0;//超声波测距
  113.  
  114. unsigned int speed=0;//电机速度
  115. unsigned int pulses=0;//脉冲数
  116. bool colorCheck = true;
  117. volatile char i=0,j=0;
  118. unsigned int i1=0;
  119. unsigned int goal=38;     //搬运到目标点计分区的参数   //数值越大表步数约长
  120. unsigned int a=0;        //用来判断搬fg  和hi颜色是  需要修正的方向
  121. int count = 1;                //颜色识别次数计数
  122. int LRp = 12;
  123. int aim = 7;
  124. bool have=true;
  125. /*************************************
  126.   * @brief  Configure peripheral RCC.
  127.   * @param  None
  128.   * @retval None
  129.   ************************************/
  130. void RCC_Configuration(void)
  131. {
  132.         /* 将外设RCC寄存器重设为默认值，即有关寄存器复位，但该函数不改禦CC_CR的HSITRIM[4:0]位，也不重置寄存器RCC_BDCR和寄存器RCC_CSR */
  133.         RCC_DeInit();
  134.         /* 使能外部HSE高速晶振 */
  135.         RCC_HSEConfig(RCC_HSE_ON);
  136.         /* 等待HSE高速晶振稳定，或者在超时的情况下退出 */
  137.         HSEStartUpStatus = RCC_WaitForHSEStartUp();
  138.         /* SUCCESS:HSE晶振稳定且就绪，ERROR：HSE晶振未就绪 */
  139.         if(HSEStartUpStatus == SUCCESS)
  140.         {
  141.                 /* 使能flash预取指令缓冲区。这两句跟RCC没直接关系 */
  142.                 FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
  143.                 /* 设置FLASH存储器延时时钟周期数，2是针对高频时钟的，
  144.                 FLASH_Latency_0：0延时周期，FLASH_Latency_1：1延时周期
  145.                 FLASH_Latency_2：2延时周期 */
  146.                 FLASH_SetLatency(FLASH_Latency_2);
  147.                 /* HCLK=SYSCLK 设置高速总线时钟=系统时钟 */
  148.                 RCC_HCLKConfig(RCC_SYSCLK_Div1);
  149.                 /* PCLK1=HCLK/2 设置低速总线1时钟=高速时钟的二分频*/
  150.                 RCC_PCLK1Config(RCC_HCLK_Div2);
  151.                 /* PCLK2=HCLK 设置低速总线2时钟=高速总线时钟 */
  152.                 RCC_PCLK2Config(RCC_HCLK_Div1);
  153.                 /* Set PLL clock output to 72MHz using HSE (8MHz) as entry clock */
  154.                 /* 利用锁相环将HSE外部8MHz晶振9倍频到72MHz */
  155.                 RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
  156.                 /* Enable PLL：使能PLL锁相环 */
  157.                 RCC_PLLCmd(ENABLE);
  158.                
  159.                  /* Wait till PLL is ready：等待锁相环输出稳定 */
  160.      /* RCC_FLAG_HSIRDY：HSI晶振就绪，RCC_FLAG_HSERDY：HSE晶振就绪
  161.         RCC_FLAG_PLLRDY：PLL就绪，RCC_FLAG_LSERDY：LSE晶振就绪
  162.         RCC_FLAG_LSIRDY：LSI晶振就绪，RCC_FLAG_PINRST：引脚复位
  163.         RCC_FLAG_PORRST：POR/PDR复位，RCC_FLAG_SFTRST：软件复位
  164.         RCC_FLAG_IWDGRST：IWDG复位，RCC_FLAG_WWDGRST：WWDG复位
  165.         RCC_FLAG_LPWRRST：低功耗复位 */
  166.                 while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)      ;
  167.                
  168.                  /* Select PLL as system clock source：将锁相环输出设置为系统时钟 */
  169.      /* RCC_SYSCLKSource_HSI：选择HSI作为系统时钟
  170.         RCC_SYSCLKSource_HSE：选择HSE作为系统时钟
  171.         RCC_SYSCLKSource_PLLCLK：选择PLL作为系统时钟*/
  172.                 RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
  173.                  /* 等待PLL作为系统时钟标志位置位 */
  174.      /* 0x00：HSI作为系统时钟；0x04：HSE作为系统时钟
  175.         0x08：PLL作为系统时钟 */
  176.                 while(RCC_GetSYSCLKSource() != 0x08);  
  177.         }
  178.        
  179.          /* Enable GPIOA~E and AFIO clocks：使能外围端口总线时钟。注意各外设的隶属情况，不同芯片和开发板的分配不同*/
  180.          RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|
  181.                                                                                                         RCC_APB2Periph_GPIOB|
  182.                                                                                                         RCC_APB2Periph_GPIOC|
  183.                                                                                                         RCC_APB2Periph_GPIOD|
  184.                                                                                                         RCC_APB2Periph_GPIOE|
  185.                                                                                                         RCC_APB2Periph_AFIO, ENABLE);
  186.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);   
  187.         /* TIM3 clock enable */
  188.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  189.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 ,ENABLE);
  190.         /* TIM5 clock enable */
  191.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
  192.  
  193.         /* Enable USART1 clocks */
  194.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
  195.        
  196.        
  197.  
  198. }
  199.  
  200.  
  201. /************************************************
  202.   * @brief  Configure NVIC.
  203.   * @param  None
  204.   * @retval None
  205.   ***********************************************/
  206. void NVIC_Configuration(void)
  207. {
  208.         #ifdef  VECT_TAB_RAM  
  209.           /* Set the Vector Table base location at 0x20000000 */
  210.           NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
  211.         #else  /* VECT_TAB_FLASH  */
  212.           /* Set the Vector Table base location at 0x08000000 */
  213.           NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);  
  214.         #endif
  215.  
  216.         /* Configure the Priority Grouping with 0 bit */
  217.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
  218.        
  219.         /* Configure the Priority Grouping with 1 bit */
  220.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
  221.         /* Enable TIM2 global interrupt with Preemption Priority 1 and Sub
  222.         Priority as 5 */
  223.         NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  224.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  225.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  226.         NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
  227.         NVIC_Init(&NVIC_InitStructure);
  228.  
  229.         NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
  230.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  231.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
  232.         NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
  233.         NVIC_Init(&NVIC_InitStructure);
  234.  
  235.         /* Configure the Priority Grouping with 2 bit */
  236.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  237.         /* Enable TIM3 global interrupt with Preemption Priority 1 and Sub
  238.         Priority as 6 */
  239.         NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
  240.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  241.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  242.         NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
  243.         NVIC_Init(&NVIC_InitStructure);
  244.         /* Configure the Priority Grouping with 2 bit */
  245.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_3);
  246.  
  247.   NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;        //超声波
  248.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
  249.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  250.         NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
  251.         NVIC_Init(&NVIC_InitStructure);
  252. }
  253. /************************************************
  254.   * @brief  Configure the TIM2 Timer for 1ms.
  255.   * @param  None
  256.   * @retval None
  257.   ***********************************************/
  258. void TIM2_Configure(void)
  259. {
  260.    TIM_DeInit( TIM2);//复位TIM2定时器
  261.    TIM_TimeBaseStructure.TIM_Period = 99;
  262.    TIM_TimeBaseStructure.TIM_Prescaler = 7199;
  263.    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  264.    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  265.    TIM_TimeBaseInit(TIM2, & TIM_TimeBaseStructure);
  266.    /* Clear TIM2 update pending flag[清除TIM2溢出中断标志] */
  267.    TIM_ClearFlag(TIM2, TIM_FLAG_Update);
  268.    /* Enable TIM2 Update interrupt [TIM2溢出中断允许]*/
  269.    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);  
  270.    /* TIM2 enable counter [允许tim2计数]*/
  271.    TIM_Cmd(TIM2, DISABLE);
  272. }
  273. /************************************************
  274.   * @brief  Configure the TIM3 Counter.
  275.   * @param  None
  276.   * @retval None
  277.   ***********************************************/
  278. void TIM3_Counter_Configure(void)
  279. {
  280.         TIM_TimeBaseStructure.TIM_Period = 0xFFFF;//设置自动装载寄存器
  281.         TIM_TimeBaseStructure.TIM_Prescaler = 0x00;//分频计数
  282.         TIM_TimeBaseStructure.TIM_ClockDivision = 0x00;
  283.         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//选择向上计数
  284.         TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);// Time base configuration
  285.  
  286.         TIM_ETRClockMode2Config(TIM3, TIM_ExtTRGPSC_OFF, TIM_ExtTRGPolarity_NonInverted, 0);
  287.  
  288.         TIM_SetCounter(TIM3, 0);//设置计数器的值
  289.         TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);//使能定时器中断
  290.         TIM_Cmd(TIM3, DISABLE); //使能定时器
  291. }
  292. void TIM4_Configuration(void)
  293. {
  294.   TIM_DeInit(TIM4);
  295.  
  296.   /* Time Base configuration */
  297.   TIM_TimeBaseStructure.TIM_Prescaler = 0;
  298.   TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  299.   TIM_TimeBaseStructure.TIM_Period = 0xffff;
  300.   TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  301.   TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
  302.   TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);  
  303.   TIM_PrescalerConfig(TIM4,71, TIM_PSCReloadMode_Immediate);  
  304.    /* enable preload value */
  305.   TIM_ARRPreloadConfig(TIM4,DISABLE);  
  306.   TIM_SetCounter(TIM4,0);                        //TIM4计数值清零
  307.   TIM_Cmd(TIM4,DISABLE);                         //TIM4计数器失能
  308. }
  309.  
  310. void TIM5_Configure(void)
  311. {
  312.    TIM_DeInit(TIM5);//复位TIM5定时器
  313.        
  314.    TIM_TimeBaseStructure.TIM_Period = 9;
  315.    TIM_TimeBaseStructure.TIM_Prescaler = 7199;
  316.          
  317.        
  318.        
  319.        
  320.    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  321.    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  322.    TIM_TimeBaseInit(TIM5, & TIM_TimeBaseStructure);
  323.    /* Clear TIM2 update pending flag[清除TIM2溢出中断标志] */
  324.    TIM_ClearFlag(TIM5, TIM_FLAG_Update);
  325.    /* Enable TIM2 Update interrupt [TIM2溢出中断允许]*/
  326.    TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE);  
  327.    /* TIM2 enable counter [允许tim2计数]*/
  328.    TIM_Cmd(TIM5, DISABLE);
  329. }
  330. /************************************************
  331.   * @brief  Configure USART1.
  332.   * @param  None
  333.   * @retval None
  334.   ***********************************************/
  335. void USART1_Configuration(void)
  336. {
  337.         USART_InitStructure.USART_BaudRate = 115200;
  338.         USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  339.         USART_InitStructure.USART_StopBits = USART_StopBits_1;
  340.         USART_InitStructure.USART_Parity = USART_Parity_No;
  341.         USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  342.         USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
  343.         USART_Init(USART1, &USART_InitStructure);
  344.  
  345.         USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;
  346.         USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;
  347.         USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;
  348.         USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;
  349.         USART_ClockInit(USART1, &USART_ClockInitStructure);
  350.  
  351.         /* Enables the USART1 transmit interrupt */
  352.         USART_ClearFlag(USART1, USART_FLAG_TC);  
  353.         /* Enable the USART1 */
  354.         USART_Cmd(USART1, ENABLE);
  355. }
  356.  
  357. /************************************************
  358.   * @brief  Configure the TCS230 Pins.
  359.   * @param  None
  360.   * @retval None
  361.   ***********************************************/
  362. void GPIO_TCS230_Configure(void)
  363. {
  364.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_7|GPIO_Pin_6|GPIO_Pin_8|GPIO_Pin_10;
  365.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;    
  366.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  
  367.         GPIO_Init(GPIOE, &GPIO_InitStructure);  
  368.        
  369.        
  370.  
  371. }
  372. /***********************************************
  373.   * @brief  Configure USART1 Pins.
  374.   * @param  None
  375.   * @retval None
  376.   ***********************************************/
  377. void GPIO_USART1_Configure(void)
  378. {
  379.         /*USART1_TX*/
  380.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
  381.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  382.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  383.         GPIO_Init(GPIOA, &GPIO_InitStructure);
  384.        
  385.         /*USART1_RX*/
  386.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
  387.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  388.         GPIO_Init(GPIOA, &GPIO_InitStructure); 
  389. }
  390. /************************************************
  391.   * @brief  Configure the TIM3 Pins.
  392.   * @param  None
  393.   * @retval None
  394.   ***********************************************/
  395. void GPIO_TIM3_Configure(void)
  396. {
  397.         //设置TIM3的外部计数器 PD2  ETR  （引脚的设置跟硬件有关）
  398.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
  399.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  400.   GPIO_Init(GPIOD, &GPIO_InitStructure);
  401. }
  402. void GPIO_Dist_Config(void)
  403. {                              
  404.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
  405.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  406.         GPIO_Init(GPIOC, &GPIO_InitStructure);
  407.  
  408.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
  409.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  410.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  411.         GPIO_Init(GPIOC, &GPIO_InitStructure);
  412. }
  413. void GPIO_Motor_Config(void)
  414. {
  415.   /* Configure Motors IO*/
  416.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
  417.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  418.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  419.   GPIO_Init(GPIOC, &GPIO_InitStructure);
  420.  
  421.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
  422.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  423.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  424.   GPIO_Init(GPIOC, &GPIO_InitStructure);
  425. }
  426. void GPIO_QTI_Config(void)
  427. {
  428.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_15;
  429.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  430.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;   //浮空输入
  431.   GPIO_Init(GPIOE,&GPIO_InitStructure);
  432. }
  433.  
  434. u8 QTI_State(u8 pin)//获取红外值
  435. {
  436.         return GPIO_ReadInputDataBit(GPIOE,pin);        
  437. }
  438.  
  439.  
  440. /**************************************************
  441. * Function Name  : fputc
  442. * Description    : Retargets the C library printf function to the USART.
  443. **************************************************/
  444. int fputc(int ch, FILE *f)
  445. {
  446.         /* Place your implementation of fputc here */
  447.         USART1->SR;//不可少，否则出现首次发送首字符丢失
  448.         /* e.g. write a character to the USART */
  449.         USART_SendData(USART1, (u8) ch);
  450.         /* Loop until the end of transmission */
  451.         while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);  // waiting here
  452.        
  453.         return ch;
  454. }
  455.  
  456. void delay_nus(unsigned int n)  //延时n us: n>=6,最小延时单位6us
  457. {
  458.   unsigned int j;
  459.   while(n--)              // 外部晶振：8M；PLL：9；8M*9=72MHz
  460.   {
  461.     j=8;                                  // 微调参数，保证延时的精度
  462.         while(j--);
  463.   }
  464. }
  465.  
  466. void delay_nms(unsigned int n)  //延时n ms
  467. {
  468.   while(n--)               // 外部晶振：8M；PLL：9；8M*9=72MHz
  469.     delay_nus(1100);   // 1ms延时补偿
  470. }
  471. void motor_motion1(unsigned int left_val, unsigned int right_val, unsigned int count)     //左     右     计次
  472. {
  473.    unsigned int i;
  474.    for(i=0; i<count; i++)
  475.    {
  476.                         GPIO_SetBits(GPIOC, GPIO_Pin_7);
  477.                         delay_nus(left_val);
  478.                         GPIO_ResetBits(GPIOC,GPIO_Pin_7);
  479.  
  480.                         GPIO_SetBits(GPIOC, GPIO_Pin_6);
  481.                         delay_nus(right_val);
  482.                         GPIO_ResetBits(GPIOC,GPIO_Pin_6);
  483.  
  484.                         delay_nms(20);
  485.    }                                                     
  486. }
  487. /*******************************************************************************
  488.   *TCS230_CurrentColor(u16 pRGB[3],u16 rgb[3])
  489.   *  读取颜色色块RGB值，分别存在数组RGB[3]={0,0,0}内
  490.   ********************************************************************************/
  491. u8 TCS230_CurrentColor(u16 pRGB[3])
  492. {
  493.         u8 currentcolor=0;
  494.         S0_Write_0();   S1_Write_1();           //输出频率为(1/50)*500KHz=10KHz   
  495.         LED_Write_1();          //打开LED
  496.        
  497.         S2_Write_0();   S3_Write_0();           //选择红色
  498.        
  499.         times=0;
  500.         TIM_SetCounter(TIM3,0);
  501.         TIM_Cmd(TIM2, ENABLE);
  502.         TIM_Cmd(TIM3, ENABLE);
  503.         while(pRGB[0] != times);
  504.         TIM_Cmd(TIM2, DISABLE);
  505.         TIM_Cmd(TIM3, DISABLE);
  506.         RGB[0] = TIM_GetCounter(TIM3);
  507.        
  508.         S3_Write_1();//选择蓝色
  509.        
  510.         times=0;TIM_SetCounter(TIM3,0);
  511.         TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
  512.         while(pRGB[1] != times);
  513.         TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
  514.         RGB[1] = TIM_GetCounter(TIM3); 
  515.        
  516.         S2_Write_1();//选择绿色
  517.        
  518.         times=0;TIM_SetCounter(TIM3,0);
  519.         TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
  520.         while(pRGB[2] != times);
  521.         TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
  522.         RGB[2] = TIM_GetCounter(TIM3);
  523.        
  524.         LED_Write_0();//关闭LED      
  525.        
  526.         printf("Red: %d  Blue: %d  Green: %d\n",RGB[0],RGB[1],RGB[2]);
  527.         return currentcolor;
  528. }
  529. /*******************************************************************************
  530.   * @brief  TCS230_WhiteBalance Function.
  531.   *  Description计算白色色块RGB值到达255时，对应时间数分别存储在数组pColor[]里
  532.   ********************************************************************************/
  533. void TCS230_WhiteBalance(u8 pColor[3])                   //白平衡初始化
  534. {
  535.         S0_Write_0();  
  536.         S1_Write_1();//输出频率为(1/50)*500KHz=10KHz
  537.         LED_Write_1();//打开LED
  538.        
  539.         S2_Write_0();  
  540.         S3_Write_0();//选择红色
  541.  
  542.         times=0;TIM_SetCounter(TIM3,0);//计数器3清除
  543.         TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
  544.         while(TIM_GetCounter(TIM3)<255);
  545.         TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
  546.         pColor[0] = times;//时间比例因子 
  547.        
  548.         S3_Write_1();//选择蓝色
  549.  
  550.         times=0;TIM_SetCounter(TIM3,0);
  551.         TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
  552.         while(TIM_GetCounter(TIM3)<255);
  553.         TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
  554.         pColor[1] = times;//时间比例因子 
  555.  
  556.         S2_Write_1();//选择绿色
  557.  
  558.         times=0;TIM_SetCounter(TIM3,0);
  559.         TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
  560.         while(TIM_GetCounter(TIM3)<255);
  561.         TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
  562.         pColor[2] = times;//时间比例因子 
  563.        
  564.         LED_Write_0();//关闭LED      
  565.        
  566.         printf("Red**: %d  Blue**: %d  Green**: %d\n",pColor[0],pColor[1],pColor[2]);
  567. }
  568. /*******************************************************************************
  569.   * @brief  Robot_checkColor(void)
  570.   *  读取颜色色块RGB值，并根据读取的RGB值转换成颜色
  571.   ********************************************************************************/
  572.  unsigned int Robot_checkColor(void)            
  573.  {
  574.           TCS230_CurrentColor(pcolor);                      //读取颜色色块的RGB值
  575.           printf("Red: %d  Blue: %d  Green: %d\n",RGB[0],RGB[1],RGB[2]);
  576.                 //判断颜色                                             //通过RGB值判断颜色
  577.                         if((RGB[0] > 90)&&((RGB[0] - RGB[2]) > 60) && ((RGB[0] - RGB[1]) > 60)&&((RGB[2] - RGB[1]) < 80))
  578.                 {
  579.                         return 3;//红色
  580.                 }
  581.                 else if(((abs(RGB[0] - RGB[2])<50)&&(abs(RGB[0] - RGB[1])<50)&& (RGB[0]>100) ) ||(  RGB[1]-RGB[2]<15 && RGB[0]>200 && (RGB[1]<80)) || ( (RGB[0]>200)&& (RGB[2]>200)&&(RGB[1]<15)      )  )
  582.                 {
  583.                         return 2;//白色
  584.                 }
  585.                 else if((abs(RGB[0] - RGB[2])<30)&&(abs(RGB[0] - RGB[1])<30)&& (RGB[0]<40)&& (RGB[1]<40)&& (RGB[2]<40) )
  586.                 {
  587.                         return 4;//黑色
  588.                 }
  589.                 else if(        ((RGB[0] < RGB[1]) && (RGB[2] < RGB[1])&&((RGB[1] - RGB[0])>10)) || ((RGB[0] < RGB[1]) && (RGB[2] < RGB[1])&&((RGB[1] - RGB[0])>5))     )
  590.                 {
  591.                         return 5;//蓝色
  592.                 }
  593.                 else if( (((RGB[2] - RGB[1]) > 80) && ((RGB[1] - RGB[0]) > 80)&&(RGB[2] > 90)) || (((RGB[2] - RGB[1]) > 50) && ((RGB[0]-RGB[1]) > 65) && RGB[1]>80   ) )
  594.                 {
  595.                         return 1;//黄色
  596.                 }
  597.                 return 0;
  598.                
  599.                
  600.  }
  601. int GetDis(int echo,int trig)     //获取超声波所测物块距离值
  602. {
  603.   int dis;
  604.         int count;
  605.         GPIO_ResetBits(GPIOC,echo);                     //echo端口复位
  606.         GPIO_ResetBits(GPIOC,trig);                     //trig端口复位
  607.   TIM_SetCounter(TIM4,0);                        //TIM4计数值清零
  608.         GPIO_SetBits(GPIOC,trig);                         //trig置高 发出10us的高电平信号
  609.         delay_nus(10);
  610.         GPIO_ResetBits(GPIOC,trig);
  611.         delay_nus(100);                  
  612.         while(GPIO_ReadInputDataBit(GPIOC, echo) == 0);
  613.         TIM_Cmd(TIM4,ENABLE);    //开启计数器
  614.        //开启定时器开始计时
  615.         while(GPIO_ReadInputDataBit(GPIOC, echo));   //等待echo置低
  616.         TIM_Cmd(TIM4,DISABLE);   //关闭计数器
  617.  
  618.         count = TIM_GetCounter(TIM4);//获取计数器值
  619.         dis = (int)count/60.034;//转换为距离,即29.034us超声波能传播1cm
  620.         return dis;
  621. }
  622.  
  623. /*
  624.  * //检测中间左QTI是否在黑线内
  625.  */
  626. bool IsMLeftQtiBlack2(void)   //检测中间左QTI是否在黑线内
  627. {
  628.         if(QTI_State(QTI3_Pin)==true)    //判断中间左QTI是否检测到黑线
  629.         {                                //在黑线返回ture    不在黑线返回false  
  630.                 delay_nms(2);
  631.                 if(QTI_State(QTI3_Pin)==true)
  632.                 {
  633.                         return true;  
  634.                 }
  635.           else
  636.                 {
  637.                         return false;  
  638.                 }
  639.         }
  640.         else
  641.         {
  642.                 return false;
  643.         }
  644. }
  645. bool IsMLeftQtiBlack(void)   //检测中间左QTI是否在黑线内
  646. {
  647.         if(QTI_State(QTI1_Pin)==true)    //判断中间左QTI是否检测到黑线
  648.         {                                //在黑线返回ture    不在黑线返回false  
  649.                 delay_nms(2);
  650.                 if(QTI_State(QTI1_Pin)==true)
  651.                 {
  652.                         return true;  
  653.                 }
  654.           else
  655.                 {
  656.                         return false;  
  657.                 }
  658.         }
  659.         else
  660.         {
  661.                 return false;
  662.         }
  663. }
  664. bool IsMRightQtiBlack(void)  //检测中间右QTI是否在黑线内
  665. {
  666.         if(QTI_State(QTI2_Pin)==true)   //判断中间左QTI是否检测到黑线
  667.         {                               //在黑线返回ture    不在黑线返回false  
  668.                 delay_nms(2);
  669.                 delay_nms(2);
  670.                 if(QTI_State(QTI2_Pin)==true)
  671.                 {
  672.                         return true;
  673.                 }
  674.                 else
  675.                 {
  676.                         return false;
  677.                 }
  678.         }
  679.         else
  680.         {
  681.                 return false;
  682.         }
  683. }
  684.  
  685.  
  686.  
  687.  
  688. bool IsMRightQtiBlack1(void)  //检测中间右QTI是否在黑线内
  689. {
  690.         if(QTI_State(QTI3_Pin)==true)   //判断中间左QTI是否检测到黑线
  691.         {                               //在黑线返回ture    不在黑线返回false  
  692.                 delay_nms(2);
  693.                 delay_nms(2);
  694.                 if(QTI_State(QTI3_Pin)==true)
  695.                 {
  696.                         return true;
  697.                 }
  698.                 else
  699.                 {
  700.                         return false;
  701.                 }
  702.         }
  703.         else
  704.         {
  705.                 return false;
  706.         }
  707. }
  708.  
  709. void PulseOut(uint8_t pin,int speed)    //单轮旋转设置
  710. {
  711.         GPIO_SetBits(GPIOC,pin );
  712.         delay_nus(speed);
  713.         GPIO_ResetBits(GPIOC,pin);
  714. }
  715.  
  716. void stop(void)//停止
  717. {
  718.         int i;
  719.         for(i=1;i<=2;i++)
  720.         {
  721.         GPIO_SetBits(GPIOC, GPIO_Pin_6);
  722.         delay_nus(1500);
  723.         GPIO_ResetBits(GPIOC,GPIO_Pin_6);
  724.        
  725.                 GPIO_SetBits(GPIOC, GPIO_Pin_7);
  726.                 delay_nus(1500);
  727.                 GPIO_ResetBits(GPIOC,GPIO_Pin_7);
  728.  
  729.                 delay_nms(20);
  730.         }      
  731. }
  732.  
  733. void SpinLeft(void)// 左旋转
  734. {
  735.         PulseOut(leftservo,1480);
  736.         PulseOut(rightservo,1480);
  737.         delay_nms(20);
  738. }
  739.  
  740. void SpinRight(void)// 右旋转
  741. {
  742.         PulseOut(leftservo,1520);
  743.         PulseOut(rightservo,1520);
  744.         delay_nms(20);
  745. }
  746.  
  747. void TurnLeftAnyPulse(int pulses)//左旋转某个脉冲     pluses设置旋转角度
  748. {
  749.         while(pulses--)
  750.         {
  751.                 SpinLeft();      //左旋转
  752.                 delay_nms(2);
  753.         }
  754. }
  755. void TurnRightAnyPulse(int pulses) //右旋转某个脉冲   pluses设置旋转角度
  756. {
  757.         while(pulses--)
  758.         {
  759.                 SpinRight();
  760.                 delay_nms(2);
  761.         }
  762. }
  763.  
  764. void findLline2(void)  //找左边下一根线
  765. {
  766.         TurnLeftAnyPulse(8);
  767.         while(1)
  768.         {
  769.                 SpinLeft();                        //左旋转
  770.                 if(IsMLeftQtiBlack2())              //  中间左是否检测到黑线
  771.                 {
  772.                         i1++;
  773.                         if(i1==2)
  774.                         {
  775.                                 i1=0;
  776.                                 break;
  777.                         }
  778.                 }
  779.         }
  780.         while(1)
  781.         {
  782.                 SpinLeft();          
  783.                 if(!IsMLeftQtiBlack2())
  784.                 {
  785.                         i1++;
  786.                         if(i1==2)
  787.                         {
  788.                                 i1=0;
  789.                                 break;
  790.                         }
  791.                 }
  792.         }
  793.         stop();
  794. }
  795. void findLline(void)  //找左边下一根线
  796. {
  797.         TurnLeftAnyPulse(8);
  798.         while(1)
  799.         {
  800.                 SpinLeft();                        //左旋转
  801.                 if(IsMLeftQtiBlack())              //  中间左是否检测到黑线
  802.                 {
  803.                         i1++;
  804.                         if(i1==2)
  805.                         {
  806.                                 i1=0;
  807.                                 break;
  808.                         }
  809.                 }
  810.         }
  811.         while(1)
  812.         {
  813.                 SpinLeft();          
  814.                 if(!IsMLeftQtiBlack())
  815.                 {
  816.                         i1++;
  817.                         if(i1==2)
  818.                         {
  819.                                 i1=0;
  820.                                 break;
  821.                         }
  822.                 }
  823.         }
  824.         stop();
  825. }
  826. void findRline(void) //找右边下一根线
  827. {
  828.         TurnRightAnyPulse(10);
  829.         while(1)
  830.         {
  831.                 SpinRight();
  832.                 if(IsMRightQtiBlack())
  833.                 {
  834.                         i1++;
  835.                         if(i1==2)
  836.                         {      
  837.                                 i1=0;
  838.                                 break;
  839.                         }
  840.                 }
  841.         }
  842. //      stop();
  843.         while(1)
  844.         {
  845.                 SpinRight();
  846.                 if(!IsMRightQtiBlack())
  847.                 {
  848.                         i1++;
  849.                         if(i1==2)
  850.                         {
  851.                                 i1=0;
  852.                                 break;
  853.                         }
  854.                 }
  855.         }
  856. //      stop();
  857. }
  858. void findRline1(void) //找右边下一根线
  859. {
  860.         TurnRightAnyPulse(10);
  861.         while(1)
  862.         {
  863.                 SpinRight();
  864.                 if(IsMRightQtiBlack1())
  865.                 {
  866.                         i1++;
  867.                         if(i1==2)
  868.                         {      
  869.                                 i1=0;
  870.                                 break;
  871.                         }
  872.                 }
  873.         }
  874. //      stop();
  875.         while(1)
  876.         {
  877.                 SpinRight();
  878.                 if(!IsMRightQtiBlack1())
  879.                 {
  880.                         i1++;
  881.                         if(i1==2)
  882.                         {
  883.                                 i1=0;
  884.                                 break;
  885.                         }
  886.                 }
  887.         }
  888. //      stop();
  889. }
  890. /*
  891. //函数名称：PE0_state()
  892. //功能： 获得左边第一个QTI的返回信号
  893. //参数：无参数
  894. //返回值：1：高电平，看到黑线；0：低电平，看到白线
  895. */
  896.  
  897. int PE0_state(void)
  898. {      
  899.         return PE0_ReadBit();
  900. }
  901.  
  902. /*
  903. 函数名称：PE1_state()
  904. 功能： 获得左边第二个QTI的返回信号
  905. 参数：无参数
  906. 返回值：1：高电平，看到黑线；0：低电平，看到白线
  907. */
  908. int PE1_state(void)
  909. {
  910.         return PE1_ReadBit();
  911. }
  912.  
  913. /*
  914. 函数名称：PE2_state()
  915. 功能： 获得右边第二个QTI的返回信号
  916. 参数：无参数
  917. 返回值：1：高电平，看到黑线；0：低电平，看到白线
  918. */
  919. int PE2_state(void)
  920. {
  921.         return PE2_ReadBit();
  922. }
  923.  
  924. /*
  925. 函数名称：PE3_state()
  926. 功能： 获得右边第一个QTI的返回信号
  927. 参数：无参数
  928. 返回值：1：高电平，看到黑线；0：低电平，看到白线
  929. */
  930. int PE3_state(void)
  931. {
  932.         return PE3_ReadBit();
  933. }
  934. void motor_motion2(unsigned int left2_val, unsigned int right2_val)    
  935. {
  936.         GPIO_SetBits(GPIOC, GPIO_Pin_7);
  937.         delay_nus(left2_val);
  938.         GPIO_ResetBits(GPIOC,GPIO_Pin_7);
  939.  
  940.         GPIO_SetBits(GPIOC, GPIO_Pin_6);
  941.         delay_nus(right2_val);
  942.         GPIO_ResetBits(GPIOC,GPIO_Pin_6);
  943.  
  944.         delay_nms(20);  //输出一定数量的PWM波形
  945. }
  946. void Robot_hunting1(unsigned int speed)            
  947. {
  948.          QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  949.          
  950.          switch (QTIS)
  951.                 {
  952.                  //case 0:motor_motion1(speed, (3000-speed)-35-10,3);break;
  953.                         case 1:motor_motion1(speed, (3000-speed)-35-10,3);break;
  954.                         case 2:motor_motion1(speed, (3000-speed)-35,1);break;
  955.                         case 3:motor_motion1(speed, (3000-speed)-35,1);break;
  956.                         case 4:motor_motion1(speed+35, (3000-speed),1);break;
  957.                
  958.                         case 8:motor_motion1(speed+35+10, (3000-speed),3);break;
  959.                         case 12:motor_motion1(speed+35, (3000-speed),2);break;
  960.                
  961.                         case 6:motor_motion2(speed+100, 3000-speed-100-2);break;       
  962.                         default:motor_motion2(1550, 1450);break;         
  963.                 }
  964. }
  965. void Robot_hunting2(unsigned int speed)              
  966. {                                                  
  967.          QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);      
  968.          
  969.          switch (QTIS)
  970.                 {
  971.                         case 1:motor_motion1(1500, 1400,2);break;
  972.                         case 2:motor_motion1(1500, 1400,1);break;
  973.                         case 3:motor_motion1(1500, 1400,1);break;
  974.  
  975.                         case 4:motor_motion1(1600, 1500,1);break;
  976.                         case 8:motor_motion1(1600, 1500,2);break;
  977.                         case 12:motor_motion1(1600, 1500,1);break;
  978.                         case 6:motor_motion2(speed, 3000-speed);break;   
  979.                         default:motor_motion2(speed, 3000-speed);break;    
  980.                 }
  981. }
  982. void Robot_hunting(unsigned int speed)              
  983. {
  984.          QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  985.          
  986.          switch (QTIS)
  987.                 {
  988.                         case 1:motor_motion1(speed, (3000-speed)-40-10,2);break;//左转
  989.                         case 2:motor_motion1(speed, (3000-speed)-40,1);break;//左转
  990.                         case 3:motor_motion1(speed, (3000-speed)-40,1);break;//左转
  991.  
  992.                         case 4:motor_motion1(speed+40, (3000-speed),1);break;//右转
  993.                         case 8:motor_motion1(speed+40+10, (3000-speed),2);break;//右转
  994.                         case 12:motor_motion1(speed+40, (3000-speed),1);break;//右转
  995.                        
  996.                         case 6:motor_motion2(speed, 3000-speed-2);break;          //直行
  997.                         default:motor_motion2(speed, 3000-speed-2);break;          //这一句不能省
  998.                 }
  999. }
  1000. void Peripheral_Init(void)    //各模块初始化配置函数
  1001. {
  1002.                 RCC_Configuration();     //时钟、外设使能
  1003.                 NVIC_Configuration();    //中断优先级划分
  1004.                 GPIO_USART1_Configure(); //串口1 IO口配置
  1005.                 USART1_Configuration();  //串口1配置
  1006.                 GPIO_TCS230_Configure();  //颜色传感器IO口配置
  1007.                 TIM2_Configure();        // 定时器2配置
  1008.                 GPIO_TIM3_Configure(); //定时器3外部计数引脚配置
  1009.                 TIM3_Counter_Configure();//     定时器3计数器配置
  1010.                 TIM5_Configure();         //   定时器5配置
  1011.                 GPIO_Motor_Config();  //两轮舵机IO引脚配置
  1012.                 GPIO_QTI_Config();        //QTI输入引脚配置
  1013.                 TIM4_Configuration(); //定时器4配置
  1014.                 GPIO_Dist_Config();     //
  1015. }
  1016. /*转弯控制函数*/
  1017. void TurnR_toBlackLine(void)                  //右转寻找下一条黑线
  1018. {        
  1019.                 motor_motion1(1515, 1515,5);              //右转
  1020.                 do
  1021.                         {
  1022.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);  
  1023.                                 motor_motion1(1515, 1515,1);         
  1024.                         } while(QTIS!=6);                       //检测循迹是否为1001  是则停止右转
  1025.                 do
  1026.                         {
  1027.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);  
  1028.                                 //motor_motion1(1515, 1515,1); 
  1029.                         } while(QTIS!=6);                     //再次检测循迹是否为1001  
  1030.                 motor_motion1(1515, 1515,1);
  1031. }
  1032.  
  1033. void TurnL_toBlackLine(void)                   //左转寻找下一条黑线
  1034. {      
  1035.                 motor_motion1(1485, 1485,10);
  1036.                 do{
  1037.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1038.                                 motor_motion1(1485, 1485,1);   
  1039.                         } while(QTIS!=6);                    //检测循迹是否为1001  是则停止右转
  1040.                 delay_nms(5);
  1041.                 do{
  1042.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1043.                                 //motor_motion1(1485, 1485,1); 
  1044.                         } while(QTIS!=6);                   //再次检测循迹是否为1001
  1045.                 motor_motion1(1485, 1485,1);           //左偏一点
  1046. }
  1047. void Turn1_180(void)                    //   快速旋转  //先向左偏移一定角度，然后向左旋转寻找下一条黑线
  1048. {        
  1049.                 motor_motion1(1350, 1350,15     );       //左偏一定角度
  1050.                 do{
  1051.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1052.                                 motor_motion1(1400, 1400,1);   
  1053.                         } while(QTIS!=6);                  //左旋转寻找黑线   也就是检测循迹是否为1001
  1054.                 delay_nms(5);
  1055.                  while(QTIS!=6)
  1056.                         {
  1057.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1058.                                 motor_motion1(1480, 1480,1);   
  1059.                         }                   //再次检测是否已经找到下一条黑线
  1060. }
  1061. void Turn_180(void)                    //   慢速旋转  //先向左偏移一定角度，然后向左旋转寻找下一条黑线
  1062. {        
  1063.                 motor_motion1(1465, 1465,32     );       //左偏一定角度
  1064.                 do{
  1065.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1066.                                 motor_motion1(1465, 1465,1);   
  1067.                         } while(QTIS!=6);                  //左旋转寻找黑线   也就是检测循迹是否为1001                         0110
  1068.                         motor_motion2(1500, 1500);
  1069. //              motor_motion1(1475, 1475,1);
  1070. //              delay_nms(5);
  1071. //              do{
  1072. //                              QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1073. //                              motor_motion1(1465, 1465,1);   
  1074. //                      }while(QTIS!=6);                  //再次检测是否已经找到下一条黑线
  1075. }
  1076.  
  1077.  
  1078.  
  1079. void Turn_180_four(void)                    //   慢速旋转  //先向左偏移一定角度，然后向左旋转寻找下一条黑线
  1080. {        
  1081.                 motor_motion1(1465, 1465,35     );       //左偏一定角度
  1082.                 do{
  1083.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1084.                                 motor_motion1(1465, 1465,1);   
  1085.                         } while((QTIS!=6)&&(QTIS!=0x0f));//while(QTIS!=6);                  //左旋转寻找黑线   也就是检测循迹是否为1001
  1086.                 motor_motion1(1475, 1475,1);
  1087.                 delay_nms(5);
  1088.                 do{
  1089.                                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1090.                                 motor_motion1(1465, 1465,1);   
  1091.                         }while((QTIS!=6)&&(QTIS!=0x0f));                  //再次检测是否已经找到下一条黑线
  1092. }
  1093.  
  1094.  
  1095.  
  1096.  
  1097.  
  1098.  
  1099.  
  1100.  
  1101.  
  1102.  
  1103.  
  1104.  
  1105.  
  1106.  
  1107.  
  1108. void TurnRight_nDegree(char degree)     //右转   degree   个角度
  1109. {
  1110.         motor_motion1(1522, 1522,degree);//1517
  1111.         return;
  1112. }
  1113. void TurnLeft_nDegree(char degree)       //右转   degree   个角度
  1114. {
  1115.         motor_motion1(1478, 1478,degree);//1485
  1116.         return;
  1117. }
  1118. void Back_toBlack(void)                  //返回到黑线
  1119. {
  1120.         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1121.         while(QTIS != 15)
  1122.         {
  1123.                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1124.                 motor_motion1(1485,1512,1);
  1125.         }
  1126.         return;
  1127. }
  1128. void CCheck(void)         //检查C点是否有色块
  1129. {
  1130.         motor_motion1(1500,1500,1);
  1131.         delay_nms(200);
  1132.         j=0;
  1133.         process = 9;
  1134.  
  1135. }
  1136. void BCheck(void)         //检测B点是否有色块  
  1137. {
  1138. //      TurnLeft_nDegree(10);
  1139.         findLline();
  1140.         delay_nms(200);
  1141.  
  1142.         process = 11;//1;      
  1143. }
  1144.  
  1145. void ACheck(void)         //检测A点是否有色块
  1146. {
  1147.         TurnLeft_nDegree(30);   //之前是60
  1148.         findLline();            //加findLline();后TurnLeft_nDegree(40);减20
  1149.         delay_nms(200);
  1150.         process = 1;
  1151. }
  1152. void ICheck(void)         //检测I点是否有色块
  1153. {
  1154.         findLline();
  1155.         findLline();
  1156.         TurnLeft_nDegree(35+15-5);//左转n度
  1157. }
  1158. void HCheck(void)         //检测H点是否有色块
  1159. {findLline();
  1160.         findLline();
  1161.         TurnLeft_nDegree(38+10+10+9-9+5);//左转n度
  1162.                                         Hhavesome = 1;
  1163.  
  1164.        
  1165.        
  1166. }
  1167. void GCheck(void)         //检测G点是否有色块
  1168. {findRline();
  1169.         findRline();
  1170.         TurnRight_nDegree(50);
  1171.                 Ghavesome = 1; 
  1172.  
  1173. }
  1174.  
  1175. void FCarry(void)
  1176. {
  1177.        
  1178. }
  1179.  
  1180.  
  1181.  
  1182.  
  1183.  
  1184.  
  1185.  
  1186.  
  1187.  
  1188.  
  1189.  
  1190.  
  1191.  
  1192.  
  1193.  
  1194.  
  1195.  
  1196.  
  1197.  
  1198.  
  1199.  
  1200.  
  1201.  
  1202.  
  1203.  
  1204. void GCarry(void)
  1205. {
  1206.        
  1207. }
  1208. void HCarry(void)
  1209. {
  1210.        
  1211. }
  1212. void ICarry(void)
  1213. {
  1214.        
  1215. }
  1216. void FCheck(void)         //检测F点是否有色块
  1217. {
  1218. //      findRline();
  1219. //      findRline();
  1220.        
  1221.         TurnRight_nDegree(21+90-10);//右转n度
  1222.         stop();
  1223.        
  1224.         delay_nms(200);
  1225.         /*j=0;
  1226.         for(i=0,j=0;i<10;i++)
  1227.         {
  1228.                 distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1229.                 if(distance>0&&distance < 34)
  1230.                 {
  1231.                         j=j+1;
  1232.                 }
  1233.                 delay_nms(50);
  1234.         }
  1235.         if(j<1)
  1236.         {
  1237.                 TurnRight_nDegree(2);//右转n度
  1238.                 delay_nms(200);
  1239.           for(i=0,j=0;i<10;i++)
  1240.          {
  1241.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1242.                         if(distance>0&&distance < 30)
  1243.                         {
  1244.                                 j=j+1;
  1245.                         }
  1246.                         delay_nms(50);
  1247.          }
  1248.         }
  1249.                 if(j<1)
  1250.         {
  1251.                 TurnRight_nDegree(2);//右转n度
  1252.                         delay_nms(200);
  1253.           for(i=0,j=0;i<10;i++)
  1254.          {
  1255.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1256.                         if(distance>0&&distance < 34)
  1257.                         {
  1258.                                 j=j+1;
  1259.                         }
  1260.                         delay_nms(200);
  1261.          }
  1262.         }
  1263.                         if(j<1)
  1264.         {
  1265.                 TurnLeft_nDegree(8);//
  1266.                         delay_nms(200);
  1267.           for(i=0,j=0;i<10;i++)
  1268.          {
  1269.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1270.                         if(distance>0&&distance < 34)
  1271.                         {
  1272.                                 j=j+1;
  1273.                         }
  1274.                         delay_nms(200);
  1275.          }
  1276.         }
  1277.         if(j >= 1)
  1278.         {
  1279.                 delay_nms(100);
  1280.                 Fhavesome = 1; 
  1281.         }
  1282.         else
  1283.         {
  1284.                 Fhavesome = 0;
  1285.         }      
  1286.         j=0;
  1287.         a=1;    */
  1288.         Fhavesome = 1; 
  1289. //      process = 11;          
  1290. //      LED_Write_1();
  1291. }
  1292.                                                                                                                                                                                                                                                                                                         void ECheck(void)         //检测E点是否有色块
  1293. {
  1294.         TurnRight_nDegree(30);//右转n度
  1295.         delay_nms(200);
  1296.         findRline();
  1297.         delay_nms(200);
  1298.         process = 2;//11;      
  1299. }
  1300.  
  1301. void DCheck(void)         //检测D点是否有色块
  1302. {
  1303.         findRline();
  1304.         delay_nms(200);
  1305.         process =8;            
  1306. }
  1307.  
  1308.  
  1309. /*---------------------------------------------------------------------------------------------------------
  1310. *       Function name：DownFirst(void)
  1311. *  Description：把从A点搬运的色块从十字路口位置搬运回到直线上 ,并转弯回到十字路口位置
  1312. ---------------------------------------------------------------------------------------------------------*/
  1313. void DownFirst(void)                                  //从十字路口把A色块运到起始位置在返回到十字路口
  1314. {                              
  1315.                 time =0;
  1316.                
  1317.                 TIM5_Configure();
  1318.                 TIM_Cmd(TIM5, ENABLE); //开始定时器5
  1319.                 do
  1320.                 {
  1321.                         Robot_hunting2(1550);    
  1322.  
  1323.                 }while(time < 2300);                              
  1324.                 TIM_Cmd(TIM5, DISABLE);                                 //关闭定时器5
  1325.                 motor_motion1(1450,1550,16);                      //后退
  1326.                 Turn1_180();                                        //转弯180度                                  
  1327.                 do
  1328.                 {
  1329.                         Robot_hunting2(1540);                        
  1330.                 }while(QTIS !=15);                                        
  1331.                 pulses = LRp;
  1332.                 while(pulses--)
  1333.                 {
  1334.                         motor_motion2(1550, 1450);                       
  1335.                 }              
  1336.                         //  gai wei
  1337.         pulses = 2;
  1338.         while(pulses--)  
  1339.         {
  1340.                 Robot_hunting1(1580);    
  1341.         }        
  1342.        
  1343.                 pointsth++;                                            
  1344. }
  1345. /*---------------------------------------------------------------------------------------------------------
  1346. *       Function name：DownSecond(void)
  1347. *  Description：把从B点搬运的色块从十字路口位置搬运回到直线上 ,并转弯回到十字路口位置，详细过程参DownFirst(void)
  1348. ---------------------------------------------------------------------------------------------------------*/
  1349. void DownSecond(void)                                 //从十字路口把B色块运到起始位置在返回到十字路口
  1350. {      
  1351.                 time =0;
  1352.                 TIM5_Configure();
  1353.                 TIM_Cmd(TIM5, ENABLE);                            //开始定时器5
  1354.                 do
  1355.                 {
  1356.                         Robot_hunting2(1550);
  1357.                 }while(time < 1900);                          
  1358.                 TIM_Cmd(TIM5, DISABLE);
  1359.                 motor_motion1(1450,1550,16);//后退                          
  1360.                 Turn1_180();
  1361.                 do
  1362.                 {
  1363.                         Robot_hunting2(1550);                        
  1364.                 }while(QTIS != 15);                              
  1365.                 pulses = LRp;
  1366.                 while(pulses--)
  1367.                 {
  1368.                         motor_motion2(1550, 1450);                          //直行
  1369.                 }  
  1370.         //  gai wei
  1371.         pulses = 2;
  1372.         while(pulses--)  
  1373.         {
  1374.                 Robot_hunting1(1580);    
  1375.         }        
  1376.                         //前进一小段距离
  1377.                 pointsth++;                                                       //直线上放了2块色块
  1378. }
  1379. /*---------------------------------------------------------------------------------------------------------
  1380. *       Function name：Downthird(void)    
  1381. *  Description：把从c点搬运的色块从十字路口位置搬运回到直线上 ,并转弯回到十字路口位置，详细过程参DownFirst(void)
  1382.  
  1383. 第一个搬的为c
  1384. ---------------------------------------------------------------------------------------------------------*/
  1385. void Downthird(void)                                  //从十字路口把C色块运到起始位置在返回到十字路口
  1386. {
  1387.                 time =0;
  1388.                 TIM5_Configure();
  1389.                 TIM_Cmd(TIM5, ENABLE);                            //开始定时器5
  1390.                 do
  1391.                 {
  1392.                         Robot_hunting2(1550);
  1393.                 }while(time < 1500);                              
  1394.                 TIM_Cmd(TIM5, DISABLE);
  1395.                 motor_motion1(1450,1550,16);                      //后退                            
  1396.                 Turn1_180();
  1397.                 do
  1398.                 {
  1399.                         Robot_hunting2(1530);                          
  1400.                 }while(QTIS != 15);                               //直行十字路口
  1401.                 pulses =LRp;
  1402.                 while(pulses--)
  1403.                 {
  1404.                         Robot_hunting2(1545);                  
  1405.                 }                                                 //前进一小段距离，目的是为下一次转到直线上
  1406.                         //  gai wei
  1407.         pulses = 2;
  1408.         while(pulses--)  
  1409.         {
  1410.                 Robot_hunting1(1580);    
  1411.         }        
  1412.                        
  1413.                 pointsth++;                                                   //直线上放了3块色块
  1414. }
  1415. /*---------------------------------------------------------------------------------------------------------
  1416. *       Function name：DownFourth(void)
  1417. *  Description：把从D点搬运的色块从十字路口位置搬运回到直线上 ,并转弯回到十字路口位置，详细过程参DownFirst(void)
  1418. ---------------------------------------------------------------------------------------------------------*/
  1419. void DownFourth(void)                                 //从十字路口把D色块运到起始位置在返回到十字路口
  1420. {
  1421.                 time =0;
  1422.                 TIM5_Configure();
  1423.                 TIM_Cmd(TIM5, ENABLE);                            //开始定时器5
  1424.                 do
  1425.                 {
  1426.                         Robot_hunting2(1550);
  1427.                 }while(time < 1000);
  1428.                 /*TIM_Cmd(TIM5, DISABLE);*/
  1429.                 motor_motion1(1450,1550,16);                      //后退
  1430.                 Turn_180();                               //后转
  1431. //              Turn_180_four();
  1432.                
  1433.                 do
  1434.                 {
  1435.                         Robot_hunting1(1550);                           //直行        
  1436.                 }while(QTIS != 15);                               //直行十字路口
  1437.                 pulses = LRp;
  1438.                
  1439.                 while(pulses--)
  1440.                 {
  1441.                                 motor_motion2(1550, 1450-50);                    //直行
  1442.                 }
  1443.                 pulses = 2;
  1444.                 while(pulses--)  
  1445.                 {
  1446.                         Robot_hunting1(1580);    
  1447.                 }                      
  1448.                 pointsth++;                                                       //直线上放了4块色块
  1449. }
  1450. /*---------------------------------------------------------------------------------------------------------
  1451. *       Function name：DownFiveth(void)
  1452. *  Description：把从E点搬运的色块从十字路口位置搬运回到直线上,详细过程参DownFirst(void)
  1453. ---------------------------------------------------------------------------------------------------------*/
  1454. void DownFiveth(void)                                 //从十字路口把E色块运到起始位置在返回到十字路口
  1455. {
  1456.                 time =0;
  1457.                 TIM5_Configure();
  1458.                 TIM_Cmd(TIM5, ENABLE);                            //开始定时器5
  1459.                 do
  1460.                 {
  1461.                         Robot_hunting2(1550);
  1462.                 }while(time < 800);
  1463.                 TIM_Cmd(TIM5, DISABLE);
  1464. //              motor_motion1(1450,1550,16);                      //后退
  1465.                 Turn_180();                               //后转
  1466. //              Turn_180_four();
  1467.                
  1468. //              do
  1469. //              {
  1470. //                      Robot_hunting1(1550);                           //直行        
  1471. //              }while(QTIS != 15);                               //直行十字路口
  1472. //             
  1473. //              pulses = LRp;
  1474. //             
  1475. //              while(pulses--)
  1476. //              {
  1477. //                              motor_motion2(1550, 1450);                    //直行
  1478. //              }
  1479.                 motor_motion2(1500,1500);
  1480.                 delay_ms(50);
  1481. //              pulses = 2;
  1482. //              while(pulses--)  
  1483. //              {
  1484. //                      Robot_hunting1(1580);    
  1485. //              }                      
  1486.                 pointsth++;                                                                                       //直线上放了5块色块
  1487. }
  1488. void GotoLine(void)
  1489. {
  1490.        
  1491.         int tempa=0;
  1492.         switch(pointsth)                                          // pointsth初始值为0
  1493.         {
  1494.                 case 0:DownFirst();                                   //        搬运A点时选择
  1495.                                 break;
  1496.                 case 1:DownSecond();                              //搬运B点时选着
  1497.                                 break;
  1498.                 case 2:Downthird();                                     //搬运C点时选着
  1499.                                 break;
  1500.                 case 3:DownFourth();                              //搬运E点时选着
  1501.                                 break;
  1502.                 case 4:DownFiveth();                                //搬运F点时选着
  1503.                                 break;
  1504.                 default:
  1505.                                 break;
  1506.         }
  1507.         tempa++;
  1508.         tempa=0x22;
  1509. }
  1510. /*---------------------------------------------------------------------------------------------------------
  1511. *       Function name：CarryGpoint(void)
  1512. *  Description：把从G点搬运的色块从十字路口位置搬运回到直线上,详细过程参 CarryIpoint(void)
  1513. ****************从十字路口位置搬运回到直线上***************
  1514. ---------------------------------------------------------------------------------------------------------*/
  1515. void Gback(){
  1516.                 pulses=8;
  1517.                 while(pulses--)
  1518.                 {
  1519.                         motor_motion2(1550, 1450);        //直行
  1520.                 }
  1521.                 //TurnRight_nDegree(80);     //后转120
  1522.                 findLline();
  1523.                 /*do
  1524.                 {
  1525.                         motor_motion2(1550, 1450);        //直行
  1526.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1527.                 }while(QTIS != 15);//   循迹*/
  1528.                 pulses=15;
  1529.                 while(pulses--)
  1530.                 {
  1531.                         Robot_hunting1(1550);     //直行
  1532.                 }
  1533.           TurnLeft_nDegree(80);//左转n度
  1534.                 do
  1535.                 {
  1536.                         motor_motion2(1550, 1450);        //直行
  1537.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1538.                 }while(QTIS != 15);
  1539.                 pulses=8;
  1540.                 while(pulses--)
  1541.                 {
  1542.                         motor_motion2(1550, 1450);        //直行
  1543.                 }
  1544.                 findRline();
  1545.                                
  1546.                 do
  1547.                 {
  1548.                         Robot_hunting1(1550);
  1549.                 }while(QTIS != 15);//判断是否到达第一个路口
  1550.                
  1551.                 pulses = LRp-2;
  1552.                 while(pulses--)
  1553.                 {
  1554.                         motor_motion2(1550, 1450);        //直行
  1555.                 }
  1556.                         //  gai wei
  1557.                 pulses = 2;
  1558.                 while(pulses--)  
  1559.                 {
  1560.                         Robot_hunting1(1580);    
  1561.                 }        
  1562. }
  1563.  
  1564. void Hback(){
  1565.                 motor_motion1(1550, 1450,10);
  1566.                 findRline();
  1567.                 pulses=15;
  1568.                 while(pulses--)
  1569.                 {
  1570.                         Robot_hunting1(1550);     //直行
  1571.                 }
  1572.           TurnRight_nDegree(100);//右转
  1573.                 do
  1574.                 {
  1575.                         motor_motion2(1550, 1450);        //直行
  1576.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1577.                 }while(QTIS != 15);
  1578.                 pulses=12;
  1579.                 while(pulses--)
  1580.                 {
  1581.                         motor_motion2(1550, 1450);        //直行
  1582.                 }
  1583.                 findLline();
  1584.                 do
  1585.                 {
  1586.                         Robot_hunting1(1550);
  1587.                 }while(QTIS != 15);//判断是否到达第一个路口
  1588.                
  1589.                 pulses = LRp-2;
  1590.                 while(pulses--)
  1591.                 {
  1592.                         motor_motion2(1550, 1450);        //直行
  1593.                 }
  1594.                 pulses = 2;
  1595.                 while(pulses--)  
  1596.                 {
  1597.                         Robot_hunting1(1580);    
  1598.                 }        
  1599. }
  1600.        
  1601. void CarryGpoint(void)                              //从十字路口把G色块运到起始位置在返回到十字路口
  1602. {
  1603.                 Ghavesome=0;
  1604.                 do
  1605.                 {
  1606.                         motor_motion2(1540, 1460);
  1607.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1608.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1609.                                 if(distance > 4 && QTIS == 15){
  1610.                                         have = false;
  1611.                                         Gback();
  1612.                                         process=3;     //*****待改******待改***********待改******待改***********待改*********************待改************************8888
  1613.                                         return;
  1614.                                 }
  1615.                 }while(distance > 4);  //机器朝着G点位置物块方向直走
  1616.                 pulses=15;
  1617.                 do
  1618.                 {
  1619.                         motor_motion2(1550, 1450);        //直行
  1620.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1621.                 }while(QTIS != 15);
  1622.                 pulses=50;
  1623.                 while(pulses--)
  1624.                 {
  1625.                         motor_motion2(1550, 1450);        //直行
  1626.                 }
  1627.                
  1628.                         motor_motion1(1450,1550,30);//后退
  1629.                   TurnRight_nDegree(65-3);
  1630.                   findRline();
  1631.                
  1632.                 do
  1633.                 {
  1634.                         Robot_hunting1(1550);
  1635.                 }while(QTIS !=15);
  1636.                         pulses=30-10;
  1637.                 while(pulses--)
  1638.                 {
  1639.                         motor_motion2(1550, 1450);        //直行
  1640.                 }
  1641.                 TurnRight_nDegree(90+45-20);
  1642. //              findRline1();
  1643. //              findRline1();
  1644.                 do
  1645.                 {
  1646.                         Robot_hunting1(1550);                           //直行        
  1647.                 }while(QTIS != 15);  
  1648.                
  1649.                
  1650.                         pulses=15;
  1651.                 while(pulses--)
  1652.                 {
  1653.                         motor_motion2(1550, 1450);        //直行
  1654.                 }               //直行十字路口
  1655. /*---------------------------------------------------------------------------------------------------------
  1656. *       Function name：CarryHpoint(void)
  1657. *  Description：把从H点搬运的色块从十字路口位置搬运回到直线上,详细过程参 CarryIpoint(void)
  1658. ---------------------------------------------------------------------------------------------------------*/
  1659.  
  1660. void CarryHpoint(void)                
  1661. {
  1662.                 Hhavesome=0;
  1663.                 do
  1664.                 {
  1665.                         motor_motion2(1540, 1460);
  1666.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1667.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1668.                                 if(distance > 4 && QTIS == 15){
  1669.                                         have = false;
  1670.                                         Hback();             //
  1671.                                         process=3;     //*****待改******待改***********待改******待改***********待改*********************待改************************8888
  1672.                                         return;
  1673.                                 }
  1674.                 }while(distance > 4);  //
  1675.                 pulses=15;
  1676.        
  1677.                 do
  1678.                 {
  1679.                         motor_motion2(1550, 1450);        //直行
  1680.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1681.                 }while(QTIS != 15);
  1682.                 pulses=50;
  1683.                 while(pulses--)
  1684.                 {
  1685.                         motor_motion2(1550, 1450);        //直行
  1686.                 }
  1687.                
  1688.                         motor_motion1(1450,1550,25);//后退
  1689.                   TurnLeft_nDegree(65-9);
  1690.                   findLline2();
  1691.                
  1692.                 do
  1693.                 {
  1694.                         Robot_hunting1(1550);
  1695.                 }while(QTIS !=15);
  1696.                
  1697.                
  1698.         stop();
  1699.        
  1700.        
  1701.                         pulses=30-10;
  1702.                 while(pulses--)
  1703.                 {
  1704.                         motor_motion2(1550, 1450);        //直行
  1705.                 }
  1706.                
  1707.                
  1708.         stop();
  1709.        
  1710.        
  1711.                 TurnLeft_nDegree(90+45-10-5-20-10);
  1712. //              findRline1();
  1713. //              findRline1();
  1714.                
  1715.                
  1716.         stop();
  1717.        
  1718.        
  1719.                 do
  1720.                 {
  1721.                         Robot_hunting1(1550);                           //直行        
  1722.                 }while(QTIS != 15);  
  1723.                
  1724.                
  1725.                         pulses=15;
  1726.                 while(pulses--)
  1727.                 {
  1728.                         motor_motion2(1550, 1450);        //直行
  1729.                 }               //直行十字路口
  1730. void CarryCpoint(void)                                
  1731. {
  1732.                 Chavesome=0;
  1733.                 do
  1734.                 {
  1735.                         Robot_hunting1(1550);
  1736.                 }while(QTIS);
  1737.                 pulses = 30;
  1738.                 while(pulses--)
  1739.                 {
  1740.                         Robot_hunting1(1550);     //直行
  1741.                 }
  1742.                 Turn_180();
  1743.                 do
  1744.                 {
  1745.                         Robot_hunting1(1550);
  1746.                 }while(QTIS !=15);//第一个路口
  1747.                 pulses = 15;
  1748.                 while(pulses--)
  1749.                 {
  1750.                         motor_motion2(1550, 1450);        //直行
  1751.                 }
  1752.                 GotoLine();
  1753.                 findsekuai++;
  1754.   if(findsekuai==5)
  1755.         {
  1756.                 process=11;
  1757.                 Idone=1;
  1758.         }
  1759. }
  1760. /*---------------------------------------------------------------------------------------------------------
  1761. *       Function name：CarryDpoint(void)
  1762. *  Description：把D点位置上色块搬运回到直线上 ,详细过程参看搬运A点CarryApoint(void)
  1763. ---------------------------------------------------------------------------------------------------------*/
  1764. void CarryDpoint(void)                              //从十字路口把A色块运到起始位置在返回到十字路口
  1765. {
  1766.                 Dhavesome=0;
  1767.                 do
  1768.                 {
  1769.                         Robot_hunting1(1550);
  1770.                 }while(QTIS);
  1771.                 pulses = 35;
  1772.                 while(pulses--)
  1773.                 {
  1774.                         Robot_hunting1(1550);     //直行
  1775.                 }
  1776.                 Turn_180();
  1777.                 delay_nms(200);
  1778.                 do
  1779.                 {
  1780.                         Robot_hunting1(1540);
  1781.                 }while(QTIS !=15);//第一个路口
  1782.                 pulses = 18;
  1783.                 while(pulses--)
  1784.                 {
  1785.                         motor_motion2(1550, 1450);        //直行
  1786.                 }
  1787.                 findLline();
  1788.                 GotoLine();
  1789.                 findsekuai++;
  1790.   if(findsekuai==5)
  1791.         {
  1792.                 process=11;
  1793.                 Idone=1;
  1794.         }
  1795. }
  1796. /*---------------------------------------------------------------------------------------------------------
  1797. *       Function name：CarryEpoint(void)
  1798. *  Description：把E点位置上色块搬运回到直线上 ,详细过程参看搬运A点CarryApoint(void)
  1799. ---------------------------------------------------------------------------------------------------------*/
  1800. void CarryEpoint(void)                                     //从十字路口把A色块运到起始位置在返回到十字路口
  1801. {
  1802.         Ehavesome=0;
  1803.                 do
  1804.                 {
  1805.                         Robot_hunting1(1550);
  1806.                        
  1807.                 }while(QTIS);
  1808.                 pulses = 40;
  1809.                 while(pulses--)
  1810.                 {
  1811.                         Robot_hunting1(1550);     //直行
  1812.                 }
  1813.                 Turn_180();
  1814.                 do
  1815.                 {
  1816.                         Robot_hunting1(1550);
  1817.                 }while(QTIS !=15);//第一个路口
  1818.                 pulses = 14;
  1819.                 while(pulses--)
  1820.                 {
  1821.                         motor_motion2(1550, 1450);        //直行
  1822.                 }
  1823.                 findLline();
  1824.                 GotoLine();
  1825.           findsekuai++;
  1826.           if(findsekuai==5)
  1827.                 {
  1828.                         process=11;
  1829.                         Idone=1;
  1830.                 }
  1831.         /*
  1832.                 Ehavesome=0;
  1833.                 do
  1834.                 {
  1835.                         Robot_hunting1(1550);
  1836.                        
  1837.                 }while(QTIS);
  1838.                 pulses = 40;
  1839.                 while(pulses--)
  1840.                 {
  1841.                         Robot_hunting1(1550);     //直行
  1842.                 }
  1843.                 Turn_180();
  1844.                 do
  1845.                 {
  1846.                         Robot_hunting1(1550);
  1847.                 }while(QTIS !=15);//第一个路口
  1848.                 pulses = 14;
  1849.                 while(pulses--)
  1850.                 {
  1851.                         motor_motion2(1550, 1450);        //直行
  1852.                 }
  1853.                 findLline();
  1854.                 GotoLine();
  1855.                 pulses = 18;
  1856.                 while(pulses--)
  1857.                 {
  1858.                         motor_motion2(1550,1450);
  1859.                 }
  1860.           findsekuai++;
  1861.           if(findsekuai==5)
  1862.                 {
  1863.                         process=11;
  1864.                         Idone=1;
  1865.                 }
  1866.                 */
  1867. }
  1868.  
  1869. /*---------------------------------------------------------------------------------------------------------
  1870. *       Function name：CarryBpoint(void)
  1871. *  Description：把B点位置上色块搬运回到直线上 ,详细过程参看搬运A点CarryApoint(void)
  1872. ---------------------------------------------------------------------------------------------------------*/
  1873. void CarryBpoint(void)                        //从十字路口把A色块运到起始位置在返回到十字路口
  1874. {
  1875.                 do
  1876.                 {
  1877.                         Robot_hunting1(1550);
  1878.                 }while(QTIS);
  1879.                 pulses = 35;
  1880.                 while(pulses--)
  1881.                 {
  1882.                         Robot_hunting1(1550);     //直行
  1883.                 }              
  1884.                 Turn_180();
  1885. //              stop();
  1886.                 do
  1887.                 {
  1888.                         Robot_hunting1(1550);
  1889.                 }while(QTIS !=15);//第一个路口
  1890.                 pulses = 20;
  1891.                 while(pulses--)
  1892.                 {
  1893.                         motor_motion2(1550, 1450);        //直行
  1894.                 }
  1895.                 stop();
  1896.                 TurnLeftAnyPulse(10);
  1897.                 findRline();
  1898.                
  1899.                 GotoLine();
  1900.                 motor_motion2(1500, 1500);
  1901.                 delay_ms(50);
  1902.                 findsekuai++;
  1903.   if(findsekuai==5)
  1904.         {
  1905.                 process=11;
  1906.                 Idone=1;
  1907.         }
  1908. }
  1909. /*---------------------------------------------------------------------------------------------------------
  1910. *       Function name：CarryApoint(void)
  1911. *  Description：把A点位置上色块搬运回到直线上
  1912. ---------------------------------------------------------------------------------------------------------*/
  1913. void CarryApoint(void)                  
  1914. {
  1915.                 Ahavesome=0;
  1916.                 do
  1917.                 {
  1918.                         Robot_hunting1(1550);
  1919.                 }while(QTIS);
  1920.     pulses = 35;
  1921.                 while(pulses--)
  1922.                 {
  1923.                         Robot_hunting1(1550);     //直行
  1924.                 }      
  1925.                 Turn_180();        //转弯180度
  1926.                 do
  1927.                 {
  1928.                         Robot_hunting1(1550);
  1929.                 }while(QTIS !=15);
  1930.                 pulses = 15;
  1931.                 while(pulses--) //
  1932.                 {
  1933.                         motor_motion2(1550, 1450);        //直行
  1934.                 }
  1935.                 findRline();  //向右转弯到最近黑线上
  1936.                 GotoLine();
  1937.                 findsekuai++;
  1938.   if(findsekuai==5)
  1939.         {
  1940.                 process=11;
  1941.                 Idone=1;
  1942.         }
  1943. }
  1944. /*---------------------------------------------------------------------------------------------------------
  1945. *       Function name： RedCarry(void)
  1946. *  Description：把红色色块搬运到对应的红色区域上
  1947. ---------------------------------------------------------------------------------------------------------*/
  1948.  
  1949. void goBackCenter(){
  1950.                 pulses = 25;
  1951.                 while(pulses--)
  1952.                 {
  1953.                         Robot_hunting1(1540);
  1954.                 }//判断是否到达第一个路口
  1955.                 Turn_180();//转弯180度      
  1956.                 do
  1957.                 {
  1958.                         Robot_hunting1(1550);
  1959.                 }while(QTIS != 15);//判断是否到达第一个路口
  1960.                
  1961.                 pulses = LRp;
  1962.                 while(pulses--)
  1963.                 {
  1964.                         motor_motion2(1550, 1450);
  1965.                 }                       //直行
  1966. }
  1967.  
  1968. void gotoG(){
  1969.         void moveBack();
  1970.        
  1971. if(!have){
  1972.                 moveBack();
  1973.                 process = 13;
  1974.                 return ;       
  1975.         }
  1976.                 pulses = 22;
  1977.                 while(pulses--)
  1978.                 {
  1979.                         Robot_hunting1(1550);
  1980.                 }//判断是否到达第一个路口
  1981.                 TurnLeft_nDegree(35);//左转n度
  1982.                 do
  1983.                 {
  1984.                         motor_motion2(1550, 1450);        //直行
  1985.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  1986.                 }while(QTIS != 15);
  1987.                
  1988.                 pulses=10;
  1989.                 while(pulses--)
  1990.                 {
  1991.                         motor_motion2(1550, 1450);        //直行
  1992.                 }
  1993.                 findLline();  //向右转弯到最近黑线上
  1994.                 do{
  1995.                         Robot_hunting(1540);   
  1996.                         //delay_nms(50);
  1997.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  1998.                 }while(distance>4 );
  1999.                 pulses = 10;                      //前冲步数   可修改  优化
  2000.                 while(pulses--)
  2001.                 {
  2002.                         motor_motion2(1550, 1450);        //直行
  2003.                 }
  2004.                 TurnLeft_nDegree(110);//左转n度
  2005.                         pulses=5;
  2006.                 while(pulses--)
  2007.                 {
  2008.                         motor_motion2(1550, 1450);        //直行
  2009.                 }
  2010.                 do
  2011.                 {
  2012.                         motor_motion2(1550, 1450);        //直行
  2013.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2014.                 }while(!QTIS);
  2015.                
  2016.                 pulses=14;
  2017.                 while(pulses--)
  2018.                 {
  2019.                         motor_motion2(1540, 1460);        //直行
  2020.                 }
  2021.                 findRline();  //向右转弯到最近黑线上
  2022.                 process = 11;
  2023.  
  2024. }
  2025.  
  2026.  
  2027.  
  2028. void gotoH(){
  2029.         void moveBack();
  2030.        
  2031. if(!have){
  2032.                 moveBack();
  2033.                 process = 13;
  2034.                 return ;       
  2035.         }
  2036.                 pulses = 22;
  2037.                 while(pulses--)
  2038.                 {
  2039.                         Robot_hunting1(1550);
  2040.                 }//判断是否到达第一个路口
  2041.                 TurnRight_nDegree(27);//左转n度
  2042.                 do
  2043.                 {
  2044.                         motor_motion2(1550, 1450);        //直行
  2045.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2046.                 }while(QTIS != 15);
  2047.                
  2048.                 pulses=10;
  2049.                 while(pulses--)
  2050.                 {
  2051.                         motor_motion2(1550, 1450);        //直行
  2052.                 }
  2053.        
  2054.                 findRline();  //向右转弯到最近黑线上
  2055.                 do{
  2056.                         Robot_hunting(1540);   
  2057.                         //delay_nms(50);
  2058.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2059.                 }while(distance>4 );
  2060.                 pulses = 10;                      //前冲步数   可修改  优化
  2061.                 while(pulses--)
  2062.                 {
  2063.                         motor_motion2(1550, 1450);        //直行
  2064.                 }
  2065.                 TurnRight_nDegree(79);//左转n度
  2066.                 pulses=5;
  2067.                 while(pulses--)
  2068.                 {
  2069.                         motor_motion2(1550, 1450);        //直行
  2070.                 }
  2071.                 do
  2072.                 {
  2073.                         motor_motion2(1550, 1450);        //直行
  2074.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2075.                 }while(!QTIS);
  2076.                
  2077.                 pulses=13;
  2078.                 while(pulses--)
  2079.                 {
  2080.                         motor_motion2(1550, 1450);        //直行
  2081.                 }
  2082.                 findLline();  //向右转弯到最近黑线上
  2083.                 process = 11;
  2084.  
  2085. }
  2086. /////////////////////////////////////////////////
  2087.  
  2088. void RedCarry(void)//搬红色色块到分数区    
  2089. {
  2090.         hadedone++;
  2091.         printf("hadedone:%d",hadedone);
  2092.                 do
  2093.                 {
  2094.                         Robot_hunting1(1550);
  2095.                 }while(QTIS);//当到达中间白点位置时就停止
  2096.                 stop();
  2097.                 pulses = 35;
  2098.                 while(pulses--)
  2099.                 {
  2100.                         Robot_hunting1(1530);     //直行
  2101.                 }
  2102.                
  2103.                 stop();
  2104.                
  2105.                 /*pulses = goal-1;
  2106.                 while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
  2107.                 {
  2108.                         Robot_hunting1(1550);     //直行
  2109.                 }       */
  2110.                
  2111.                 while( !(GPIO_ReadInputData(GPIOE)&0x08000))
  2112.                 {
  2113.                         delay_nms(50);
  2114.                         Robot_hunting1(1502);     //直行
  2115.                 }
  2116.                
  2117.                 stop();
  2118.                
  2119.                
  2120.                 motor_motion1(1450,1550,25);//后退
  2121.                
  2122.                 stop();
  2123.                
  2124.           Turn_180();//转弯180度
  2125.                
  2126.                 stop();
  2127.                 do
  2128.                 {
  2129.                         Robot_hunting1(1550);
  2130.                 }while(QTIS !=15);//回到第一个路口
  2131.                 pulses = 10;                      //前冲步数   可修改  优化
  2132.                 while(pulses--)
  2133.                 {
  2134.                         motor_motion2(1550, 1450);        //直行
  2135.                 }
  2136.                
  2137.                 if(hadedone >= 6){
  2138.                                 process=12;
  2139.                         printf("process:%d",process);
  2140.                         return;
  2141.                 }
  2142.                 if(hadedone >= 4)
  2143.                                 {
  2144.                                         TurnLeft_nDegree(180);
  2145. //                                      do
  2146. //                                      {
  2147. //                                              Robot_hunting1(1550);
  2148. //                                      }while(QTIS !=15);//第一个路口
  2149. //                                      pulses = 15;           //这个值是需要修改的
  2150. //                                      while(pulses--)
  2151. //                                      {
  2152. //                                              Robot_hunting2(1530);
  2153. //                                      }
  2154.                                        
  2155.                                        
  2156.                                         }
  2157.                        
  2158.                
  2159.                
  2160.                 else
  2161.                 {
  2162.                
  2163.                 do{
  2164.                         Robot_hunting(1540);   
  2165.                         //delay_nms(50);
  2166.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2167.                 }while(distance>5 );
  2168.                 pulses =10;                      //前冲步数   可修改  优化
  2169.                 while(pulses--)
  2170.                 {
  2171.                         motor_motion2(1550, 1450);        //直行
  2172.                 }
  2173.                 delay_nms(50);
  2174.                 Turn_180();
  2175.                 process=11;
  2176.         }
  2177.                
  2178. }
  2179. /*---------------------------------------------------------------------------------------------------------
  2180. *       Function name： WhiteCarry(void)
  2181. *  Description：把白色色块搬运到对应的区域上  ，详细讲解请参考函数RedCarry(void)
  2182. ---------------------------------------------------------------------------------------------------------*/
  2183. void WhiteCarry(void)//搬白色色块到分数区
  2184. {              
  2185.         hadedone++;
  2186.         printf("hadedone:%d",hadedone);
  2187.           TurnLeft_nDegree(10);
  2188.                 findLline();                              //左偏寻找下一条黑线  也就是白点对应的线   
  2189.                 delay_nms(200);
  2190.           do
  2191.                 {
  2192.                         Robot_hunting1(1550);
  2193.                 }while(QTIS);                             //当第一次检测到白点跳出循环0
  2194.                 pulses = 35;
  2195.                 while(pulses--)
  2196.                 {
  2197.                         Robot_hunting1(1530);     //直行
  2198.                 }
  2199.                
  2200.                 /*pulses = goal-1;
  2201.                 while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
  2202.                 {
  2203.                         Robot_hunting1(1550);     //直行
  2204.                 }       */
  2205.                
  2206.                 while( !(GPIO_ReadInputData(GPIOE)&0x08000))
  2207.                 {
  2208.                         delay_nms(50);
  2209.                         Robot_hunting1(1502);     //直行
  2210.                 }      
  2211.                
  2212.                 motor_motion1(1450,1550,25);//后退
  2213.                 Turn_180();                                //旋转180度   可优化修改
  2214.                 do
  2215.                 {
  2216.                         Robot_hunting1(1550);
  2217.                 }while(QTIS !=15);                       //第一个路口
  2218.                 pulses = 20;                      //前冲步数   可修改  优化
  2219.                 while(pulses--)
  2220.                 {
  2221.                         motor_motion2(1550, 1450);        //直行
  2222.                 }
  2223. //              findRline();
  2224.                
  2225.                
  2226.                
  2227.                 delay_nms(200);
  2228.                         if(hadedone >= 6){
  2229.                                 process=12;
  2230.                                 printf("process:%d",process);
  2231.                         return;
  2232.                 }
  2233. //                      if(hadedone >= 5){
  2234. //                      //process=aim;
  2235. //                      //goBackCenter();
  2236. //                      //gotoG();
  2237. //                      gotoH();
  2238. //                              return;
  2239. //              }
  2240.                
  2241.                
  2242.                 if(hadedone >= 4)
  2243.                                 {
  2244.                                         TurnLeft_nDegree(135);
  2245. //                                      do
  2246. //                                      {
  2247. //                                              Robot_hunting1(1550);
  2248. //                                      }while(QTIS !=15);//第一个路口
  2249. //                                      pulses = 15;           //这个值是需要修改的
  2250. //                                      while(pulses--)
  2251. //                                      {
  2252. //                                              Robot_hunting2(1530);
  2253. //                                      }
  2254.                                        
  2255.                                        
  2256.                                         }
  2257.                        
  2258.                
  2259.                
  2260.                 else
  2261.                 {
  2262.                        
  2263.                         findRline();
  2264.                
  2265.                 do{
  2266.                         Robot_hunting(1535);   
  2267.                         //delay_nms(50);
  2268.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2269.                 }while(distance>5);
  2270.                 pulses =10;                      //前冲步数   可修改  优化
  2271.                 while(pulses--)
  2272.                 {
  2273.                         motor_motion2(1550, 1450);        //直行
  2274.                 }
  2275.                 delay_nms(50);
  2276.                 Turn_180();
  2277.                 process=11;
  2278.         }
  2279.                
  2280. }
  2281. /*---------------------------------------------------------------------------------------------------------
  2282. *       Function name：YellowCarry(void)
  2283. *  Description：把黄色色块搬运到对应的区域上  ，详细讲解请参考函数RedCarry(void)
  2284. ---------------------------------------------------------------------------------------------------------*/
  2285. void YellowCarry(void)//搬黄色色块到分数区
  2286. {
  2287.         hadedone++;
  2288.         printf("hadedone:%d",hadedone);
  2289.  
  2290. //              if(QTTS == )
  2291.                 TurnLeft_nDegree(8);//左转n度
  2292.                
  2293.                 findLline();                        //可能需要优化  暂定  
  2294.                 delay_nms(200);
  2295.                 findLline();            //寻找右边下一条最近的直线
  2296.        
  2297.                 motor_motion2(1500,1500);
  2298.                 delay_ms(50);
  2299. //              while(1);
  2300.                 do
  2301.                 {
  2302.                         Robot_hunting1(1550);
  2303.                 }while(QTIS);
  2304.                         pulses = 35;
  2305.                 while(pulses--)
  2306.                 {
  2307.                         Robot_hunting1(1530);     //直行
  2308.                 }
  2309.                
  2310.                 /*pulses = goal-1;
  2311.                 while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
  2312.                 {
  2313.                         Robot_hunting1(1550);     //直行
  2314.                 }       */
  2315.                
  2316.                 while( !(GPIO_ReadInputData(GPIOE)&0x08000))
  2317.                 {
  2318.                         delay_nms(50);
  2319.                         Robot_hunting1(1502);     //直行
  2320.                 }      
  2321.                 motor_motion1(1450,1550,25);//后退
  2322.                 Turn_180();
  2323.                 do
  2324.                 {
  2325.                         Robot_hunting1(1550);
  2326.                 }while(QTIS !=15);//第一个路口
  2327.                 pulses = 15;           //这个值是需要修改的
  2328.                 while(pulses--)
  2329.                 {
  2330.                         Robot_hunting2(1530);
  2331.                 }
  2332. //              findRline();
  2333.                 TurnRight_nDegree(90);
  2334.                
  2335.                
  2336.                
  2337.                
  2338.                
  2339.                
  2340.                
  2341.                
  2342.                 delay_nms(200);
  2343.                         if(hadedone >= 6){
  2344.                                 process=12;
  2345.                                 printf("process:%d",process);
  2346.                         return;
  2347.                 }
  2348.                        
  2349.                
  2350.                         if(hadedone >= 4)
  2351.                                 {
  2352.                                         Turn_180();
  2353. //                                      do
  2354. //                                      {
  2355. //                                              Robot_hunting1(1550);
  2356. //                                      }while(QTIS !=15);//第一个路口
  2357. //                                      pulses = 15;           //这个值是需要修改的
  2358. //                                      while(pulses--)
  2359. //                                      {
  2360. //                                              Robot_hunting2(1530);
  2361. //                                      }
  2362.                                        
  2363.                                        
  2364.                                         }
  2365.                        
  2366.                
  2367.                
  2368.                 else
  2369.                 {
  2370.                         do{
  2371.                         Robot_hunting(1540);   
  2372.                         //delay_nms(50);
  2373.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2374.                 }while(distance>5);
  2375.                 pulses = 10;                      //前冲步数   可修改  优化
  2376.                 while(pulses--)
  2377.                 {
  2378.                         motor_motion2(1550, 1450);        //直行
  2379.                 }
  2380.                 delay_nms(50);
  2381.                 Turn_180();
  2382.                 process=11;
  2383.                 }
  2384.                
  2385. }
  2386. /*---------------------------------------------------------------------------------------------------------
  2387. *       Function name：BlackCarry(void)
  2388. *  Description：把黑色色块搬运到对应的区域上  ，详细讲解请参考函数RedCarry(void)
  2389. ---------------------------------------------------------------------------------------------------------*/
  2390. void BlackCarry(void)//搬黑色色块到分数区
  2391. {              
  2392.         hadedone++;
  2393.         printf("hadedone:%d",hadedone);
  2394.     findRline();
  2395.                 delay_nms(200);                
  2396.                 do
  2397.                 {
  2398.                         Robot_hunting1(1550);
  2399.                 }while(QTIS);
  2400.                 pulses = 40;
  2401.                 while(pulses--)
  2402.                 {
  2403.                         Robot_hunting1(1520);     //直行
  2404.                 }
  2405.                
  2406.                 /*pulses = goal-1;
  2407.                 while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
  2408.                 {
  2409.                         Robot_hunting1(1550);     //直行
  2410.                 }       */
  2411.                
  2412.                 while( !(GPIO_ReadInputData(GPIOE)&0x08000))
  2413.                 {
  2414.                         delay_nms(50);
  2415.                         Robot_hunting1(1502);     //直行
  2416.                 }      
  2417.        
  2418.                 motor_motion1(1450,1550,25);//后退
  2419.                 Turn_180();
  2420.                 do
  2421.                 {
  2422.                         Robot_hunting1(1550);
  2423.                 }while(QTIS != 15);//第一个路口
  2424.                 pulses = 18;
  2425.                 while(pulses--)
  2426.                 {
  2427.                         motor_motion2(1550, 1450);        //直行
  2428.                 }
  2429. //              findLline();
  2430.                 if(hadedone >= 6){
  2431.                                 process=12;
  2432.                                 printf("process:%d",process);
  2433.                         return;
  2434.                 }
  2435. //                      if(hadedone >= 5){
  2436. //                      //process=aim;
  2437. //                      //goBackCenter();
  2438. //                //gotoG();
  2439. //                      gotoH();
  2440. //                              return;
  2441. //              }
  2442.                        
  2443.                 if(hadedone >= 4)
  2444.                                 {
  2445.                                         TurnRight_nDegree(135);
  2446. //                                      do
  2447. //                                      {
  2448. //                                              Robot_hunting1(1550);
  2449. //                                      }while(QTIS !=15);//第一个路口
  2450. //                                      pulses = 15;           //这个值是需要修改的
  2451. //                                      while(pulses--)
  2452. //                                      {
  2453. //                                              Robot_hunting2(1530);
  2454. //                                      }
  2455.                                        
  2456.                                        
  2457.                                         }
  2458.                        
  2459.                
  2460.                
  2461.                 else
  2462.                 {
  2463.                        
  2464.                         findLline();
  2465.                
  2466.                
  2467.                
  2468.                
  2469.                
  2470.                 do{
  2471.                         Robot_hunting(1540);   
  2472.                         //delay_nms(50);
  2473.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2474.                 }while(distance>5);
  2475.                 pulses = 10;                      //前冲步数   可修改  优化
  2476.                 while(pulses--)
  2477.                 {
  2478.                         motor_motion2(1550, 1450);        //直行
  2479.                 }
  2480.                 delay_nms(50);
  2481.                 Turn_180();
  2482.                 process=11;
  2483.         }
  2484. }
  2485. /*---------------------------------------------------------------------------------------------------------
  2486. *       Function name：BlueCarry(void)
  2487. *  Description：把蓝色色块搬运到对应的区域上  ，详细讲解请参考函数RedCarry(void)
  2488. ---------------------------------------------------------------------------------------------------------*/
  2489. void BlueCarry(void)//搬蓝色色块到分数区
  2490. {
  2491.         hadedone++;
  2492.         printf("hadedone:%d",hadedone);
  2493. //        findRline();
  2494. //              delay_nms(200);
  2495. //              findRline();
  2496. //              delay_nms(200);
  2497.        
  2498.         TurnRight_nDegree(90);//右转n度
  2499.         stop();
  2500.                 do
  2501.                 {
  2502.                         Robot_hunting1(1550);
  2503.                 }while(QTIS);
  2504.         while( !(GPIO_ReadInputData(GPIOE)&0x08000))
  2505.                 {
  2506.                         delay_nms(50);
  2507.                         Robot_hunting1(1502);     //直行
  2508.                 }      
  2509.                 delay_nms(200);
  2510.                
  2511.                 pulses = 5;
  2512.                 while(pulses--)
  2513.                 {
  2514.                         delay_nms(20);
  2515.                         Robot_hunting1(1502);     //直行
  2516.                 }
  2517. //              while( (GPIO_ReadInputData(GPIOE)&0x08000))
  2518. //              {
  2519. //                      delay_nms(50);
  2520. //                      Robot_hunting1(1501);     //直行
  2521. //              }      
  2522.                 delay_nms(200);
  2523.                 motor_motion1(1450,1550,18);//后退
  2524.                 Turn_180();
  2525.                  do
  2526.             {
  2527.                Robot_hunting1(1540);
  2528.         }while(QTIS != 15);//第二个路口
  2529.                 pulses = 14;
  2530.                 while(pulses--)
  2531.                 {
  2532.                         motor_motion2(1550, 1450);        //直行
  2533.                 }
  2534. //              findLline();
  2535.                 TurnLeft_nDegree(90);
  2536.                
  2537.                 delay_nms(200);
  2538.                         if(hadedone >= 6){
  2539.                                 process=12;
  2540.                         return;
  2541.                 }
  2542. //                      if(hadedone >= 5){
  2543. //                      //process=aim;
  2544. //                      //goBackCenter();
  2545. //                //gotoG();
  2546. //                      gotoH();
  2547. //                              return;
  2548. //              }
  2549. //             
  2550.                 if(hadedone >= 4)
  2551.                                 {
  2552.                                         TurnRight_nDegree(180);
  2553. //                                      do
  2554. //                                      {
  2555. //                                              Robot_hunting1(1550);
  2556. //                                      }while(QTIS !=15);//第一个路口
  2557. //                                      pulses = 15;           //这个值是需要修改的
  2558. //                                      while(pulses--)
  2559. //                                      {
  2560. //                                              Robot_hunting2(1530);
  2561. //                                      }
  2562.                                        
  2563.                                        
  2564.                                         }
  2565.                        
  2566.                
  2567.                
  2568.                 else
  2569.                 {
  2570.                
  2571.                
  2572.                
  2573.                
  2574.                 do{
  2575.                         Robot_hunting(1540);   
  2576.                         //delay_nms(50);
  2577.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2578.                 }while(distance>3);
  2579.                 pulses = 10;                      //前冲步数   可修改  优化
  2580.                 while(pulses--)
  2581.                 {
  2582.                         motor_motion2(1550, 1450);        //直行
  2583.                 }
  2584.                 delay_nms(50);
  2585.                 Turn_180();
  2586.                 process=11;
  2587.         }
  2588. }
  2589. /*---------------------------------------------------------------------------------------------------------
  2590. *       Function name：StartCarryToScore(void)
  2591. *  Description：开始搬运直线上物块
  2592. ---------------------------------------------------------------------------------------------------------*/
  2593. void StartCarryToScore(void)
  2594. {                      
  2595.         count = 1;                // 每次从1开始
  2596.         printf("hadedone: %d\n",hadedone);
  2597.         if(hadedone >= 6){
  2598.                 process = 12;
  2599.                 return;
  2600.         }
  2601.        
  2602.        
  2603.         if(hadedone >= 4&&hadedone <6){
  2604.                
  2605.                 if(f_flag==1)
  2606.                 {
  2607.                         FCheck();
  2608.                         FCarry();
  2609.                 }
  2610.                 else if(g_flag==1)
  2611.                 {
  2612.                         GCheck();
  2613.                         GCarry();
  2614.                 }
  2615.                 else if(h_flag==1)
  2616.                 {
  2617.                         HCheck();
  2618.                         HCarry();
  2619.                 }
  2620.                 else if(i_flag==1)
  2621.                 {
  2622.                         ICheck();
  2623.                         ICarry();
  2624.                 }
  2625.                        
  2626.                                  
  2627.                 }  
  2628.         else
  2629.         {
  2630.                 delay_nms(200);
  2631. //              printf("Color: %d\n",Robot_checkColor());
  2632.                 printf("Color: %d\n",xm_colourjudge(pcolor));
  2633.        
  2634.                 colorCheck = true;
  2635.                 while(colorCheck)
  2636.                 {
  2637.                         //color = Robot_checkColor();//识别颜色
  2638.                         color =xm_colourjudge(pcolor);
  2639.                         count++;
  2640. //                      if(color == 4)
  2641.                                         //color = Robot_checkColor();//识别颜色
  2642. //                                      color =xm_colourjudge(pcolor);
  2643.                         switch(color)//颜色选着
  2644.                          {
  2645.                                         case Red:RedCarry();colorCheck = false;break;
  2646.                                         case White:WhiteCarry();colorCheck = false;break;
  2647.                                         case Yellow:YellowCarry();colorCheck = false;break;
  2648.                                         case Black:BlackCarry();colorCheck = false;break;
  2649.                                         case Blue:BlueCarry();colorCheck = false;break;
  2650.                                         default:colorCheck = true;break; ////////////////////////////////////////////////////////////////////////////////
  2651.                         }
  2652.                 }
  2653.         }
  2654.  
  2655.                 if(hadedone >= 6){
  2656.                        
  2657.                                   process = 12;
  2658.                                 colorCheck = false;
  2659.                          }
  2660. }
  2661. /*---------------------------------------------------------------------------------------------------------
  2662. *       Function name：CarryIpoint(void)
  2663. *   Description：把I点位置色块搬运到对应的区域上  
  2664. ---------------------------------------------------------------------------------------------------------*/
  2665. void CarryIpoint(void)
  2666. {
  2667.                 Ihavesome=0;
  2668.         do
  2669.                 {
  2670.                         motor_motion2(1540, 1460);
  2671.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2672.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2673.                                 if(distance > 4 && QTIS == 15){
  2674.                                         have = false;
  2675.                                         Hback();             //
  2676.                                         process=3;     //*****待改******待改***********待改******待改***********待改*********************待改************************8888
  2677.                                         return;
  2678.                                 }
  2679.                 }while(distance > 4);  //
  2680.                 pulses=15;
  2681.        
  2682.                 do
  2683.                 {
  2684.                         motor_motion2(1550, 1450);        //直行
  2685.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2686.                 }while(QTIS != 15);
  2687.                 pulses=35;
  2688.                 while(pulses--)
  2689.                 {
  2690.                         motor_motion2(1550, 1450);        //直行
  2691.                 }
  2692.                
  2693.                         motor_motion1(1450,1550,14);//后退
  2694.                   TurnLeft_nDegree(65-9);
  2695.                   findLline2();
  2696.                
  2697.                 do
  2698.                 {
  2699.                         Robot_hunting1(1550);
  2700.                 }while(QTIS !=15);
  2701.                
  2702.                
  2703.         stop();
  2704.        
  2705.        
  2706.                         pulses=30-10;
  2707.                 while(pulses--)
  2708.                 {
  2709.                         motor_motion2(1550, 1450);        //直行
  2710.                 }
  2711.                
  2712.                
  2713.         stop();
  2714.        
  2715.        
  2716.                 TurnLeft_nDegree(90+45-10-5-20-10);
  2717. //              findRline1();
  2718. //              findRline1();
  2719.                
  2720.                
  2721.         stop();
  2722.        
  2723.        
  2724.                 do
  2725.                 {
  2726.                         Robot_hunting1(1550);                           //直行        
  2727.                 }while(QTIS != 15);  
  2728.                
  2729.                
  2730.                         pulses=15;
  2731.                 while(pulses--)
  2732.                 {
  2733.                         motor_motion2(1550, 1450);        //直行
  2734.                 }               //直行十字路口
  2735.                
  2736.                 process=11;
  2737.        
  2738.        
  2739.        
  2740.        
  2741. }      
  2742.        
  2743. void CarryFpoint(void)
  2744. {
  2745.         if(Fhavesome==1)
  2746.         {
  2747.                 Fhavesome=0;
  2748.                         do
  2749.                 {
  2750.                         motor_motion2(1540, 1460);
  2751.                         QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2752.                         distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
  2753.                                 if(distance > 4 && QTIS == 15){
  2754.                                         have = false;
  2755.                                         Gback();
  2756.                                         process=3;     //*****待改******待改***********待改******待改***********待改*********************待改************************8888
  2757.                                         return;
  2758.                                 }
  2759.                 }while(distance > 4);  //机器朝着F点位置物块方向直走
  2760.                 pulses=15;
  2761.         do
  2762.                 {
  2763.                         motor_motion2(1550, 1450);        //直行
  2764.                 }while(!QTI_State(QTI4_Pin));
  2765.                 pulses=35;
  2766.                 while(pulses--)
  2767.                 {
  2768.                         motor_motion2(1550, 1450);        //直行
  2769.                 }
  2770.                
  2771.                         motor_motion1(1450,1550,14);//后退
  2772.                   TurnRight_nDegree(65-3);
  2773.                   findRline();
  2774.                
  2775.                 do
  2776.                 {
  2777.                         Robot_hunting1(1550);
  2778.                 }while(QTIS !=15);
  2779.                         pulses=30-5;
  2780.                 while(pulses--)
  2781.                 {
  2782.                         motor_motion2(1550, 1450);        //直行
  2783.                 }
  2784.                 TurnRight_nDegree(90+45-10);
  2785.                 do
  2786.                 {
  2787.                         Robot_hunting1(1550);                           //直行        
  2788.                 }while(QTIS != 15);  
  2789.                
  2790.                
  2791.                         pulses=15;
  2792.                 while(pulses--)
  2793.                 {
  2794.                         motor_motion2(1550, 1450);     
  2795.                                         process=11;
  2796. }
  2797. }      
  2798.         }
  2799.        
  2800. void GoStart(void)
  2801.  {
  2802.          do
  2803.         {
  2804.                 Robot_hunting1(1550);
  2805.         }while(QTIS != 15);//判断是否到达第一个路口
  2806.        
  2807.         pulses = 15;
  2808.         while(pulses--)   //走出出发区
  2809.         {
  2810.                 motor_motion2(1650, 1350);        //直行
  2811.         }        
  2812.        
  2813.         do
  2814.         {
  2815.                 Robot_hunting1(1550);
  2816.         }while(QTIS != 15);//判断是否到达第一个路口
  2817.        
  2818.         pulses = LRp-2;
  2819.         while(pulses--)
  2820.         {
  2821.                 motor_motion2(1550, 1450);        //直行
  2822.         }
  2823.         //  gai wei
  2824.         pulses = 2;
  2825.         while(pulses--)  
  2826.         {
  2827.                 Robot_hunting1(1550);    
  2828.         }        
  2829.        
  2830.   }    
  2831.  
  2832. void moveBack(){
  2833.         printf("gohome");
  2834.         pulses = 10;
  2835.         while(pulses--)
  2836.         {
  2837.                 Robot_hunting1(1560);     //直行
  2838.         }
  2839.         do
  2840.         {
  2841.                 Robot_hunting1(1560);
  2842.                 QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
  2843.         }while(QTIS != 15);//判断是否到达第一个路口
  2844.         pulses = 40;
  2845.         while(pulses--)
  2846.         {
  2847.                 motor_motion2(1550, 1450);        //直行
  2848.         }
  2849.         process = 13;
  2850. }
  2851.  
  2852.  
  2853.  
  2854.  
  2855. void turn_back(){
  2856.         Turn_180();
  2857.         process = 11;
  2858.         return;
  2859. }
  2860.  
  2861.  
  2862. //***************************************************************************************
  2863. void EXTIX_Init(void)
  2864. {
  2865.  
  2866.         EXTI_InitTypeDef EXTI_InitStructure;
  2867.           NVIC_InitTypeDef NVIC_InitStructure;
  2868.  
  2869.    GPIO_InitTypeDef GPIO_InitStructure;
  2870.  
  2871.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOE,ENABLE);//使能PORTA,PORTE时钟
  2872.  
  2873.         GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_5;//|GPIO_Pin_6|GPIO_Pin_7;//KEY0-KEY1
  2874.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //设置成上拉输入
  2875.         GPIO_Init(GPIOE, &GPIO_InitStructure);//初始化GPIOE4,3
  2876.  
  2877. //      //初始化 WK_UP-->GPIOA.0       下拉输入
  2878. //      GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_0;
  2879. //      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //PA0设置成输入，默认下拉        
  2880. //      GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.0
  2881.  
  2882.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);     //使能复用功能时钟
  2883.  
  2884.  
  2885.  
  2886. //   //GPIOE.3    中断线以及中断初始化配置 下降沿触发 //KEY1
  2887. //      GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource3);
  2888. //      EXTI_InitStructure.EXTI_Line=EXTI_Line3;
  2889. //      EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;    
  2890. //      EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
  2891. //      EXTI_Init(&EXTI_InitStructure);         //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
  2892.  
  2893. //   //GPIOE.4    中断线以及中断初始化配置  下降沿触发 //KEY0
  2894. //      GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource4);
  2895. //      EXTI_InitStructure.EXTI_Line=EXTI_Line4;
  2896. //      EXTI_Init(&EXTI_InitStructure);         //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
  2897.  
  2898.  
  2899.  
  2900.    //GPIOE.5      中断线以及中断初始化配置  下降沿触发 //KEY0
  2901.         GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource5);
  2902.         EXTI_InitStructure.EXTI_Line=EXTI_Line5;
  2903.                 EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;    
  2904.         EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
  2905.         EXTI_Init(&EXTI_InitStructure);         //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
  2906.        
  2907.                 NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;                      //使能按键KEY0所在的外部中断通道
  2908.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;    //抢占优先级2
  2909.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;                                   //子优先级0
  2910.         NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                                                         //使能外部中断通道
  2911.         NVIC_Init(&NVIC_InitStructure);           //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
  2912.  
  2913. }
  2914.  
  2915.  
  2916.  
  2917. //******************************************************************************************/
  2918.  
  2919. void EXTI9_5_IRQHandler(void)
  2920. {
  2921.         if( EXTI_GetITStatus(EXTI_Line5) != RESET)
  2922.         {
  2923.                 count_tcs230++;
  2924. //              tim5_interruput_flag=0x01;
  2925.         }
  2926.         EXTI_ClearITPendingBit(EXTI_Line5);
  2927.        
  2928. }
  2929.  
  2930. //******************************************************************************************/
  2931. int main(void)
  2932. {              
  2933.                                                 Peripheral_Init();//各模块初始化配置//
  2934.                                                 TIM5_Int_Init(0xfff0-1,720-1);
  2935.                                                 delay_init();
  2936.                                                 EXTIX_Init();
  2937.                                         process =11;//4;//7;//5;//6;     //将C/B/A/D/E/G/H点有的色块搬到开始线上，如果比赛，只需要修改该参数即可  程序首先从case11开始
  2938. //                                              process =8;      //将C/B/A/D/E/G/H点有的色块搬到开始线上，如果比赛，只需要修改该参数即可  程序首先从case11开始
  2939.                                                 led=1;
  2940.                                                 delay_nms(1000);
  2941.                                                 led=0;
  2942.                                                 delay_nms(1000);
  2943.                                                 xm_TCS230_WhiteBalance(pcolor);  //白平衡
  2944.        
  2945.        
  2946.                                                 led=0;
  2947.        
  2948.        
  2949.                                                 delay_nms(1000);
  2950.                 led=0;
  2951.                 delay_nms(1000);
  2952.                 delay_nms(1000);
  2953. //              process = 8;
  2954.                 //GoStart();             //走出开始区域
  2955. //      process = 11;
  2956.                 while(1)
  2957.                 {
  2958.                 //printf("process:%d\n",process);
  2959.    //     motor_motion2(1500, 1500);           
  2960.                        
  2961.                        
  2962.        
  2963. //                     
  2964. //                      process = 9;
  2965.                         switch(process)  //开始时process=1
  2966.                         {
  2967.                                 /**************一般1~10选着用不到********************************/
  2968.                                 case 1: CCheck();
  2969.                                         CarryCpoint();
  2970.                                                 break;//超声波检测c点
  2971.                                
  2972.                                 case 2: BCheck();
  2973.                                         CarryBpoint();
  2974. //                                                              while(1);
  2975.                                                 break;//超声波检测b点
  2976.                                
  2977.                                 case 3: ACheck();
  2978.                                         CarryApoint();
  2979.                                                                
  2980.                                                 break;//超声波检测a点
  2981.                                
  2982.                                 case 4: ICheck();
  2983.                                                 CarryIpoint();
  2984.                                                 break;//超声波检测i点    //i点为颜色识别跳出程序所在之处
  2985.                                
  2986.                                 case 5: HCheck();
  2987.                                         CarryHpoint();
  2988.                                                 break;//超声波检测h点
  2989.                                
  2990.                                 case 6: GCheck();
  2991.                                         CarryGpoint();
  2992.                                                 break;//超声波检测g点
  2993.                                                
  2994.                                 case 7: FCheck();
  2995.                                         CarryFpoint();
  2996.                                                 break;//超声波检测f点
  2997.                                                
  2998.                                 case 8: ECheck();
  2999.                                         CarryEpoint();
  3000.                                                 break;//超声波检测e点
  3001.                                                
  3002.                                 case 9: DCheck();
  3003.                                         CarryDpoint();
  3004.                                                 break;//超声波检测d点    
  3005.                                                
  3006.                                 //case 10:turn_back();break;
  3007.                                 case 11:StartCarryToScore();
  3008.                                                 break;//将C/B/A/D/E/G/H点有的色块搬计分区
  3009.                                                
  3010.                                 case 12:moveBack();
  3011.                                                 break;
  3012.                                 default:break;
  3013.                         }
  3014. //
  3015. //                      while(1);
  3016.         }
  3017. }
  3018.  
  3019.  

回复 2 rss

标题 提交人 语言 时间
Re: 2 1 text 2 天 前.
Re: 2 1 text 2 天 前.

回复 "2"

这儿你可以回复上面这条便签

captcha

贴代码片段 - Code666