#include "stdio.h"
#include "StackMacros.h"
#include "FreeRTOS.h"
#include "mpu_wrappers.h"
#include "stm32f10x_conf.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "misc.h"
#include "LiBo.h"
#include "stm32f10x.h"
extern vu32 Stamp,Stamp_old;
extern vu16 Left,Right;
extern vu16 ADC_ConvertedValue[256];
extern vu8 Debug_mes[33];
vu8 flag_edege=0;//定义到达边缘的标志
/******************** zhaozm制作********************
* 描述 : PC0接前方灰度传感器 PC1接后方灰度传感器
-----------------------------------------------------------------------------*/
void Libo_QTI_task(void )
{
u32 sum=0;
u16 i=0;
u16 ave_back=0,ave_front=0/*,front_old*/;
sum=0;
ave_back=0;
ave_front=0;
for(i=0;i<256;i+=2)
{
sum+=ADC_ConvertedValue[i];
}
ave_front=sum/128;
// printf(" 前方灰度传感器数值 %d \r\n",ave);
// printf("\r\n 数组0数值 %d \r\n",ADC_ConvertedValue[0]);
// printf("\r\n 数组1数值 %d \r\n",ADC_ConvertedValue[1]);
// printf("\r\n 数组2数值 %d \r\n",ADC_ConvertedValue[2]);
// printf("\r\n 数组3数值 %d \r\n",ADC_ConvertedValue[3]);
//
sum=0;
// printf(" 后方传感器数值 %d\r\n",ave);
Debug_mes[3]=ave_front&0xff;
Debug_mes[4]=ave_front>>8;
if(ave_front>3200)
{
Left=4500;
Right=4500;
TurnLeft(Left,Right);//
Stamp_old=Stamp;
while((Stamp-Stamp_old)<500)
{
}
Left=4500;
Right=4500;
Foward(Left,Right);//
//
// front_old=ave_front;
// for(i=0;i<256;i+=2)
// {
// sum+=ADC_ConvertedValue[i];
// }
// ave_front=sum/128;
// while(ave_front>front_old)
// {
// for(i=0;i<256;i+=2)
// {
// sum+=ADC_ConvertedValue[i];
// }
// ave_front=sum/128;
// sum=0;
// if(ave_front>front_old)
// {
//
// break;
// }
//
// }
// }
ave_front=0;
for(i=1;i<256;i+=2)
{
sum+=ADC_ConvertedValue[i];
}
ave_back=sum/128;
Debug_mes[5]=ave_back&0xff;
Debug_mes[6]=ave_back>>8;
Debug_mes[0]|=0x02;//表明灰度传感器的任务已经完成
NRF24L01_TxPacket(Debug_mes);
}
Left=4500;
Right=4500;
Foward(Left,Right);//
}
void Libo_Pong_task(void)
{
u16 Data=0;
Stamp_old=Stamp;
Data=GPIO_ReadInputData(GPIOE);
Debug_mes[1]=Data&0xff;
Debug_mes[2]=Data>>8;
Data&=0x00f0;
Data=~Data;
Data&=0x00f0;
if(Data!=0&&flag_edege==0)
{
Data=GPIO_ReadInputData(GPIOE);
Data&=0x00f0;
Data=~Data;
Data&=0xf0;
Data=GPIO_ReadInputData(GPIOE);
// if(Data==0)
// break;
Data=GPIO_ReadInputData(GPIOE);
Data&=0x00f0;
Data=~Data;
Data&=0xf0;
while(flag_edege==0)
{
Data=GPIO_ReadInputData(GPIOE);
if((Stamp-Stamp_old)>500)
{
Debug_mes[1]=Data&0xff;
Debug_mes[2]=Data>>8;
Debug_mes[11]=1;
NRF24L01_TxPacket(Debug_mes);//发送了调试信息噢噢`(*∩_∩*)′
Stamp_old=Stamp;
}
Data&=0x00f0;
Data=~Data;
Data&=0xf0;
switch(Data)
{
case 0x00:
Left=4200;
Right=4200;
Foward(Left,Right);//
break;
case 0x10:
Left-=60;
Right+=60;
Foward(Left,Right);//
break;
case 0x20:
Left-=30;
Right+=30;
Foward(Left,Right);//
break;
case 0x30:
Left-=10;
Right+=10;
Foward(Left,Right);//
break;
case 0x40:
Left+=30;
Right-=30;
Foward(Left,Right);//
break;
case 0x60:
Left=4200;
Right=4200;
Foward(Left,Right);//
break;
case 0x70:
Left=4200;
Right=4200;
Foward(Left,Right);//
break;
case 0x80:
Left+=60;
Right-=60;
Foward(Left,Right);//
break;
case 0xc0:
Left+=10;
Right-=10;
Foward(Left,Right);//
break;
case 0xe0:
Left=4200;
Right=4200;
Foward(Left,Right);//
break;
case 0xf0:
Left=4200;
Right=4200;
Foward(Left,Right);//
break;
}
}
flag_edege=0;
// default:
}
Left=3600;
Right=3600;
Foward(Left,Right);//
if((Stamp-Stamp_old)>500)
{
Debug_mes[11]=1;
NRF24L01_TxPacket(Debug_mes);//发送了调试信息噢噢`(*∩_∩*)′
Stamp_old=Stamp;
}
}
评论0