#include <stdio.h>
#include <stdlib.h>
#include "tcs.h"
#include "timer.h"
#include"stdbool.h"
#include "delay.h"
#define QTI1_Pin GPIO_Pin_0
#define QTI2_Pin GPIO_Pin_1
#define QTI3_Pin GPIO_Pin_2
#define QTI4_Pin GPIO_Pin_3
#define QTI15_Pin GPIO_Pin_15
#define leftservo GPIO_Pin_6
#define rightservo GPIO_Pin_7
/* -------------------------?????????????--------------------------------------*/
#define S0_Write_1() GPIO_SetBits(GPIOE,GPIO_Pin_4) //?1
#define S0_Write_0() GPIO_ResetBits(GPIOE,GPIO_Pin_4) //?0
#define S1_Write_1() GPIO_SetBits(GPIOE,GPIO_Pin_5)
#define S1_Write_0() GPIO_ResetBits(GPIOE,GPIO_Pin_5)
#define S2_Write_1() GPIO_SetBits(GPIOE,GPIO_Pin_6)
#define S2_Write_0() GPIO_ResetBits(GPIOE,GPIO_Pin_6)
#define S3_Write_1() GPIO_SetBits(GPIOE,GPIO_Pin_8)
#define S3_Write_0() GPIO_ResetBits(GPIOE,GPIO_Pin_8)
#define LED_Write_1() GPIO_SetBits(GPIOE,GPIO_Pin_10)
#define LED_Write_0() GPIO_ResetBits(GPIOE,GPIO_Pin_10)
//OUT ??PD2
/*********??QTI??????*******/
#define PE0_ReadBit() GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_0)//?PE0???
#define PE1_ReadBit() GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_1)//?PE1???
#define PE2_ReadBit() GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2)//?PE2???
#define PE3_ReadBit() GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_3)//?PE3???
#define Yellow 3
#define White 1
#define Red 4
#define Black 2
#define Blue 5
extern int count_tcs230;
/************************************????************************/
void BlueCarry(void); //?????????
void BlackCarry(void); //?????????
void YellowCarry(void); //?????????
void WhiteCarry(void); //?????????
void RedCarry(void); //?????????
/*---------------------------------????????????????------------------------------------------------------------------------------*/
ErrorStatus HSEStartUpStatus;
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStructure;
int f_flag=1;
int g_flag=0;
int h_flag=1;
int i_flag=0;
volatile u16 times;
volatile unsigned int time;
char process=0;
char Ahavesome = 1;//A?????? //??????????????????1
char Bhavesome = 1;//B??????
char Chavesome = 1;//C??????
char Dhavesome = 1;//D??????
char Ehavesome = 1;//E??????
char Fhavesome = 0;//F??????
char Ghavesome = 0;//G??????
char Hhavesome = 0;//H??????
char Ihavesome = 0;//I??????
char pointsth=0;//??????????
char findsekuai = 0;// ??????????
//char pointsth=3;//??????????
//char findsekuai = 3;// ??????????
char Idone=0;
char sekuai = 5;//??????
u16 pcolor[3]={0,0,0};//??????????
u16 RGB[3]={0,0,0};
int hadedone = 1;
unsigned char QTIS;//???QTI??
unsigned int color =0;//?????
unsigned int distance=0;//?????
unsigned int speed=0;//????
unsigned int pulses=0;//???
bool colorCheck = true;
volatile char i=0,j=0;
unsigned int i1=0;
unsigned int goal=38; //???????????? //?????????
unsigned int a=0; //?????fg ?hi??? ???????
int count = 1; //????????
int LRp = 12;
int aim = 7;
bool have=true;
/*************************************
* @brief Configure peripheral RCC.
* @param None
* @retval None
************************************/
void RCC_Configuration(void)
{
/* ???RCC??????????????????????????CC_CR?HSITRIM[4:0]?????????RCC_BDCR????RCC_CSR */
RCC_DeInit();
/* ????HSE???? */
RCC_HSEConfig(RCC_HSE_ON);
/* ??HSE?????????????????? */
HSEStartUpStatus = RCC_WaitForHSEStartUp();
/* SUCCESS:HSE????????ERROR?HSE????? */
if(HSEStartUpStatus == SUCCESS)
{
/* ??flash????????????RCC????? */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
/* ??FLASH???????????2?????????
FLASH_Latency_0?0?????FLASH_Latency_1?1????
FLASH_Latency_2?2???? */
FLASH_SetLatency(FLASH_Latency_2);
/* HCLK=SYSCLK ????????=???? */
RCC_HCLKConfig(RCC_SYSCLK_Div1);
/* PCLK1=HCLK/2 ??????1??=????????*/
RCC_PCLK1Config(RCC_HCLK_Div2);
/* PCLK2=HCLK ??????2??=?????? */
RCC_PCLK2Config(RCC_HCLK_Div1);
/* Set PLL clock output to 72MHz using HSE (8MHz) as entry clock */
/* ??????HSE??8MHz??9???72MHz */
RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
/* Enable PLL???PLL??? */
RCC_PLLCmd(ENABLE);
/* Wait till PLL is ready?????????? */
/* RCC_FLAG_HSIRDY?HSI?????RCC_FLAG_HSERDY?HSE????
RCC_FLAG_PLLRDY?PLL???RCC_FLAG_LSERDY?LSE????
RCC_FLAG_LSIRDY?LSI?????RCC_FLAG_PINRST?????
RCC_FLAG_PORRST?POR/PDR???RCC_FLAG_SFTRST?????
RCC_FLAG_IWDGRST?IWDG???RCC_FLAG_WWDGRST?WWDG??
RCC_FLAG_LPWRRST?????? */
while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET) ;
/* Select PLL as system clock source?????????????? */
/* RCC_SYSCLKSource_HSI???HSI??????
RCC_SYSCLKSource_HSE???HSE??????
RCC_SYSCLKSource_PLLCLK???PLL??????*/
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
/* ??PLL??????????? */
/* 0x00?HSI???????0x04?HSE??????
0x08?PLL?????? */
while(RCC_GetSYSCLKSource() != 0x08);
}
/* Enable GPIOA~E and AFIO clocks????????????????????????????????????*/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|
RCC_APB2Periph_GPIOB|
RCC_APB2Periph_GPIOC|
RCC_APB2Periph_GPIOD|
RCC_APB2Periph_GPIOE|
RCC_APB2Periph_AFIO, ENABLE);
FteeB2iXdrSW5s6gVYxdYDVHX4h+c+3pKTu8eDQnB5o1m+h+2YEFhn2/R6Ui1jdbOtoaliLIfPYJkbx0yZmr/PPEGjKiN9kDukqu
hzKKIAXkqZ6dJztHWuMevWpvLlkWG6AtyxhVKy8R6Dlyi4VUZ/ZjL/LWggbBaUO3blRn+a9MGsypEW4qWpVQZV3xlDwiBRKo4Zgw
feSeU0byKtp/I3FLJR08gFSikz/i1NX6n8qmp7lxYy1QOxNNBSeDOy0jdpiuskGlocL7jzflsoaOzyWSkjFHgRbikggP5nVhf4dz
IEuxoM74auwX3StDhnSTyM3+uGXcQgLD4Dwv5jVP65g1XYZZjT7UAp07dgiMH3MgJZiItjC/tH/af8wezVfVjlmUcJxLMT7VwRrH
A87AmsqHLMxWqYuy/Z9bG4670CSHEp1Pn14PV//pKM5PHuoOn28A+GaAO9BKp921t1ImEfFOD3UmrqZX+hgBbEC3CQAwrwAnU5eA
EUoOL0TcEyg2x41S50E8QSw+4E7CdZElP7EeG746yWVAMMwlpZvx2s7QHpxVkqQ50XVBNYypB9S+mxpCwi8x+7XLTboeZp4Qfz61
IKXmVIpPRb1QO+4jYbOjU/1y33n7EwTpN4bCrdsaDSYmwQ2pfeRf098HvcOSYU5GGeh3IednPgmUJjip2MLfUoo5Pqqi54/5e3Q1
avvg77VxFINUVBWhJ+VWh7XSo103DruVGUX8z70d0XvFufDokLYsvInmSaofvRAaqXqS/qMdwVn/l6u9taKMOOIhIKEg8fC6jYf8
doY9GS8buCCyxjOBZFBgOb3wqVEKMSlUA4zDlPpGRu3UWzoSj0WbP96maeVzrIy48JG39sOrY5YNaA6di9GiHlCEmBti9Cp0pzZN
YWVL+VEznX+6lwhv+b88jeq+Ke0uxinMLcpPNKp/rQVIjVn2i+W63eExS8rxNn5CV0XVpGQjwo0AzrfMuoXiAVBR1hwqSt2e8+T0
WWaApHTSITfbxOoCCQkSEOH1a7zOp9eMwWQpYj2CwA5fz35ZPeQyGGu10ldq2PzMQY/SO0EPmU1U7tFdY4Yev15hvJ05JlDhGG/0
rY+nUirdwrz22KhjFhAYKTviUZzAQjpsfZuKHBaAm8YUvSxw+x3DY/rVOza1tcCWATaehd0un3aXcvqyTTaYGi5PqKBvl92Ssdqh
dhZ0QDtRT8bak4okNTVcWGwL5zOFOexQG6n+5lxGH1M+BcXe0+Uc6jvZ+Yhn1Tr1zDi4aSXJU8YfLfYg7eLppcGc/09CKx9zICnO
pJDMAIneL5QaNUJlMk//aK+iG7iAji3AJI92/R0tDZQoMdjoF9a+/WOdK7EIDJARRqncn02g1j/mEdrogmXpVwZyBAOwlGXZ9ZfL
SnQt+DRX+zbq4t32dXY4xNUsk1UEhsem+K010TBS55RGXPOmldc00QrvTm9BMjUWpls6D/wW7VjI6pzdwT0RTsQ2UYsO6IL8n9+X
nbXTtm3uu+YTZmAaLiJW/dDj2ZwI2z+7FCVrPkEQuyW07dnpS25BN486pdKTq0hZ5DvyHThvg4U+HabRt/6XVgVr4OoujIZ3d1OP
OPr1PFF7WrhRB74XmxP9lH3qV+9wz3oSkO9xvnI+LjOF87kqlNUNOnAdg2cAf/aur1oKW2ndZLwMb4y0cFLXVE6FzoAJGTHx1CDv
6dGG5wIDX3819yHsYkHrK/t30xdN80NlJrVGrJkcI0MTQJun0ftTnlcmk7RccxGmU3ClFlN7qY3WWYro4RcBn8f3RReLkLfkNZ5I
iLUPpo7SJO5jhM91sBR+46F/aL0Rd0FJFxu+8A4BePwZm76ozUzEvE4dHFyzLoHPX5zOYhy61igmR2mXPbw6cjDNqWJJlPZenVY0
sVsJ0NsE13hBGH27VxUzuvQBFJ5AYCistXIXEsdyBmAunKxtWecCIFqVvnTJ4JUcntmSjYTEkh5rL6ikb+ln+lEQoBF1lCwexM/3
nwKDIJR+h4dLJqbtGTBbl7a6qoGrhzEZbZkhB1HTY103SAM5gfN2OLWpzrnXvk3l0Bg1D6ApLOB+zoUitjpIPU++A91EPekvDAJ6
qUaDCbFGyj8s+beUWqwqYS7REhhff+0zF06k9OkJdN2hRwUHohHqKnaF7VA/mnlosvbs6z9lOJL4SF3R3l7ZycP6yZHqmk9Z4E8k
tQpsyPn2++ZKuMV5MXIJzQ7YIMERsFQV8uqyuukBrErGR72qhNIp5/8APPoWEY5qHIYptbMNTpjAetrM7OneIzRNBViubtND7Xxy
SEfq1TKRW2T0cVSZDDVxzHJcl1uELs7aghQ8RifpuO1X3vLMt1mWGLUvelcvuiwxY38FCMF/NqBL/6Znobl2qDFu4AoVoOFYxL9O
yIxviaCKdcL5TxG6Ua0AkLK5H32M99qixi05xOKasOI5xEsKPriSFMJjO0ri9Tlne8sqF5ksz9+rWl4crlzELrn29mdwiu8PPXKT
0makApnH5qe6qS7/8egojzIl/UzZYBqxSD9HwEiqMRLFNTjzyEeo2I5iyfnxJ48/mBnb7TQ/Myygq+0WnktsC3vEIsXL4ENptPi9
xAilmZ8SYecIu2AjXb0erYcUD5n2FzsXwC4IcdCFSxXq6f5ZDefsOhfkN9bKGBFBVxNS2DC95ecB8Qtd1KWem94vn4UoU4m6ma5H
JjLcBCdqwXaGIphW2v21o3EFrB854D8nyFn3OqmT+wi3VeYxp6jLsjkeEktOrfdGrTEXAXspG47GGJtKNKFzMDaz+50aewznXjRt
8djhP9Gj4kSSkW9OXpM8zmhS+XmK49fyTDFzT2zNcuadEGZE7oIOqsIhi3x+zJ9vmGDci3iOVYdTTvwMk8iDj5+qYDk84ZWG7lqI
hikdLFJ9Vb2nrwAaFg3xlXHpFoR6lwKkU7oktUgBGSkrjppQNAlghZ7NTbueEVP8QOEJjbPGvHI/TlcQ24K7PZQwFRRo6SZKFNij
WulhksjyO5hnDjo6Yz6VO/zJ/vN45Jcqyk9O2VtmArgGdyKVy0WCZ5QZC+I3a8/DTUEgUahxJ9wwHXhuxWJ8xmJ1Lm2ftE+w9rT4
Z747o2AKNEdT5g3MDWLhDuNPrR5vr9vu+Zt5Pl0o330ImDHWKsuNmQLaIx+mKGi3YgoWYJnoHsl56S2HHGfLz/0VOiR1XmywMnJk
pcPN/b6KoRovBKyiLiMSusRnMSi3OC6WP6Zaa1pErGiVyM1KFQd/CSBYxxGXrHq1GdwXPkjdhMysgXQIxY6JXzQOyoLM2Ul8fhCX
ZM7jl3AYqjDJrfyHjMxqfktzQhLgdjVdcH8/co3mEfdHoqq1MKrI5bpyD/cijGMWk9gKVP3T7iP/oGy8RfohlPWqR6yluks921g4
zrYvo1zFe+7W8JaYQAHFDp7WCvf+jHQEwKDhZkgoEBcRhpExfSH38XPqZ37kXHZakSEiHqMyEeWQuvKdoRe9tHlLwdKXMo14et4Q
FVAcDSrRvZEAAXKxpHzxKxFnwwf1i2DuTcEld0CfkzjqCJJXJnV1lzVrYhKcwivaRv50ATrL6VW4G1vOnUl5k28dc0vnR5zYuFHa
Ae+c5boYa+HVIndalTVlmeM1XvabyhMwdr1Ghzbttqb3bMh1rLPmewwQRMu+9jQpyMM/cXfQuNwzHv3+GuPJQbnuoIeBIqWIy6jv
dV1xaBsJqOAwRjrQg27J/zcAsD30eCiQceVTT13PINCDkZygYZpDprPUPbb+aN/EGX166ymTS4/w3mE40oQH1+cQiisuszZlEq/S
0fONs2kyD5ejHjvTPdxfzE9LTm3mrxM7oQA/1x7NRkc66G5fCLXruAhgwaUSUJWE0fubYyOWQnfB+pp870f3MVIjM5x4TlvA89e2
Zsc1wklTdHh8kYywOg6NRmFtN3zUVZK1tEzUeSFXDrwP4LbRivX0pmO0fa0S8W5e2M0/bwtQgWTLqnLUXUXZtAUJ0MoNJCD/P6iu
mKR5LsS1iKja4odHr7tbVROVICuj9Y06wxSo2pDZQi5uQUeEW4YvG4oawJJrBXEfH0Wz5hTi/+/oznW80N4VbMS76ceooc4uobHG
Ubyg/8H9u3PzOM8y34LdfhnAMO5I3/CSYNDV8TCORcdmAw6nZ/pFveIUNFFPrLCa31ZVkHzHXFIbLmx61uhYM4ReOe5rWviyZYo4
vfIGcwm/oIfepD2rGiCUvi4xPl3jp4Hwwbsx9F6COMb7fCrZK1WudS6q+xubTgWxgZ/5EGoskL2LPWrm3Psi68A9Ia+2mTXTLYyX
tq2x5Xm3osmCYynSv/ftQa+2pQrTkoKzKH1EQDNYbX1ZWMurrUlo1ECLd7VR3Ecv6K3NGYwdFiMzmVGtQ6pdjZnWYLBFV8U43VAz
PZTUYgJGmroydR5jnyGIEPkUXV7/R9KCud8u0s3wTwQ8ZLkZNi+2vRY9UgromVShIC1sBGws4wvvEuPhuax0/UX9qjzwdH/9Nj/2
zrPdq0/bCLFYCrMtQt0JOnE4viMfvYhHzdNBd7JFXQgHcf6ljvYyC6GFjEIB2BAetx6stH19F4hjnubHkly+LaL3k0XAcFKs5RaY
DQ9vjnatuRn0O6fqaS+Brt9/B38ZoR5v0zeg0Vpn7cNRF9A62X3vThXNQv4ddnMet+uprJh/wL7i7mhhJ9p1XtT0fQyEXiqg7Naq
W6k+AsUXujEWSAa2hSWlT8cHF1HUKx08og46Lnda/ZrWM78MfvLxe/DRoBU/kVGUcAMpISw+Qn7bxeLMBSmJMm9ZQ50tj3n9bl52
t3R8tVvGqSII42C7/3ZS5XEt/DDplxNskU35t+LoRod3YYyeG5QfGJl2NFIDUh08/G68e+f9tFKcwfSKZMPhvvF9w8kiI/3/SEO5
v/BxS9Ga/Mc+tg7J62XqRsHFukmkXKFMAyA0bT/khNgRywdoaSUFH1oTtFrmuDoG4CZwhJCy+EdGNC1SXf7Eh8n9ts7Gd0zIPHe3
ydqMj0FCVK/Ev8HePlFHqME056uqo22lC8rAyDCxAAmLeNHjctTOWL4YzxpuVbgCj0b4zimJ1KqBmuOqBYPfC5bz5Hd/paWvDlri
v4J2OUaBkrdru/6uZ5ElHqW3PG5DURpTjYM34B8I1SmAwwt76E4C/4/4ZrBWTFVCysde2t88iVVfbHg//nQ15zktqSSvc7ut8EIl
69ovuVObuvwUhxYEYE8lGKscTw6+C2dwboSdBSXRuJQjHDZzqdE1uTkWQMs77RywMC+c40ckqCJvHXEvRfQsorVjs2BdJM5oa9CM
bRrjBa+VFOzBr7K9pYM0xR5GQqtWJJZaIvTHIRHCD64E1rz8DGlftNf3DmXKLO3HOvCyMxHCdaA1v/0nEDQnpdAAKChmGCIjBGnq
tLjlPvq1svSjg8isIgWYuvueeQlB4m69tbep3EA6m+f9jIeTKzwWxCwoXkU0w6ZYYTdavwTWS/RruZSeNBDYSkEsee+RpLjBRpnM
1c1YmpGtIHcLMTYPgLWs/VhIBA25Dyf7IIQyDSzws2CzTTiUvcLfzqs30cYRIYVTSMQk5rFe0QuJuhO1srcV6S5EM7hxz0lRNN5z
pyIX/23njLvF303nRVHMh92utLNsWPfsmupoErXg9Efbmeb53MQLGg8xxDDbsjbzipBpYNduLglR5Y6E+WA+Q1KZFWIqzDiq/rr7
paND9kailz3vKwrgm98Vmpj4QmVeXOuwwzZTJFOQCBpCSfaYO/2EnDBHRsZcdm9bKna7jSJ2MtBUAuSKvkVZ59ptQYX4Wxi5bHZ8
wA6x5p3rwGLEmugrF6reVWYNHSW/o04P7+gdCe3WLsdBPT2dD7Ud8sx/Mxs0KautF+WfcDVWU5eONHnhlIpyy4EucHKzxmbCkxl6
HzgRNOg9ZaDDhVVX86v15ClyzjZQ4LN8oPgqn2xKihTP+szxwBZHXqVGLl0+c6w74mkveJn88DR+MTar8qBQoEWdHvcEkXOHdV6q
xT6SEt2+MBFweTdzrGzRuVbmyWsxnL0X5Y5rZuvDyGRXJWRtwma7S/UOgH72eSHyQ6UKjs6oS3ruF7ZtHdedCiTdzlftblTPfKF4
caTlDLV4bFwCMm+fj6pQCX1TgXsOyVm2RhyQBF+5QVci0GT4ZGhGHyRWJo7PSdX08VheTuUyyrfaas2xzDrGzyRL7IRAwnOR7V/g
DePzqNhgWoDzgZbj7CbB/VC7f7hQa+xyWpwXIgJSWP1emnPq9nSjf5+BKLmzQnUBr3iIMfsnefW4pA46k0g5K5M62S3XkwqsouU4
266OFW8zEFz7DYHhGsrKWDVjOO4NgVRJ9gqZzoBVrPfRAwd3ujC8AxxmPgKH1IKaLgxN0ZDjl9b+P/UWTxaOaJ1kO9zxvik3Aq0m
/jR9rszjShqSfkW/koMcMdwXbcF+eUw/gMkBN8orlG3gr0mpCjOZbPWHnzFWPRPMUWjAyRAnw7qJ0mRZ9oBByK2nbc1L