#include "stm32f10x.h"
#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);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
/* TIM3 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 ,ENABLE);
/* TIM5 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
/* Enable USART1 clocks */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
}
/************************************************
* @brief Configure NVIC.
* @param None
* @retval None
***********************************************/
void NVIC_Configuration(void)
{
#ifdef VECT_TAB_RAM
/* Set the Vector Table base location at 0x20000000 */
NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
#else /* VECT_TAB_FLASH */
/* Set the Vector Table base location at 0x08000000 */
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
#endif
/* Configure the Priority Grouping with 0 bit */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
/* Configure the Priority Grouping with 1 bit */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
/* Enable TIM2 global interrupt with Preemption Priority 1 and Sub
Priority as 5 */
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Configure the Priority Grouping with 2 bit */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/* Enable TIM3 global interrupt with Preemption Priority 1 and Sub
Priority as 6 */
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Configure the Priority Grouping with 2 bit */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_3);
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; //???
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/************************************************
* @brief Configure the TIM2 Timer for 1ms.
* @param None
* @retval None
***********************************************/
void TIM2_Configure(void)
{
TIM_DeInit( TIM2);//??TIM2???
TIM_TimeBaseStructure.TIM_Period = 99;
TIM_TimeBaseStructure.TIM_Prescaler = 7199;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, & TIM_TimeBaseStructure);
/* Clear TIM2 update pending flag[??TIM2??????] */
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
/* Enable TIM2 Update interrupt [TIM2??????]*/
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
/* TIM2 enable counter [??tim2??]*/
TIM_Cmd(TIM2, DISABLE);
}
/************************************************
* @brief Configure the TIM3 Counter.
* @param None
* @retval None
***********************************************/
void TIM3_Counter_Configure(void)
{
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;//?????????
TIM_TimeBaseStructure.TIM_Prescaler = 0x00;//????
TIM_TimeBaseStructure.TIM_ClockDivision = 0x00;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//??????
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);// Time base configuration
TIM_ETRClockMode2Config(TIM3, TIM_ExtTRGPSC_OFF, TIM_ExtTRGPolarity_NonInverted, 0);
TIM_SetCounter(TIM3, 0);//???????
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);//???????
TIM_Cmd(TIM3, DISABLE); //?????
}
void TIM4_Configuration(void)
{
TIM_DeInit(TIM4);
/* Time Base configuration */
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);
TIM_PrescalerConfig(TIM4,71, TIM_PSCReloadMode_Immediate);
/* enable preload value */
TIM_ARRPreloadConfig(TIM4,DISABLE);
TIM_SetCounter(TIM4,0); //TIM4?????
TIM_Cmd(TIM4,DISABLE); //TIM4?????
}
void TIM5_Configure(void)
{
TIM_DeInit(TIM5);//??TIM5???
TIM_TimeBaseStructure.TIM_Period = 9;
TIM_TimeBaseStructure.TIM_Prescaler = 7199;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, & TIM_TimeBaseStructure);
/* Clear TIM2 update pending flag[??TIM2??????] */
TIM_ClearFlag(TIM5, TIM_FLAG_Update);
/* Enable TIM2 Update interrupt [TIM2??????]*/
TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE);
/* TIM2 enable counter [??tim2??]*/
TIM_Cmd(TIM5, DISABLE);
}
/************************************************
* @brief Configure USART1.
* @param None
* @retval None
***********************************************/
void USART1_Configuration(void)
{
USART_InitStructure.USART_BaudRate = 115200;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
USART_Init(USART1, &USART_InitStructure);
USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;
USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;
USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;
USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;
USART_ClockInit(USART1, &USART_ClockInitStructure);
/* Enables the USART1 transmit interrupt */
USART_ClearFlag(USART1, USART_FLAG_TC);
/* Enable the USART1 */
USART_Cmd(USART1, ENABLE);
}
/************************************************
* @brief Configure the TCS230 Pins.
* @param None
* @retval None
***********************************************/
void GPIO_TCS230_Configure(void)
{
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_7|GPIO_Pin_6|GPIO_Pin_8|GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOE, &GPIO_InitStructure);
}
/***********************************************
* @brief Configure USART1 Pins.
* @param None
* @retval None
***********************************************/
void GPIO_USART1_Configure(void)
{
/*USART1_TX*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/*USART1_RX*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
/************************************************
* @brief Configure the TIM3 Pins.
* @param None
* @retval None
***********************************************/
void GPIO_TIM3_Configure(void)
{
//??TIM3?????? PD2 ETR ????????????
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}
void GPIO_Dist_Config(void)
{
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
}
void GPIO_Motor_Config(void)
{
/* Configure Motors IO*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
}
void GPIO_QTI_Config(void)
{
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_15;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //????
GPIO_Init(GPIOE,&GPIO_InitStructure);
}
u8 QTI_State(u8 pin)//?????
{
return GPIO_ReadInputDataBit(GPIOE,pin);
}
/**************************************************
* Function Name : fputc
* Description : Retargets the C library printf function to the USART.
**************************************************/
int fputc(int ch, FILE *f)
{
/* Place your implementation of fputc here */
USART1->SR;//?????????????????
/* e.g. write a character to the USART */
USART_SendData(USART1, (u8) ch);
/* Loop until the end of transmission */
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); // waiting here
return ch;
}
void delay_nus(unsigned int n) //??n us: n>=6,??????6us
{
unsigned int j;
while(n--) // ?????8M?PLL?9?8M*9=72MHz
{
j=8; // ????????????
while(j--);
}
}
void delay_nms(unsigned int n) //??n ms
{
while(n--) // ?????8M?PLL?9?8M*9=72MHz
delay_nus(1100); // 1ms????
}
void motor_motion1(unsigned int left_val, unsigned int right_val, unsigned int count) //? ? ??
{
unsigned int i;
for(i=0; i<count; i++)
{
GPIO_SetBits(GPIOC, GPIO_Pin_7);
delay_nus(left_val);
GPIO_ResetBits(GPIOC,GPIO_Pin_7);
GPIO_SetBits(GPIOC, GPIO_Pin_6);
delay_nus(right_val);
GPIO_ResetBits(GPIOC,GPIO_Pin_6);
delay_nms(20);
}
}
/*******************************************************************************
*TCS230_CurrentColor(u16 pRGB[3],u16 rgb[3])
* ??????RGB????????RGB[3]={0,0,0}?
********************************************************************************/
u8 TCS230_CurrentColor(u16 pRGB[3])
{
u8 currentcolor=0;
S0_Write_0(); S1_Write_1(); //?????(1/50)*500KHz=10KHz
LED_Write_1(); //??LED
S2_Write_0(); S3_Write_0(); //????
times=0;
TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM2, ENABLE);
TIM_Cmd(TIM3, ENABLE);
while(pRGB[0] != times);
TIM_Cmd(TIM2, DISABLE);
TIM_Cmd(TIM3, DISABLE);
RGB[0] = TIM_GetCounter(TIM3);
S3_Write_1();//????
times=0;TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
while(pRGB[1] != times);
TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
RGB[1] = TIM_GetCounter(TIM3);
S2_Write_1();//????
times=0;TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
while(pRGB[2] != times);
TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
RGB[2] = TIM_GetCounter(TIM3);
LED_Write_0();//??LED
printf("Red: %d Blue: %d Green: %d\n",RGB[0],RGB[1],RGB[2]);
return currentcolor;
}
/*******************************************************************************
* @brief TCS230_WhiteBalance Function.
* Description??????RGB???255??????????????pColor[]?
********************************************************************************/
void TCS230_WhiteBalance(u8 pColor[3]) //??????
{
S0_Write_0();
S1_Write_1();//?????(1/50)*500KHz=10KHz
LED_Write_1();//??LED
S2_Write_0();
S3_Write_0();//????
times=0;TIM_SetCounter(TIM3,0);//???3??
TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
while(TIM_GetCounter(TIM3)<255);
TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
pColor[0] = times;//??????
S3_Write_1();//????
times=0;TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
while(TIM_GetCounter(TIM3)<255);
TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
pColor[1] = times;//??????
S2_Write_1();//????
times=0;TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM2, ENABLE);TIM_Cmd(TIM3, ENABLE);
while(TIM_GetCounter(TIM3)<255);
TIM_Cmd(TIM2, DISABLE);TIM_Cmd(TIM3, DISABLE);
pColor[2] = times;//??????
LED_Write_0();//??LED
printf("Red**: %d Blue**: %d Green**: %d\n",pColor[0],pColor[1],pColor[2]);
}
/*******************************************************************************
* @brief Robot_checkColor(void)
* ??????RGB????????RGB??????
********************************************************************************/
unsigned int Robot_checkColor(void)
{
TCS230_CurrentColor(pcolor); //???????RGB?
printf("Red: %d Blue: %d Green: %d\n",RGB[0],RGB[1],RGB[2]);
//???? //??RGB?????
if((RGB[0] > 90)&&((RGB[0] - RGB[2]) > 60) && ((RGB[0] - RGB[1]) > 60)&&((RGB[2] - RGB[1]) < 80))
{
return 3;//??
}
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) ) )
{
return 2;//??
}
else if((abs(RGB[0] - RGB[2])<30)&&(abs(RGB[0] - RGB[1])<30)&& (RGB[0]<40)&& (RGB[1]<40)&& (RGB[2]<40) )
{
return 4;//??
}
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)) )
{
return 5;//??
}
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 ) )
{
return 1;//??
}
return 0;
}
int GetDis(int echo,int trig) //????????????
{
int dis;
int count;
GPIO_ResetBits(GPIOC,echo); //echo????
GPIO_ResetBits(GPIOC,trig); //trig????
TIM_SetCounter(TIM4,0); //TIM4?????
GPIO_SetBits(GPIOC,trig); //trig?? ??10us??????
delay_nus(10);
GPIO_ResetBits(GPIOC,trig);
delay_nus(100);
while(GPIO_ReadInputDataBit(GPIOC, echo) == 0);
TIM_Cmd(TIM4,ENABLE); //?????
//?????????
while(GPIO_ReadInputDataBit(GPIOC, echo)); //??echo??
TIM_Cmd(TIM4,DISABLE); //?????
count = TIM_GetCounter(TIM4);//??????
dis = (int)count/60.034;//?????,?29.034us??????1cm
return dis;
}
/*
* //?????QTI??????
*/
bool IsMLeftQtiBlack2(void) //?????QTI??????
{
if(QTI_State(QTI3_Pin)==true) //?????QTI???????
{ //?????ture ??????false
delay_nms(2);
if(QTI_State(QTI3_Pin)==true)
{
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
bool IsMLeftQtiBlack(void) //?????QTI??????
{
if(QTI_State(QTI1_Pin)==true) //?????QTI???????
{ //?????ture ??????false
delay_nms(2);
if(QTI_State(QTI1_Pin)==true)
{
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
bool IsMRightQtiBlack(void) //?????QTI??????
{
if(QTI_State(QTI2_Pin)==true) //?????QTI???????
{ //?????ture ??????false
delay_nms(2);
delay_nms(2);
if(QTI_State(QTI2_Pin)==true)
{
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
bool IsMRightQtiBlack1(void) //?????QTI??????
{
if(QTI_State(QTI3_Pin)==true) //?????QTI???????
{ //?????ture ??????false
delay_nms(2);
delay_nms(2);
if(QTI_State(QTI3_Pin)==true)
{
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
void PulseOut(uint8_t pin,int speed) //??????
{
GPIO_SetBits(GPIOC,pin );
delay_nus(speed);
GPIO_ResetBits(GPIOC,pin);
}
void stop(void)//??
{
int i;
for(i=1;i<=2;i++)
{
GPIO_SetBits(GPIOC, GPIO_Pin_6);
delay_nus(1500);
GPIO_ResetBits(GPIOC,GPIO_Pin_6);
GPIO_SetBits(GPIOC, GPIO_Pin_7);
delay_nus(1500);
GPIO_ResetBits(GPIOC,GPIO_Pin_7);
delay_nms(20);
}
}
void SpinLeft(void)// ???
{
PulseOut(leftservo,1480);
PulseOut(rightservo,1480);
delay_nms(20);
}
void SpinRight(void)// ???
{
PulseOut(leftservo,1520);
PulseOut(rightservo,1520);
delay_nms(20);
}
void TurnLeftAnyPulse(int pulses)//??????? pluses??????
{
while(pulses--)
{
SpinLeft(); //???
delay_nms(2);
}
}
void TurnRightAnyPulse(int pulses) //??????? pluses??????
{
while(pulses--)
{
SpinRight();
delay_nms(2);
}
}
void findLline2(void) //???????
{
TurnLeftAnyPulse(8);
while(1)
{
SpinLeft(); //???
if(IsMLeftQtiBlack2()) // ??????????
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
while(1)
{
SpinLeft();
if(!IsMLeftQtiBlack2())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
stop();
}
void findLline(void) //???????
{
TurnLeftAnyPulse(8);
while(1)
{
SpinLeft(); //???
if(IsMLeftQtiBlack()) // ??????????
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
while(1)
{
SpinLeft();
if(!IsMLeftQtiBlack())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
stop();
}
void findRline(void) //???????
{
TurnRightAnyPulse(10);
while(1)
{
SpinRight();
if(IsMRightQtiBlack())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
// stop();
while(1)
{
SpinRight();
if(!IsMRightQtiBlack())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
// stop();
}
void findRline1(void) //???????
{
TurnRightAnyPulse(10);
while(1)
{
SpinRight();
if(IsMRightQtiBlack1())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
// stop();
while(1)
{
SpinRight();
if(!IsMRightQtiBlack1())
{
i1++;
if(i1==2)
{
i1=0;
break;
}
}
}
// stop();
}
/*
//?????PE0_state()
//??? ???????QTI?????
//??????
//????1??????????0?????????
*/
int PE0_state(void)
{
return PE0_ReadBit();
}
/*
?????PE1_state()
??? ???????QTI?????
??????
????1??????????0?????????
*/
int PE1_state(void)
{
return PE1_ReadBit();
}
/*
?????PE2_state()
??? ???????QTI?????
??????
????1??????????0?????????
*/
int PE2_state(void)
{
return PE2_ReadBit();
}
/*
?????PE3_state()
??? ???????QTI?????
??????
????1??????????0?????????
*/
int PE3_state(void)
{
return PE3_ReadBit();
}
void motor_motion2(unsigned int left2_val, unsigned int right2_val)
{
GPIO_SetBits(GPIOC, GPIO_Pin_7);
delay_nus(left2_val);
GPIO_ResetBits(GPIOC,GPIO_Pin_7);
GPIO_SetBits(GPIOC, GPIO_Pin_6);
delay_nus(right2_val);
GPIO_ResetBits(GPIOC,GPIO_Pin_6);
delay_nms(20); //???????PWM??
}
void Robot_hunting1(unsigned int speed)
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
switch (QTIS)
{
//case 0:motor_motion1(speed, (3000-speed)-35-10,3);break;
case 1:motor_motion1(speed, (3000-speed)-35-10,3);break;
case 2:motor_motion1(speed, (3000-speed)-35,1);break;
case 3:motor_motion1(speed, (3000-speed)-35,1);break;
case 4:motor_motion1(speed+35, (3000-speed),1);break;
case 8:motor_motion1(speed+35+10, (3000-speed),3);break;
case 12:motor_motion1(speed+35, (3000-speed),2);break;
case 6:motor_motion2(speed+100, 3000-speed-100-2);break;
default:motor_motion2(1550, 1450);break;
}
}
void Robot_hunting2(unsigned int speed)
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
switch (QTIS)
{
case 1:motor_motion1(1500, 1400,2);break;
case 2:motor_motion1(1500, 1400,1);break;
case 3:motor_motion1(1500, 1400,1);break;
case 4:motor_motion1(1600, 1500,1);break;
case 8:motor_motion1(1600, 1500,2);break;
case 12:motor_motion1(1600, 1500,1);break;
case 6:motor_motion2(speed, 3000-speed);break;
default:motor_motion2(speed, 3000-speed);break;
}
}
void Robot_hunting(unsigned int speed)
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
switch (QTIS)
{
case 1:motor_motion1(speed, (3000-speed)-40-10,2);break;//??
case 2:motor_motion1(speed, (3000-speed)-40,1);break;//??
case 3:motor_motion1(speed, (3000-speed)-40,1);break;//??
case 4:motor_motion1(speed+40, (3000-speed),1);break;//??
case 8:motor_motion1(speed+40+10, (3000-speed),2);break;//??
case 12:motor_motion1(speed+40, (3000-speed),1);break;//??
case 6:motor_motion2(speed, 3000-speed-2);break; //??
default:motor_motion2(speed, 3000-speed-2);break; //??????
}
}
void Peripheral_Init(void) //??????????
{
RCC_Configuration(); //???????
NVIC_Configuration(); //???????
GPIO_USART1_Configure(); //??1 IO???
USART1_Configuration(); //??1??
GPIO_TCS230_Configure(); //?????IO???
TIM2_Configure(); // ???2??
GPIO_TIM3_Configure(); //???3????????
TIM3_Counter_Configure();// ???3?????
TIM5_Configure(); // ???5??
GPIO_Motor_Config(); //????IO????
GPIO_QTI_Config(); //QTI??????
TIM4_Configuration(); //???4??
GPIO_Dist_Config(); //
}
/*??????*/
void TurnR_toBlackLine(void) //?????????
{
motor_motion1(1515, 1515,5); //??
do
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1515, 1515,1);
} while(QTIS!=6); //???????1001 ??????
do
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
//motor_motion1(1515, 1515,1);
} while(QTIS!=6); //?????????1001
motor_motion1(1515, 1515,1);
}
void TurnL_toBlackLine(void) //?????????
{
motor_motion1(1485, 1485,10);
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1485, 1485,1);
} while(QTIS!=6); //???????1001 ??????
delay_nms(5);
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
//motor_motion1(1485, 1485,1);
} while(QTIS!=6); //?????????1001
motor_motion1(1485, 1485,1); //????
}
void Turn1_180(void) // ???? //???????????????????????
{
motor_motion1(1350, 1350,15 ); //??????
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1400, 1400,1);
} while(QTIS!=6); //??????? ??????????1001
delay_nms(5);
while(QTIS!=6)
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1480, 1480,1);
} //???????????????
}
void Turn_180(void) // ???? //???????????????????????
{
motor_motion1(1465, 1465,32 ); //??????
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1465, 1465,1);
} while(QTIS!=6); //??????? ??????????1001 0110
motor_motion2(1500, 1500);
// motor_motion1(1475, 1475,1);
// delay_nms(5);
// do{
// QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
// motor_motion1(1465, 1465,1);
// }while(QTIS!=6); //???????????????
}
void Turn_180_four(void) // ???? //???????????????????????
{
motor_motion1(1465, 1465,35 ); //??????
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1465, 1465,1);
} while((QTIS!=6)&&(QTIS!=0x0f));//while(QTIS!=6); //??????? ??????????1001
motor_motion1(1475, 1475,1);
delay_nms(5);
do{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1465, 1465,1);
}while((QTIS!=6)&&(QTIS!=0x0f)); //???????????????
}
void TurnRight_nDegree(char degree) //?? degree ???
{
motor_motion1(1522, 1522,degree);//1517
return;
}
void TurnLeft_nDegree(char degree) //?? degree ???
{
motor_motion1(1478, 1478,degree);//1485
return;
}
void Back_toBlack(void) //?????
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
while(QTIS != 15)
{
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
motor_motion1(1485,1512,1);
}
return;
}
void CCheck(void) //??C??????
{
motor_motion1(1500,1500,1);
delay_nms(200);
j=0;
process = 9;
}
void BCheck(void) //??B??????
{
// TurnLeft_nDegree(10);
findLline();
delay_nms(200);
process = 11;//1;
}
void ACheck(void) //??A??????
{
TurnLeft_nDegree(30); //???60
findLline(); //?findLline();?TurnLeft_nDegree(40);?20
delay_nms(200);
process = 1;
}
void ICheck(void) //??I??????
{
findLline();
findLline();
TurnLeft_nDegree(35+15-5);//??n?
}
void HCheck(void) //??H??????
{findLline();
findLline();
TurnLeft_nDegree(38+10+10+9-9+5);//??n?
Hhavesome = 1;
}
void GCheck(void) //??G??????
{findRline();
findRline();
TurnRight_nDegree(50);
Ghavesome = 1;
}
void FCarry(void)
{
}
void GCarry(void)
{
}
void HCarry(void)
{
}
void ICarry(void)
{
}
void FCheck(void) //??F??????
{
// findRline();
// findRline();
TurnRight_nDegree(21+90-10);//??n?
stop();
delay_nms(200);
/*j=0;
for(i=0,j=0;i<10;i++)
{
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance>0&&distance < 34)
{
j=j+1;
}
delay_nms(50);
}
if(j<1)
{
TurnRight_nDegree(2);//??n?
delay_nms(200);
for(i=0,j=0;i<10;i++)
{
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance>0&&distance < 30)
{
j=j+1;
}
delay_nms(50);
}
}
if(j<1)
{
TurnRight_nDegree(2);//??n?
delay_nms(200);
for(i=0,j=0;i<10;i++)
{
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance>0&&distance < 34)
{
j=j+1;
}
delay_nms(200);
}
}
if(j<1)
{
TurnLeft_nDegree(8);//
delay_nms(200);
for(i=0,j=0;i<10;i++)
{
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance>0&&distance < 34)
{
j=j+1;
}
delay_nms(200);
}
}
if(j >= 1)
{
delay_nms(100);
Fhavesome = 1;
}
else
{
Fhavesome = 0;
}
j=0;
a=1; */
Fhavesome = 1;
// process = 11;
// LED_Write_1();
}
void ECheck(void) //??E??????
{
TurnRight_nDegree(30);//??n?
delay_nms(200);
findRline();
delay_nms(200);
process = 2;//11;
}
void DCheck(void) //??D??????
{
findRline();
delay_nms(200);
process =8;
}
/*---------------------------------------------------------------------------------------------------------
* Function name?DownFirst(void)
* Description???A???????????????????? ,???????????
---------------------------------------------------------------------------------------------------------*/
void DownFirst(void) //??????A????????????????
{
time =0;
TIM5_Configure();
TIM_Cmd(TIM5, ENABLE); //?????5
do
{
Robot_hunting2(1550);
}while(time < 2300);
TIM_Cmd(TIM5, DISABLE); //?????5
motor_motion1(1450,1550,16); //??
Turn1_180(); //??180?
do
{
Robot_hunting2(1540);
}while(QTIS !=15);
pulses = LRp;
while(pulses--)
{
motor_motion2(1550, 1450);
}
// gai wei
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
pointsth++;
}
/*---------------------------------------------------------------------------------------------------------
* Function name?DownSecond(void)
* Description???B???????????????????? ,?????????????????DownFirst(void)
---------------------------------------------------------------------------------------------------------*/
void DownSecond(void) //??????B????????????????
{
time =0;
TIM5_Configure();
TIM_Cmd(TIM5, ENABLE); //?????5
do
{
Robot_hunting2(1550);
}while(time < 1900);
TIM_Cmd(TIM5, DISABLE);
motor_motion1(1450,1550,16);//??
Turn1_180();
do
{
Robot_hunting2(1550);
}while(QTIS != 15);
pulses = LRp;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// gai wei
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
//???????
pointsth++; //?????2???
}
/*---------------------------------------------------------------------------------------------------------
* Function name?Downthird(void)
* Description???c???????????????????? ,?????????????????DownFirst(void)
??????c
---------------------------------------------------------------------------------------------------------*/
void Downthird(void) //??????C????????????????
{
time =0;
TIM5_Configure();
TIM_Cmd(TIM5, ENABLE); //?????5
do
{
Robot_hunting2(1550);
}while(time < 1500);
TIM_Cmd(TIM5, DISABLE);
motor_motion1(1450,1550,16); //??
Turn1_180();
do
{
Robot_hunting2(1530);
}while(QTIS != 15); //??????
pulses =LRp;
while(pulses--)
{
Robot_hunting2(1545);
} //????????????????????
// gai wei
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
pointsth++; //?????3???
}
/*---------------------------------------------------------------------------------------------------------
* Function name?DownFourth(void)
* Description???D???????????????????? ,?????????????????DownFirst(void)
---------------------------------------------------------------------------------------------------------*/
void DownFourth(void) //??????D????????????????
{
time =0;
TIM5_Configure();
TIM_Cmd(TIM5, ENABLE); //?????5
do
{
Robot_hunting2(1550);
}while(time < 1000);
/*TIM_Cmd(TIM5, DISABLE);*/
motor_motion1(1450,1550,16); //??
Turn_180(); //??
// Turn_180_four();
do
{
Robot_hunting1(1550); //??
}while(QTIS != 15); //??????
pulses = LRp;
while(pulses--)
{
motor_motion2(1550, 1450-50); //??
}
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
pointsth++; //?????4???
}
/*---------------------------------------------------------------------------------------------------------
* Function name?DownFiveth(void)
* Description???E????????????????????,?????DownFirst(void)
---------------------------------------------------------------------------------------------------------*/
void DownFiveth(void) //??????E????????????????
{
time =0;
TIM5_Configure();
TIM_Cmd(TIM5, ENABLE); //?????5
do
{
Robot_hunting2(1550);
}while(time < 800);
TIM_Cmd(TIM5, DISABLE);
// motor_motion1(1450,1550,16); //??
Turn_180(); //??
// Turn_180_four();
// do
// {
// Robot_hunting1(1550); //??
// }while(QTIS != 15); //??????
//
// pulses = LRp;
//
// while(pulses--)
// {
// motor_motion2(1550, 1450); //??
// }
motor_motion2(1500,1500);
delay_ms(50);
// pulses = 2;
// while(pulses--)
// {
// Robot_hunting1(1580);
// }
pointsth++; //?????5???
}
void GotoLine(void)
{
int tempa=0;
switch(pointsth) // pointsth????0
{
case 0:DownFirst(); // ??A????
break;
case 1:DownSecond(); //??B????
break;
case 2:Downthird(); //??C????
break;
case 3:DownFourth(); //??E????
break;
case 4:DownFiveth(); //??F????
break;
default:
break;
}
tempa++;
tempa=0x22;
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryGpoint(void)
* Description???G????????????????????,????? CarryIpoint(void)
****************??????????????***************
---------------------------------------------------------------------------------------------------------*/
void Gback(){
pulses=8;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
//TurnRight_nDegree(80); //??120
findLline();
/*do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);// ??*/
pulses=15;
while(pulses--)
{
Robot_hunting1(1550); //??
}
TurnLeft_nDegree(80);//??n?
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=8;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findRline();
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//???????????
pulses = LRp-2;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// gai wei
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
}
void Hback(){
motor_motion1(1550, 1450,10);
findRline();
pulses=15;
while(pulses--)
{
Robot_hunting1(1550); //??
}
TurnRight_nDegree(100);//??
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=12;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline();
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//???????????
pulses = LRp-2;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
pulses = 2;
while(pulses--)
{
Robot_hunting1(1580);
}
}
void CarryGpoint(void) //??????G????????????????
{
Ghavesome=0;
do
{
motor_motion2(1540, 1460);
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance > 4 && QTIS == 15){
have = false;
Gback();
process=3; //*****??******??***********??******??***********??*********************??************************8888
return;
}
}while(distance > 4); //????G?????????
pulses=15;
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=50;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
motor_motion1(1450,1550,30);//??
TurnRight_nDegree(65-3);
findRline();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);
pulses=30-10;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
TurnRight_nDegree(90+45-20);
// findRline1();
// findRline1();
do
{
Robot_hunting1(1550); //??
}while(QTIS != 15);
pulses=15;
while(pulses--)
{
motor_motion2(1550, 1450); //??
} //??????
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryHpoint(void)
* Description???H????????????????????,????? CarryIpoint(void)
---------------------------------------------------------------------------------------------------------*/
void CarryHpoint(void)
{
Hhavesome=0;
do
{
motor_motion2(1540, 1460);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
if(distance > 4 && QTIS == 15){
have = false;
Hback(); //
process=3; //*****??******??***********??******??***********??*********************??************************8888
return;
}
}while(distance > 4); //
pulses=15;
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=50;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
motor_motion1(1450,1550,25);//??
TurnLeft_nDegree(65-9);
findLline2();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);
stop();
pulses=30-10;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
stop();
TurnLeft_nDegree(90+45-10-5-20-10);
// findRline1();
// findRline1();
stop();
do
{
Robot_hunting1(1550); //??
}while(QTIS != 15);
pulses=15;
while(pulses--)
{
motor_motion2(1550, 1450); //??
} //??????
void CarryCpoint(void)
{
Chavesome=0;
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 30;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//?????
pulses = 15;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
GotoLine();
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryDpoint(void)
* Description??D????????????? ,????????A?CarryApoint(void)
---------------------------------------------------------------------------------------------------------*/
void CarryDpoint(void) //??????A????????????????
{
Dhavesome=0;
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 35;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180();
delay_nms(200);
do
{
Robot_hunting1(1540);
}while(QTIS !=15);//?????
pulses = 18;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline();
GotoLine();
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryEpoint(void)
* Description??E????????????? ,????????A?CarryApoint(void)
---------------------------------------------------------------------------------------------------------*/
void CarryEpoint(void) //??????A????????????????
{
Ehavesome=0;
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 40;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//?????
pulses = 14;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline();
GotoLine();
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
/*
Ehavesome=0;
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 40;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//?????
pulses = 14;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline();
GotoLine();
pulses = 18;
while(pulses--)
{
motor_motion2(1550,1450);
}
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
*/
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryBpoint(void)
* Description??B????????????? ,????????A?CarryApoint(void)
---------------------------------------------------------------------------------------------------------*/
void CarryBpoint(void) //??????A????????????????
{
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 35;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180();
// stop();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//?????
pulses = 20;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
stop();
TurnLeftAnyPulse(10);
findRline();
GotoLine();
motor_motion2(1500, 1500);
delay_ms(50);
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryApoint(void)
* Description??A?????????????
---------------------------------------------------------------------------------------------------------*/
void CarryApoint(void)
{
Ahavesome=0;
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 35;
while(pulses--)
{
Robot_hunting1(1550); //??
}
Turn_180(); //??180?
do
{
Robot_hunting1(1550);
}while(QTIS !=15);
pulses = 15;
while(pulses--) //
{
motor_motion2(1550, 1450); //??
}
findRline(); //??????????
GotoLine();
findsekuai++;
if(findsekuai==5)
{
process=11;
Idone=1;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name? RedCarry(void)
* Description?????????????????
---------------------------------------------------------------------------------------------------------*/
void goBackCenter(){
pulses = 25;
while(pulses--)
{
Robot_hunting1(1540);
}//???????????
Turn_180();//??180?
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//???????????
pulses = LRp;
while(pulses--)
{
motor_motion2(1550, 1450);
} //??
}
void gotoG(){
void moveBack();
if(!have){
moveBack();
process = 13;
return ;
}
pulses = 22;
while(pulses--)
{
Robot_hunting1(1550);
}//???????????
TurnLeft_nDegree(35);//??n?
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=10;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline(); //??????????
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>4 );
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
TurnLeft_nDegree(110);//??n?
pulses=5;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(!QTIS);
pulses=14;
while(pulses--)
{
motor_motion2(1540, 1460); //??
}
findRline(); //??????????
process = 11;
}
void gotoH(){
void moveBack();
if(!have){
moveBack();
process = 13;
return ;
}
pulses = 22;
while(pulses--)
{
Robot_hunting1(1550);
}//???????????
TurnRight_nDegree(27);//??n?
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=10;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findRline(); //??????????
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>4 );
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
TurnRight_nDegree(79);//??n?
pulses=5;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(!QTIS);
pulses=13;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
findLline(); //??????????
process = 11;
}
/////////////////////////////////////////////////
void RedCarry(void)//?????????
{
hadedone++;
printf("hadedone:%d",hadedone);
do
{
Robot_hunting1(1550);
}while(QTIS);//?????????????
stop();
pulses = 35;
while(pulses--)
{
Robot_hunting1(1530); //??
}
stop();
/*pulses = goal-1;
while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
{
Robot_hunting1(1550); //??
} */
while( !(GPIO_ReadInputData(GPIOE)&0x08000))
{
delay_nms(50);
Robot_hunting1(1502); //??
}
stop();
motor_motion1(1450,1550,25);//??
stop();
Turn_180();//??180?
stop();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//???????
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
if(hadedone >= 6){
process=12;
printf("process:%d",process);
return;
}
if(hadedone >= 4)
{
TurnLeft_nDegree(180);
// do
// {
// Robot_hunting1(1550);
// }while(QTIS !=15);//?????
// pulses = 15; //?????????
// while(pulses--)
// {
// Robot_hunting2(1530);
// }
}
else
{
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>5 );
pulses =10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
delay_nms(50);
Turn_180();
process=11;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name? WhiteCarry(void)
* Description??????????????? ??????????RedCarry(void)
---------------------------------------------------------------------------------------------------------*/
void WhiteCarry(void)//?????????
{
hadedone++;
printf("hadedone:%d",hadedone);
TurnLeft_nDegree(10);
findLline(); //????????? ?????????
delay_nms(200);
do
{
Robot_hunting1(1550);
}while(QTIS); //?????????????0
pulses = 35;
while(pulses--)
{
Robot_hunting1(1530); //??
}
/*pulses = goal-1;
while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
{
Robot_hunting1(1550); //??
} */
while( !(GPIO_ReadInputData(GPIOE)&0x08000))
{
delay_nms(50);
Robot_hunting1(1502); //??
}
motor_motion1(1450,1550,25);//??
Turn_180(); //??180? ?????
do
{
Robot_hunting1(1550);
}while(QTIS !=15); //?????
pulses = 20; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// findRline();
delay_nms(200);
if(hadedone >= 6){
process=12;
printf("process:%d",process);
return;
}
// if(hadedone >= 5){
// //process=aim;
// //goBackCenter();
// //gotoG();
// gotoH();
// return;
// }
if(hadedone >= 4)
{
TurnLeft_nDegree(135);
// do
// {
// Robot_hunting1(1550);
// }while(QTIS !=15);//?????
// pulses = 15; //?????????
// while(pulses--)
// {
// Robot_hunting2(1530);
// }
}
else
{
findRline();
do{
Robot_hunting(1535);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>5);
pulses =10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
delay_nms(50);
Turn_180();
process=11;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?YellowCarry(void)
* Description??????????????? ??????????RedCarry(void)
---------------------------------------------------------------------------------------------------------*/
void YellowCarry(void)//?????????
{
hadedone++;
printf("hadedone:%d",hadedone);
// if(QTTS == )
TurnLeft_nDegree(8);//??n?
findLline(); //?????? ??
delay_nms(200);
findLline(); //????????????
motor_motion2(1500,1500);
delay_ms(50);
// while(1);
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 35;
while(pulses--)
{
Robot_hunting1(1530); //??
}
/*pulses = goal-1;
while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
{
Robot_hunting1(1550); //??
} */
while( !(GPIO_ReadInputData(GPIOE)&0x08000))
{
delay_nms(50);
Robot_hunting1(1502); //??
}
motor_motion1(1450,1550,25);//??
Turn_180();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);//?????
pulses = 15; //?????????
while(pulses--)
{
Robot_hunting2(1530);
}
// findRline();
TurnRight_nDegree(90);
delay_nms(200);
if(hadedone >= 6){
process=12;
printf("process:%d",process);
return;
}
if(hadedone >= 4)
{
Turn_180();
// do
// {
// Robot_hunting1(1550);
// }while(QTIS !=15);//?????
// pulses = 15; //?????????
// while(pulses--)
// {
// Robot_hunting2(1530);
// }
}
else
{
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>5);
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
delay_nms(50);
Turn_180();
process=11;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?BlackCarry(void)
* Description??????????????? ??????????RedCarry(void)
---------------------------------------------------------------------------------------------------------*/
void BlackCarry(void)//?????????
{
hadedone++;
printf("hadedone:%d",hadedone);
findRline();
delay_nms(200);
do
{
Robot_hunting1(1550);
}while(QTIS);
pulses = 40;
while(pulses--)
{
Robot_hunting1(1520); //??
}
/*pulses = goal-1;
while( (GPIO_ReadInputData(GPIOE)&0x08000) || pulses--)
{
Robot_hunting1(1550); //??
} */
while( !(GPIO_ReadInputData(GPIOE)&0x08000))
{
delay_nms(50);
Robot_hunting1(1502); //??
}
motor_motion1(1450,1550,25);//??
Turn_180();
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//?????
pulses = 18;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// findLline();
if(hadedone >= 6){
process=12;
printf("process:%d",process);
return;
}
// if(hadedone >= 5){
// //process=aim;
// //goBackCenter();
// //gotoG();
// gotoH();
// return;
// }
if(hadedone >= 4)
{
TurnRight_nDegree(135);
// do
// {
// Robot_hunting1(1550);
// }while(QTIS !=15);//?????
// pulses = 15; //?????????
// while(pulses--)
// {
// Robot_hunting2(1530);
// }
}
else
{
findLline();
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>5);
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
delay_nms(50);
Turn_180();
process=11;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?BlueCarry(void)
* Description??????????????? ??????????RedCarry(void)
---------------------------------------------------------------------------------------------------------*/
void BlueCarry(void)//?????????
{
hadedone++;
printf("hadedone:%d",hadedone);
// findRline();
// delay_nms(200);
// findRline();
// delay_nms(200);
TurnRight_nDegree(90);//??n?
stop();
do
{
Robot_hunting1(1550);
}while(QTIS);
while( !(GPIO_ReadInputData(GPIOE)&0x08000))
{
delay_nms(50);
Robot_hunting1(1502); //??
}
delay_nms(200);
pulses = 5;
while(pulses--)
{
delay_nms(20);
Robot_hunting1(1502); //??
}
// while( (GPIO_ReadInputData(GPIOE)&0x08000))
// {
// delay_nms(50);
// Robot_hunting1(1501); //??
// }
delay_nms(200);
motor_motion1(1450,1550,18);//??
Turn_180();
do
{
Robot_hunting1(1540);
}while(QTIS != 15);//?????
pulses = 14;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// findLline();
TurnLeft_nDegree(90);
delay_nms(200);
if(hadedone >= 6){
process=12;
return;
}
// if(hadedone >= 5){
// //process=aim;
// //goBackCenter();
// //gotoG();
// gotoH();
// return;
// }
//
if(hadedone >= 4)
{
TurnRight_nDegree(180);
// do
// {
// Robot_hunting1(1550);
// }while(QTIS !=15);//?????
// pulses = 15; //?????????
// while(pulses--)
// {
// Robot_hunting2(1530);
// }
}
else
{
do{
Robot_hunting(1540);
//delay_nms(50);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
}while(distance>3);
pulses = 10; //???? ??? ??
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
delay_nms(50);
Turn_180();
process=11;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?StartCarryToScore(void)
* Description??????????
---------------------------------------------------------------------------------------------------------*/
void StartCarryToScore(void)
{
count = 1; // ???1??
printf("hadedone: %d\n",hadedone);
if(hadedone >= 6){
process = 12;
return;
}
if(hadedone >= 4&&hadedone <6){
if(f_flag==1)
{
FCheck();
FCarry();
}
else if(g_flag==1)
{
GCheck();
GCarry();
}
else if(h_flag==1)
{
HCheck();
HCarry();
}
else if(i_flag==1)
{
ICheck();
ICarry();
}
}
else
{
delay_nms(200);
// printf("Color: %d\n",Robot_checkColor());
printf("Color: %d\n",xm_colourjudge(pcolor));
colorCheck = true;
while(colorCheck)
{
//color = Robot_checkColor();//????
color =xm_colourjudge(pcolor);
count++;
// if(color == 4)
//color = Robot_checkColor();//????
// color =xm_colourjudge(pcolor);
switch(color)//????
{
case Red:RedCarry();colorCheck = false;break;
case White:WhiteCarry();colorCheck = false;break;
case Yellow:YellowCarry();colorCheck = false;break;
case Black:BlackCarry();colorCheck = false;break;
case Blue:BlueCarry();colorCheck = false;break;
default:colorCheck = true;break; ////////////////////////////////////////////////////////////////////////////////
}
}
}
if(hadedone >= 6){
process = 12;
colorCheck = false;
}
}
/*---------------------------------------------------------------------------------------------------------
* Function name?CarryIpoint(void)
* Description??I??????????????
---------------------------------------------------------------------------------------------------------*/
void CarryIpoint(void)
{
Ihavesome=0;
do
{
motor_motion2(1540, 1460);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
if(distance > 4 && QTIS == 15){
have = false;
Hback(); //
process=3; //*****??******??***********??******??***********??*********************??************************8888
return;
}
}while(distance > 4); //
pulses=15;
do
{
motor_motion2(1550, 1450); //??
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);
pulses=35;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
motor_motion1(1450,1550,14);//??
TurnLeft_nDegree(65-9);
findLline2();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);
stop();
pulses=30-10;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
stop();
TurnLeft_nDegree(90+45-10-5-20-10);
// findRline1();
// findRline1();
stop();
do
{
Robot_hunting1(1550); //??
}while(QTIS != 15);
pulses=15;
while(pulses--)
{
motor_motion2(1550, 1450); //??
} //??????
process=11;
}
void CarryFpoint(void)
{
if(Fhavesome==1)
{
Fhavesome=0;
do
{
motor_motion2(1540, 1460);
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
distance = GetDis(GPIO_Pin_12,GPIO_Pin_13);
if(distance > 4 && QTIS == 15){
have = false;
Gback();
process=3; //*****??******??***********??******??***********??*********************??************************8888
return;
}
}while(distance > 4); //????F?????????
pulses=15;
do
{
motor_motion2(1550, 1450); //??
}while(!QTI_State(QTI4_Pin));
pulses=35;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
motor_motion1(1450,1550,14);//??
TurnRight_nDegree(65-3);
findRline();
do
{
Robot_hunting1(1550);
}while(QTIS !=15);
pulses=30-5;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
TurnRight_nDegree(90+45-10);
do
{
Robot_hunting1(1550); //??
}while(QTIS != 15);
pulses=15;
while(pulses--)
{
motor_motion2(1550, 1450);
process=11;
}
}
}
void GoStart(void)
{
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//???????????
pulses = 15;
while(pulses--) //?????
{
motor_motion2(1650, 1350); //??
}
do
{
Robot_hunting1(1550);
}while(QTIS != 15);//???????????
pulses = LRp-2;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
// gai wei
pulses = 2;
while(pulses--)
{
Robot_hunting1(1550);
}
}
void moveBack(){
printf("gohome");
pulses = 10;
while(pulses--)
{
Robot_hunting1(1560); //??
}
do
{
Robot_hunting1(1560);
QTIS = (GPIO_ReadInputData(GPIOE)&0x0f);
}while(QTIS != 15);//???????????
pulses = 40;
while(pulses--)
{
motor_motion2(1550, 1450); //??
}
process = 13;
}
void turn_back(){
Turn_180();
process = 11;
return;
}
//***************************************************************************************
void EXTIX_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOE,ENABLE);//??PORTA,PORTE??
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;//|GPIO_Pin_6|GPIO_Pin_7;//KEY0-KEY1
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //???????
GPIO_Init(GPIOE, &GPIO_InitStructure);//???GPIOE4,3
// //??? WK_UP-->GPIOA.0 ????
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //PA0??????????
// GPIO_Init(GPIOA, &GPIO_InitStructure);//???GPIOA.0
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); //????????
// //GPIOE.3 ???????????? ????? //KEY1
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource3);
// EXTI_InitStructure.EXTI_Line=EXTI_Line3;
// EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
// EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
// EXTI_Init(&EXTI_InitStructure); //??EXTI_InitStruct???????????EXTI???
// //GPIOE.4 ???????????? ????? //KEY0
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource4);
// EXTI_InitStructure.EXTI_Line=EXTI_Line4;
// EXTI_Init(&EXTI_InitStructure); //??EXTI_InitStruct???????????EXTI???
//GPIOE.5 ???????????? ????? //KEY0
GPIO_EXTILineConfig(GPIO_PortSourceGPIOE,GPIO_PinSource5);
EXTI_InitStructure.EXTI_Line=EXTI_Line5;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_Init(&EXTI_InitStructure); //??EXTI_InitStruct???????????EXTI???
NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn; //????KEY0?????????
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //?????2
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00; //????0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //????????
NVIC_Init(&NVIC_InitStructure); //??NVIC_InitStruct???????????NVIC???
}
//******************************************************************************************/
void EXTI9_5_IRQHandler(void)
{
if( EXTI_GetITStatus(EXTI_Line5) != RESET)
{
count_tcs230++;
// tim5_interruput_flag=0x01;
}
EXTI_ClearITPendingBit(EXTI_Line5);
}
//******************************************************************************************/
int main(void)
{
Peripheral_Init();//????????//
TIM5_Int_Init(0xfff0-1,720-1);
delay_init();
EXTIX_Init();
process =11;//4;//7;//5;//6; //?C/B/A/D/E/G/H??????????????????????????? ?????case11??
// process =8; //?C/B/A/D/E/G/H??????????????????????????? ?????case11??
led=1;
delay_nms(1000);
led=0;
delay_nms(1000);
xm_TCS230_WhiteBalance(pcolor); //???
led=0;
delay_nms(1000);
led=0;
delay_nms(1000);
delay_nms(1000);
// process = 8;
//GoStart(); //??????
// process = 11;
while(1)
{
//printf("process:%d\n",process);
// motor_motion2(1500, 1500);
//
// process = 9;
switch(process) //???process=1
{
/**************??1~10?????********************************/
case 1: CCheck();
CarryCpoint();
break;//?????c?
case 2: BCheck();
CarryBpoint();
// while(1);
break;//?????b?
case 3: ACheck();
CarryApoint();
break;//?????a?
case 4: ICheck();
CarryIpoint();
break;//?????i? //i??????????????
case 5: HCheck();
CarryHpoint();
break;//?????h?
case 6: GCheck();
CarryGpoint();
break;//?????g?
case 7: FCheck();
CarryFpoint();
break;//?????f?
case 8: ECheck();
CarryEpoint();
break;//?????e?
case 9: DCheck();
CarryDpoint();
break;//?????d?
//case 10:turn_back();break;
case 11:StartCarryToScore();
break;//?C/B/A/D/E/G/H?????????
case 12:moveBack();
break;
default:break;
}
//
// while(1);
}
}