void laser_setup(void)
{
USART3_Config();
// USART3_NVIC_Configuration();
USART3_DMA_Config();
USART3_DMA_NVIC_Configuration(); //使能DMA IRQn通道
laser_set_initial_value();
}
///*
int main(void)
{
char keyVal;
static bool sw_adxl345 = true,sw_l3g4200d = true, sw_hmc5883l=true,sw_bmp085 =true;
SystemInit();
USART3_Config();
I2C_config();
L3G4200D_Init();
printf("Initial successed!\n\r");
while(1)
{
// L3G4200D_MultRead(&l3g4200d); //讀陀螺儀數據(速度:較快)
L3G4200D_Read(&l3g4200d);
L3G4200D_Printf(&l3g4200d);
Delayms(10);
}
}
int main(void)
{
u8 i=0,m=0;
//char buff[6];
SystemInit();//初始化RCC 设置系统主频为72MHZ
USART1_Config();
delay_init(); //延时初始化
Ultran_Init();
I2c_Init_MI();
TIM5_Cap_Init(0xfffe,8);
TIM2_Cap_Init(0xfffe,8);
TIM3_Cap_Init(0xfffe,8);
TIM1_Cap_Init(0xfffe,8);
TIM4_Cap_Init(0xfffe,8);
TIM8_Cap_Init(0xfffe,8);
USART1_Config();
USART2_Config();
USART3_Config();
delay_ms(1000);
delay_ms(1000);
esp8266_ap_server_init();
for(i=0;i<99;i++)//数组初始化
{
data_pc[i]=0x30;
}
data_pc[0]=0xff;
data_pc[1]=0xff;
while(1)
{
// char *buff[10]={"AT",
// "AT+CIPSTATUS"};
Ultra_Ranging();
if(state_0x0a==1)//字符串解析函数
{
u8 i=0,weizhi=0;
data_exist=bf( data_cu,ESP_Responses[5]);
if(data_exist!=-1)
{
data_exist1=bf( data_cu,ESP_Responses[10]);
if(data_exist1!=-1)
{
data_exist2=data_exist1-data_exist-7;
if(data_exist2==1)
{
data_length1=data_cu[data_exist+7];
data_length1=data_length1-0x30;
data_length=data_length1;
weizhi=9;
}
if(data_exist2==2)
{
data_length1=data_cu[data_exist+7];
data_length1=data_length1-0x30;
data_length1=data_length1*10;
data_length2=data_cu[data_exist+8];
data_length2=data_length2-0x30;
data_length=data_length1+data_length2;
weizhi=10;
}
for(i=0;i<data_length;i++)
{
data_wifi[i]=data_cu[data_exist+weizhi+i];
}
// USART_printf(USART1, data_right);//发送通过校验的数据到电脑上方便调试
// USART_printf(USART1, "\r\n");
}
}
cun=0;
for(i=0;i<100;i++)//清空接收数组
{
data_cu[i]=0;
}
state_0x0a=0;//接收状态变量清零
//data_exist=0;
}
if(state_0x0a1==1)//字符串解析函数
{
u8 i=0,weizhi=0;
data_exist3=bf( data_cu1,ESP_Responses[5]);
if(data_exist3!=-1)
{
data_exist4=bf( data_cu1,ESP_Responses[10]);
if(data_exist4!=-1)
{
data_exist5=data_exist4-data_exist3-7;
if(data_exist5==1)
{
data_length4=data_cu1[data_exist3+7];
data_length4=data_length4-0x30;
data_length3=data_length4;
weizhi=9;
}
if(data_exist5==2)
{
data_length4=data_cu1[data_exist3+7];
data_length4=data_length4-0x30;
data_length4=data_length4*10;
data_length5=data_cu1[data_exist3+8];
data_length5=data_length5-0x30;
data_length3=data_length4+data_length5;
weizhi=10;
}
for(i=0;i<data_length3;i++)
{
data_wifi1[i]=data_cu1[data_exist3+weizhi+i];
}
//USART_printf(USART1, data_right);//发送通过校验的数据到电脑上方便调试
//USART_printf(USART1, "\r\n");
}
}
cun1=0;
for(i=0;i<100;i++)//清空接收数组
{
data_cu1[i]=0;
}
state_0x0a1=0;//接收状态变量清零
//data_exist=0;
}
data_test=0;
for(data_long=0;data_long<9;data_long++)//将接收到的数据进行加和，准备进行校验
{
data_test+=data_wifi[data_long];
}
if(data_test==data_wifi[9])//校验通过就将数据转移并且保存
{
for(data_long=0;data_long<9;data_long++)
{
data_right[data_long]=data_wifi[data_long];
}
}
data_test=0;
for(data_long=0;data_long<9;data_long++)//将接收到的数据进行加和，准备进行校验
{
data_test+=data_wifi1[data_long];
}
if(data_test==data_wifi1[9])//校验通过就将数据转移并且保存
{
for(data_long=0;data_long<9;data_long++)
{
data_right1[data_long]=data_wifi1[data_long];
}
}
for(i=0;i<6;i++)//将转速数据放入data_pc数据包
{
sprintf(revolving,"%d",juli[i]);
for(m=0;m<5;m++)
{
if(revolving[m]==0)
revolving[m]=0xaa;
}
for(m=revolving_long;m>0;m--)
{
data_pc[i*revolving_long+m+1]=revolving[m-1];
}
for(m=0;m<revolving_long;m++) //将revolving在这里清零
{
revolving[m]=0;
}
}
for(i=0;i<8;i++)
{
data_pc[32+i+8*(data_right[8]-1)]=data_right[i];
}
for(i=0;i<8;i++)
{
data_pc[32+i+8*(data_right1[8]-1)]=data_right1[i];
}
crc_code = yb_crc(data_pc,80);//计算CRC校验码
USART_printf(USART1, data_pc);//发送通过校验的数据到电脑上方便调试
USART_printf(USART1, "\r\n");
//printf(data_right);
//printf("\r\n");
// printf("电机1=%d ",juli);
// printf("电机2=%d\r\n",juli1);
// printf("*********\r\n");
//USART_printf(USART1,buff[1] );
//delay_ms(100);
//esp8266_ap_server_init();
}
//printf("电机1=%d\s\n",juli);
//printf("电机2=%s\r\n",juli1);
}
int main(void)
{
int i=1;
USART3_Config();
SysTick_Init();
init_I2C1();
sensor_ayarla();
printf("l3g4200d\r\n");
while (1){
if((I2C_readreg(L3G4200D_ADDR,STATUS_REG)&0x08)==0x08)
{
/* read the rate from the register */
gyrox_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
gyrox_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
gyrox_temp=((gyrox_h_temp << 8) | gyrox_l_temp);
gyroy_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
gyroy_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
gyroy_temp=((gyroy_h_temp << 8) | gyroy_l_temp);
gyroz_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
gyroz_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
gyroz_temp=((gyroz_h_temp << 8) | gyroz_l_temp);
printf("gyrox_temp=%d gyroy_temp=%d gyroz_temp=%d\r\n",gyrox_temp,gyroy_temp,gyroz_temp);
/* compute the value of rate */
gyrox = gyrox_temp * gyro_sensitivity_250 /1000 ;
gyroy = gyroy_temp * gyro_sensitivity_250 /1000 ;
gyroz = gyroz_temp * gyro_sensitivity_250 /1000 ;
/* print the value of rate*/
gyrox_int = (int16_t)gyrox;
gyrox_decimal = (gyrox - gyrox_int)*1000;
if(gyrox_decimal < 0)
gyrox_decimal=-gyrox_decimal;
gyroy_int = (int16_t)gyroy;
gyroy_decimal = (gyroy - gyroy_int)*1000;
if(gyroy_decimal < 0)
gyroy_decimal=-gyroy_decimal;
gyroz_int = (int16_t)gyroz;
gyroz_decimal = (gyroz - gyroz_int)*1000;
if(gyroz_decimal < 0)
gyroz_decimal=-gyroz_decimal;
printf("gyrox_float=%d.%d\r\n",gyrox_int,gyrox_decimal);
printf("gyroy_float=%d.%d\r\n",gyroy_int,gyroy_decimal);
printf("gyroz_float=%d.%d\r\n",gyroz_int,gyroz_decimal);
/* 頃角計算 */
i=1;
for(i;i<=50;i++)
gyro_sample();
//gyrox_angle=gyrox_angle+gyrox*0.5;
//gyroy_angle=gyroy_angle+gyroy*0.5;
//gyroz_angle=gyroz_angle+gyroz*0.5;
/* print the value of angle */
gyrox_angle_int = (int16_t)gyrox_angle;
gyrox_angle_decimal = (gyrox_angle - gyrox_angle_int)*1000;
if(gyrox_angle_decimal < 0)
gyrox_angle_decimal=-gyrox_angle_decimal;
gyroy_angle_int = (int16_t)gyroy_angle;
gyroy_angle_decimal = (gyroy_angle - gyroy_angle_int)*1000;
if(gyroy_angle_decimal < 0)
gyroy_angle_decimal=-gyroy_angle_decimal;
gyroz_angle_int = (int16_t)gyroz_angle;
gyroz_angle_decimal = (gyroz_angle - gyroz_angle_int)*1000;
if(gyroz_angle_decimal < 0)
gyroz_angle_decimal=-gyroz_angle_decimal;
printf("gyrox_angle_float=%d.%d\r\n",gyrox_angle_int,gyrox_angle_decimal);
printf("gyroy_angle_float=%d.%d\r\n",gyroy_angle_int,gyroy_angle_decimal);
printf("gyroz_angle_float=%d.%d\r\n",gyroz_angle_int,gyroz_angle_decimal);
printf("\r\n-------------------------------------------------------\r\n");
Delay(500);
}
}
}
void Init(void)
{
/* 串口3初始化 */
USART3_Config();
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
USART3_Config();
uint8_t ctrl = 0;
LIS302DL_InitTypeDef LIS302DL_InitStruct;
LIS302DL_InterruptConfigTypeDef LIS302DL_InterruptStruct;
/* SysTick end of count event each 10ms */
SysTick_Config(SystemCoreClock/ 100);
/* Set configuration of LIS302DL*/
LIS302DL_InitStruct.Power_Mode = LIS302DL_LOWPOWERMODE_ACTIVE;
LIS302DL_InitStruct.Output_DataRate = LIS302DL_DATARATE_100;
LIS302DL_InitStruct.Axes_Enable = LIS302DL_X_ENABLE | LIS302DL_Y_ENABLE | LIS302DL_Z_ENABLE;
LIS302DL_InitStruct.Full_Scale = LIS302DL_FULLSCALE_2_3;
LIS302DL_InitStruct.Self_Test = LIS302DL_SELFTEST_NORMAL;
LIS302DL_Init(&LIS302DL_InitStruct);
/* Set configuration of Internal High Pass Filter of LIS302DL*/
LIS302DL_InterruptStruct.Latch_Request = LIS302DL_INTERRUPTREQUEST_LATCHED;
LIS302DL_InterruptStruct.SingleClick_Axes = LIS302DL_CLICKINTERRUPT_Z_ENABLE;
LIS302DL_InterruptStruct.DoubleClick_Axes = LIS302DL_DOUBLECLICKINTERRUPT_Z_ENABLE;
LIS302DL_InterruptConfig(&LIS302DL_InterruptStruct);
/* Required delay for the MEMS Accelerometre: Turn-on time = 3/Output data Rate
= 3/100 = 30ms */
Delay(30);
/* Configure Interrupt control register: enable Click interrupt1 */
ctrl = 0x07;
LIS302DL_Write(&ctrl, LIS302DL_CTRL_REG3_ADDR, 1);
/* Enable Interrupt generation on click/double click on Z axis */
ctrl = 0x70;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_CFG_REG_ADDR, 1);
/* Configure Click Threshold on X/Y axis (10 x 0.5g) */
ctrl = 0xAA;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_THSY_X_REG_ADDR, 1);
/* Configure Click Threshold on Z axis (10 x 0.5g) */
ctrl = 0x0A;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_THSZ_REG_ADDR, 1);
/* Configure Time Limit */
ctrl = 0x03;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_TIMELIMIT_REG_ADDR, 1);
/* Configure Latency */
ctrl = 0x7F;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_LATENCY_REG_ADDR, 1);
/* Configure Click Window */
ctrl = 0x7F;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_WINDOW_REG_ADDR, 1);
/* TIM configuration -------------------------------------------------------*/
TIM_Config();
LIS302DL_Read(Buffer, LIS302DL_OUT_X_ADDR, 6);
XOffset = (int8_t)Buffer[0];
YOffset = (int8_t)Buffer[2];
ZOffset = (int8_t)Buffer[4];
while(1)
{
a=(int8_t)(Buffer[0]);
if(a<0){
a=-a;
printf("\r\n X = -%d \r\n", a);
}
else{
printf("\r\n X = %d \r\n", a);
}
//printf("\r\n Y =%d \r\n", (int8_t)Buffer[2]);
//printf("\r\n Z =%d \r\n", (int8_t)Buffer[4]);
/*
temp=XOffset/sqrt((YOffset*YOffset+ZOffset*ZOffset));
angle_x=atan(temp);
angle_x=((angle_x*180)/3.14);
temp1=(int16_t)angle_x;
temp2=(angle_x-temp1)*1000;
printf("\r\n Angle_X=%d.%d \r\n", temp1,temp2);//use sqrt & atan
//原子教你玩的方法*/
/*
angle_ax=(XOffset-1100)/64;
angle_ax=(angle_ax*1.2*180/3.14);
temp1=(int16_t)angle_ax;
temp2=(angle_ax-temp1)*1000;
if(temp1<0){
a=-temp1;
b=-temp2;
printf("\r\n Angle_ax=%d.%d \r\n",a,b);
}
//網路上的方法*/
b=(float)((int8_t)Buffer[0]-XOffset)*(int32_t)LIS302DL_SENSITIVITY_2_3G/1000*180/3.14;
if(b<0){
x_acc=-b;
temp1=(int8_t)x_acc;
temp2=(x_acc-temp1)*10000;
if(temp2<0){temp2=-temp2;}
printf("\r\n X_Acc= -%d.%d \r\n", temp1,temp2);
}
else{
x_acc=b;
temp1=(int8_t)x_acc;
temp2=(x_acc-temp1)*10000;
if(temp2<0){temp2=-temp2;}
printf("\r\n X_Acc= %d.%d \r\n", temp1,temp2);
}
//openbox
Delay(100);
}
}
/*
* 函数名：main
* 描述 ：主函数
* 输入 ：无
* 输出 ：无
*/
int main(void)
{
uint32_t i, u32RxLen;
u16 led0pwmval1=59999;
u16 led0pwmval2=59999;
u16 led0pwmval3=59999;
u16 led0pwmval4=59999;
u16 adcx;
float temp;
double x1,y1,x2,y2,fai;
u8 LAT1,LNG1;//定义两个指针指向存放经纬度的内存地址
u8 processok=0;//所有数据每20ms处理一次
double lat,lng;
double speed2;
// double seitatest=0;
static u8 heartbeat=0;
const u8 at[] = {"receiveok"};
u8 reply[6]={0x01,0x04,0x01};
u8 gpsdata[100]={0};//存放待处理的GPS数组
u8 reply1[3];//准备存放回应的数据
u8 reply2[3];//存放左右倾角，滚转角
u8 reply3[3];//存放前后倾角，俯仰角
u8 package1[10];//存放纬度
u8 package2[10];//存放经度
u8 package3[10];//存放方位角
u8 package4[10];//存放滚转角
u8 package5[10];//存放前后倾角
u8 package6[4];//存放速度
u8 package7[4];//存放剩余电量
uint8_t source=0x01;//源地址
uint8_t destination=0x02;//目的地址
tsPacketOfAppInfo truesendout;//给这个类型定义一个名字
uint8_t len=61;
// u8 gpgga[]={"$GPGGA,061021.05,3102.02726033,N,12126.86794498,E,2,12,0.9,25.022,M,8.127,M,1.0,0000*46\r\n"};
uint8_t sendbuff[]="receiveok";
u32RxLen=sizeof(at);
ptuoqiu->latitude=&latitude3;
ptuoqiu->longitude=&longitude3;
gaosi->gaosix =&gaosix3;
gaosi->gaosiy =&gaosiy3;
gaosi->gaosix1 =&gaosix4;
gaosi->gaosiy1 =&gaosiy4;
DK_NVIC_Init(4);
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);//设置systick的时钟是AHB不分频的
SysTick_Config(7200000);//设置两次中断的时间间隔为100ms
/* USART1,2,3 ,UART4 config 115200 8-N-1 */
USART1_Config();
USART2_Config();
USART3_Config();
UART4_Config();
Usart_DMA_Config();
LED_GPIO_Config();
delay_ms(1000);
PWM_Initch1(60000,0); //不分频。PWM频率=72000000/60000=1.2Khz
PWM_Initch2(60000,0);
PWM_Initch3(60000,0);
PWM_Initch4(60000,0);
TIM3->CR1|=0x01; //使能定时器3
LED0_PWM_VAL1=led0pwmval1;
LED0_PWM_VAL2=led0pwmval2;
LED0_PWM_VAL3=led0pwmval3;
LED0_PWM_VAL4=led0pwmval4;
//这个是测试用的数据
/*填充将要发送的数据*/
// for (i = 0; i < sizeof(at); i++)
// {
// SendBuff[i] = at[i];
// }
// USART_SEND(SendBuff, sizeof(at)-1);
// delay_ms(1000);
/*填充将要发送的数据*/
// for (i = 0; i < SENDBUFF_SIZE; i++)
// {
// SendBuff[i] = 0x31;
// }
// USART_SEND(SendBuff, 30);
// //测试数据结束
// /*串口向 DMA发出请求 */
// USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
/*在DMA尚未传送完成时，cpu继续执行main函数中的代码*/
/*点亮了LED灯*/
/*而同时DMA在向串口运送数据，当DMA发送完成时，在中断函数关闭LED灯 */
Adc_Init();//ADC转换是分频为12MHz
//order_sendinit();
while(1)
{
// gpggaresolve(gpgga);
//Usart1_Send(command1, sizeof(comd1));
//delay_ms(500);
// static u8 pathen=0;
if (sUsart2_entity.u8RxFlag == 1)
{//如果收到罗盘返回的数据
sUsart2_entity.u8RxFlag = 0;
u32RxLen = sUsart2_entity.u16RxTotalNum;
sUsart2_entity.u16RxTotalNum = 0;
processok=1;//50ms到，可以开始处理数据
if(sUsart2_entity.u8RX_Buffer[1]==0x0d)//如果传回的数据是角度
{
for(i=0;i<3;i++)
{
reply1[i]=sUsart2_entity.u8RX_Buffer[10+i];//取出方向角
reply2[i]=sUsart2_entity.u8RX_Buffer[4+i];//取出左右倾角，滚转角
reply3[i]=sUsart2_entity.u8RX_Buffer[7+i];//取出前后倾角，俯仰角
}
}
memset(sUsart2_entity.u8RX_Buffer,0,15);
}
if(sUsart3_entity.u8RxFlag==1)
{//如果收到GPS传回的数据
sUsart3_entity.u8RxFlag = 0;
u32RxLen = sUsart3_entity.u16RxTotalNum;
sUsart3_entity.u16RxTotalNum = 0;
// processok=1;//20ms到，可以开始处理数据
for(i=0;i<sizeof(sUsart3_entity.u8RX_Buffer);i++)
{
sUsart3_entity.u8RX_Buffer[i]=gpsdata[i];
}
memset(sUsart3_entity.u8RX_Buffer,0,150);
// gpggaresolve(sUsart3_entity.u8RX_Buffer);
}
if (sUart4_entity.u8RxFlag == 1)
{//如果收到指令
sUart4_entity.u8RxFlag = 0;
u32RxLen = sUart4_entity.u16RxTotalNum;
sUart4_entity.u16RxTotalNum = 0;
}
if(speeden==1)//如果2秒到了的话，测速一次，检测剩余电量一次
{
speeden=0;
heartbeat++;
memset(package6,0,4);//速度和电量的数组先清掉再赋新的值
memset(package7,0,4);
speed2=getspeed((*(gaosi->gaosix1)),(*(gaosi->gaosiy1)));//1秒测速一次
// speed2=10.6;
// seita=getheading(0.5,-1,1,-2);//检测方位角函数是否正常
// gettoorbit(200,330);//检测大调整函数
// changedirections(1,4,0.5,-1,1,-2);//检测微调是否正常
if(heartbeat==7)//每隔14s检测一次心跳是否正常
{
heartbeat=0;
hearttest();//检测心跳是否正常函数
}
// (*(gaosi->gaosix1))+=0.05;
// (*(gaosi->gaosiy1))+=0.05;//测试速度
// if((*(gaosi->gaosix1))>=5)
// *(gaosi->gaosix1)=0;
// if((*(gaosi->gaosiy1))>=5)
// *(gaosi->gaosiy1)=0;
// GaussProjCal(*(ptuoqiu->longitude),*(ptuoqiu->latitude),gaosi->gaosix,gaosi->gaosiy);
// speed2=getspeed((*(gaosi->gaosix1)),(*(gaosi->gaosiy1)));//1秒测速一次
sprintf(package6,"%4.1f",speed2);
for(i=50;i<=53;i++)
{
package[i]=package6[i-50];//速度数组赋值，准备打包
}
temp=powadc();//1秒转换电压一次
sprintf(package7,"%4.1lf",temp);
for (i=54;i<=57;i++)
{
package[i]=package7[i-54];//将电压值赋值到数组的后面
}
}
if(processok==1)//如果50ms到了的话，那么所有数据进行处理
{
memset(package1,0,10);//数据域经纬度，方位角，前后倾角，左右滚转角数组清零
memset(package2,0,10);
memset(package3,0,10);
memset(package4,0,10);
memset(package5,0,10);
// memset(package6,0,4);//速度和电量的数组先清掉再赋新的值
// memset(package7,0,4);
processok=0;//清零50ms标记
head2=hextodex(reply1);//将角度转换为十进制
roll2=rolltodec(reply2);
pitch2=pitchtodec(reply3);
gpggaresolve(sUsart3_entity.u8RX_Buffer,&LAT1,&LNG1);//处理GPS的数据
lat=longtodouble(ptuoqiu->LAT);
lng=longtodouble(ptuoqiu->LNG);
changeformat(lng,lat,ptuoqiu->latitude,ptuoqiu->longitude);//将GPS数据解析成经纬度
order();//判断串口4是否接收到什么指令了
if((pathok==1)&&(heartok==1))//如果允许路径转换的话
{
move();//路径转换,大约耗时4ms左右
}
sprintf(package1,"%10.6lf",*(ptuoqiu->latitude));
sprintf(package2,"%10.6lf",*(ptuoqiu->longitude));
sprintf(package3,"%10.6lf",head2);
sprintf(package4,"%10.6lf",roll2);
sprintf(package5,"%10.6lf",pitch2);
// GaussProjCal(*(ptuoqiu->longitude),*(ptuoqiu->latitude),gaosi->gaosix,gaosi->gaosiy);//将小车当前的坐标转换为平面坐标
for(i=0;i<=9;i++)//纬度赋值到数据域
{
package[i]=package1[i];
}
for(i=10;i<=19;i++)//经度赋值到数据域
{
package[i]=package2[i-10];
}
for(i=20;i<=29;i++)//将方位角赋值到数据域中
{
package[i]=package3[i-20];
}
for(i=30;i<=39;i++)//将左右倾角赋值到数据域中
{
package[i]=package4[i-30];
}
for(i=40;i<=49;i++)//将前后倾角赋值到数据域中
{
package[i]=package5[i-40];
}
memset(sUart4_entity.u8RX_Buffer,0,150);
count=SendData_thisprotocol(truesendout,source,destination,len);//所有要打包的数据准备到一个数组里
//返回打包后的数据个数,函数很好，但是用于VB非常麻烦，太麻烦了简直，所以不用了,解析的时候实在是崩溃了
head2=0;
roll2=0;
pitch2=0;
memset(reply1,0,10);//三个角度数组都清零
memset(reply2,0,10);
memset(reply3,0,10);
// memset(package1,0,10);
// memset(package2,0,10);
// memset(package3,0,10);
// memset(package4,0,10);
// memset(package5,0,10);
// memset(package6,0,4);
// memset(package7,0,4);
}
}
}
int main()
{
USART3_Config();
USART2_Config();
Encoder_Init();
//while(1);
Motor_Init();
SysTick_Init(72);
Car_Run_Speed(0);
while(!Start_Due)
{
Car_Turn_Speed(2000); //Z向输出标定
}
Car_Turn_Angle(0);
while(1)
{
//USART1_printf(USART2,"angle=%d cnt=%d\r\n", (int)Car_Angle,TIM2->CNT);
//USART_SendData(USART2,USART_ReceiveData(USART2));
cmd_speed_pre = speed;
cmd_angle_pre = turn;
sscanf(Cmd_Ble, "#%d-%d*", &speed, &turn);
if (turn > 180)
{
turn = turn % 180;
}
else if (turn <= -180)
{
turn = turn % -180;
}
#ifdef BOTTOMIMPROVE
// //转向优化
// if (is_close(Car_Angle,turn))
// turn = turn;
// else
// turn = turn + 180;
// speed = -speed;
// if (turn > 180)
// {
// turn = turn % 180;
// }
// else if (turn <= -180)
// {
// turn = -((-turn) % 180);
// }
// Car_Turn_Angle(turn);
// Car_Run(speed);
if (abs(speed) > 0) //速度为零时不要优化，便于调零
{
src_1 = Car_Angle;
if (Car_Angle > 0)
{
src_2 = -180 + Car_Angle;
}
else
{
src_2 = 180 + Car_Angle;
}
//calc the angle distance
diff_4 = abs(turn - src_1); //normal
diff_5 = abs(turn - src_2); //reverse
if (diff_4 > 180)
{
diff_4 = 360 - diff_4;
}
if (diff_5 > 180)
{
diff_5 = 360 - diff_5;
}
if (diff_5 >= diff_4)
{
//normal turn
}
else
{
if (turn >=0)
{
turn = -180 + turn;
}
else
{
turn = 180 + turn;
}
speed = -speed; //reverse turn
}
}
#endif
//解决170 -> -170反转的问题，原理：以170->-170为例，未过180前，turn为190，但是对于calcpid而言，仍然能够正常输出。过了180后，
//turn就变回-170了，从而恢复正常。
diff_1 = abs(turn - Car_Angle);
diff_2 = abs(360 + turn - Car_Angle);
diff_3 = abs(turn - 360 - Car_Angle);
if (diff_2 < diff_1) //+360较小,即170 -> -170
{
turn = 360 + turn;
}
else if (diff_3 < diff_1) //-360较小,即-170 -> 170
{
turn = turn - 360;
}
else
{
turn = turn;
}
//#ifndef BOTTOMIMPROVE
if(abs(turn - cmd_angle_pre) != 0) //cmd_angle_pre 在读取新值之前被赋值，接下来被赋值的turn又会被更改，因此没有问题
{
Car_Turn_Angle(turn);
}
if(abs(speed - cmd_speed_pre) != 0)
{
Car_Run(speed);
}
stop_protect_val = (130 - abs(speed)) > 60?60:(130-abs(speed));
//Car_Turn(1);
//#endif
}
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
USART3_Config(); //USART3 init
uint8_t ctrl = 0;
LIS302DL_InitTypeDef LIS302DL_InitStruct;
LIS302DL_InterruptConfigTypeDef LIS302DL_InterruptStruct;
/* SysTick end of count event each 10ms */
SysTick_Config(SystemCoreClock/ 100);
/* Set configuration of LIS302DL*/
LIS302DL_InitStruct.Power_Mode = LIS302DL_LOWPOWERMODE_ACTIVE;
LIS302DL_InitStruct.Output_DataRate = LIS302DL_DATARATE_100;
LIS302DL_InitStruct.Axes_Enable = LIS302DL_X_ENABLE | LIS302DL_Y_ENABLE | LIS302DL_Z_ENABLE;
LIS302DL_InitStruct.Full_Scale = LIS302DL_FULLSCALE_2_3;
LIS302DL_InitStruct.Self_Test = LIS302DL_SELFTEST_NORMAL;
LIS302DL_Init(&LIS302DL_InitStruct);
/* Set configuration of Internal High Pass Filter of LIS302DL*/
LIS302DL_InterruptStruct.Latch_Request = LIS302DL_INTERRUPTREQUEST_LATCHED;
LIS302DL_InterruptStruct.SingleClick_Axes = LIS302DL_CLICKINTERRUPT_Z_ENABLE;
LIS302DL_InterruptStruct.DoubleClick_Axes = LIS302DL_DOUBLECLICKINTERRUPT_Z_ENABLE;
LIS302DL_InterruptConfig(&LIS302DL_InterruptStruct);
/* Required delay for the MEMS Accelerometre: Turn-on time = 3/Output data Rate
= 3/100 = 30ms */
Delay(30);
/* Configure Interrupt control register: enable Click interrupt1 */
ctrl = 0x07;
LIS302DL_Write(&ctrl, LIS302DL_CTRL_REG3_ADDR, 1);
/* Enable Interrupt generation on click/double click on Z axis */
ctrl = 0x70;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_CFG_REG_ADDR, 1);
/* Configure Click Threshold on X/Y axis (10 x 0.5g) */
ctrl = 0xAA;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_THSY_X_REG_ADDR, 1);
/* Configure Click Threshold on Z axis (10 x 0.5g) */
ctrl = 0x0A;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_THSZ_REG_ADDR, 1);
/* Configure Time Limit */
ctrl = 0x03;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_TIMELIMIT_REG_ADDR, 1);
/* Configure Latency */
ctrl = 0x7F;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_LATENCY_REG_ADDR, 1);
/* Configure Click Window */
ctrl = 0x7F;
LIS302DL_Write(&ctrl, LIS302DL_CLICK_WINDOW_REG_ADDR, 1);
/* TIM configuration -------------------------------------------------------*/
TIM_Config();
LIS302DL_Read(Buffer, LIS302DL_OUT_X_ADDR, 6);
XOffset = (int8_t)Buffer[0];
YOffset = (int8_t)Buffer[2];
ZOffset = (int8_t)Buffer[4];
while(1)
{
b=(float)((int8_t)Buffer[0]-XOffset)*(int32_t)LIS302DL_SENSITIVITY_2_3G/1000*180/3.14; //把X軸裡面的值轉換成角度
if(b<0){
x_acc=-b;
temp1=(int8_t)x_acc;
temp2=(x_acc-temp1)*10000; //把小數點部分乘上10000，以便程式印出(usart3的printf功能無法印出float)
if(temp2<0){temp2=-temp2;}
printf("\r\n X_Acc= -%d.%d \r\n", temp1,temp2);
}
else{
x_acc=b;
temp1=(int8_t)x_acc;
temp2=(x_acc-temp1)*10000;
if(temp2<0){temp2=-temp2;}
printf("\r\n X_Acc= %d.%d \r\n", temp1,temp2);
}
Delay(100);
}
}
/*==================================================================
* Function : init_usart
* Description :
* Inputpara :
* Outputpara :
==================================================================*/
void init_usart(void)
{
USART1_Config() ;
USART3_Config() ;
}
