循环中超时的strlen

#include "upstandingcar.h" #include "I2C_MPU6050.h" #include "MOTOR.h" #include "led.h" #include "USART.H" #include "MPU6050.H" #include "UltrasonicWave.h" #include "stm32f10x_gpio.h" #include "math.h" #include <string.h> #include <stdlib.h> #include <stdio.h> #include "adc.h" #include "delay.h" /*****************************************/ u8 newLineReceived = 0; u8 inputString[80] = {0}; u8 ProtocolString[80] = {0}; /*小车运行状态枚举*/ enum{ enSTOP = 0, enRUN, enLOW, enMEDIUM, enHIGH, enBACK, enLEFT, enRIGHT, enTLEFT, enTRIGHT, enLED, enLED1, enBEEP, enBEEP1 }enCarState; #define run_car '1'//按键前 #define back_car '2'//按键后 #define left_car '3'//按键左 #define right_car '4'//按键右 #define stop_car '0'//按键停 #define low_car '5'//按键前 #define medium_car '6'//按键前 #define high_car '7'//按键前 #define led_car 'a'//按键亮 #define led1_car 'b'//按键灭 #define beep_car 'c'//按键响 #define beep1_car 'd'//按键不响 int g_newcarstate = enSTOP; // 1前2后3左4右0停止 char returntemp[] = "$0,0,0,0,0,0,0,0,0,0,0,0cm,8.2V#"; char piddisplay[50] ="$AP"; char manydisplay[80] ={0}; char updata[80] ={0}; /*****************多数据************************/ u8 BST_u8MainEventCount; //主循环判断计数 在SysTick_Handler(void)中使用 每1ms加1 u8 BST_u8SpeedControlCount; //速度控制循环计数 在SysTick_Handler(void)中使用 每5ms加1 u8 BST_u8SpeedControlPeriod; u8 BST_u8DirectionControlPeriod; u8 BST_u8DirectionControlCount; //转向控制循环计数 在SysTick_Handler(void)中使用 每5ms加1 u8 BST_u8trig; u8 ucBluetoothValue; //蓝牙接收数据 float volt = 12.0; /******电机控制参数******/ float BST_fSpeedControlOut; //速度控制PWM float BST_fSpeedControlOutOld; float BST_fSpeedControlOutNew; float BST_fAngleControlOut; float BST_fLeftMotorOut; float BST_fRightMotorOut; float BST_fCarAngle; //角度控制PWM float gyro_z; float gyrx; float gy0; /*-----角度环和速度环PID控制参数-----*///以下参考为重点调试参考，同电池电压有关，建议充好电再调试 float BST_fCarAngle_P =90.2 ;// 91.3 //调大小时会左右摆，调大时会振动 请调到基本能够站立 P=91.3是用于给小车在运动过程使用 float BST_fCarAngle_D =0.21; // 0.001 0.002 0.004 0.008 0.0010 0.011 调小时反应慢，调大时会干扰 float BST_fCarSpeed_P= 10.15; float BST_fCarSpeed_I=0.31; const double PID_Original[4] ={0, 0, 0, 0}; char alldata[80]; char *iap; /******速度控制参数******/ s16 BST_s16LeftMotorPulse; //左电机脉冲数 s16 BST_s16RightMotorPulse; //右电机脉冲数 s32 BST_s32LeftMotorPulseOld; s32 BST_s32RightMotorPulseOld; s32 BST_s32LeftMotorPulseSigma; //50ms左电机叠加值 s32 BST_s32RightMotorPulseSigma; //50ms右电机叠加值 float BST_fCarSpeed; //测速码盘得出的车速 float BST_fCarSpeedOld; float BST_fCarPosition; //测速码盘通过计算得到的小车位移 /*-----悬停参数-----*/ int leftstop=0; int rightstop=0; int stopflag=0; /******超声波********/ float fchaoshengbo = 0; //超声波输出量 float juli = 0; //超声波距离 /******蓝牙控制参数******/ float BST_fBluetoothSpeed; //蓝牙控制车速 float BST_fBluetoothDirectionNew; //用于平缓输出车速使用 float BST_fBluetoothDirectionSL; //左转标志位 由于PWM转向输出使用判断输出方式 固需要标志位 float BST_fBluetoothDirectionSR; //右转标志位 由于PWM转向输出使用判断输出方式 固需要标志位 int chaoflag=0; //static int speedLevel = 0; int x,y1,z1,y2,z2,flagbt; int g_autoup = 0; int g_uptimes = 5000; //自动上报间隔 char charkp[10],charkd[10],charksp[10],charksi[10]; char lspeed[10],rspeed[10],daccel[10],dgyro[10],csb[10],vi[10]; char kp,kd,ksp,ksi; float dac = 0,dgy = 0; /************旋转*****************/ float BST_fBluetoothDirectionL; //左旋转标志位 由于PWM旋转输出使用判断输出方式 固需要标志位 float BST_fBluetoothDirectionR; //右旋转标志位 由于PWM旋转输出使用判断输出方式 固需要标志位 int driectionxco=400; //******卡尔曼参数************ float Q_angle=0.001; float Q_gyro=0.003; float R_angle=0.5; float dt=0.005; //dt为kalman滤波器采样时间; char C_0 = 1; float Q_bias, Angle_err; float PCt_0=0, PCt_1=0, E=0; float K_0=0, K_1=0, t_0=0, t_1=0; float Pdot[4] ={0,0,0,0}; float PP[2][2] = { { 1, 0 },{ 0, 1 } }; void SendAutoUp(void); /**********延时子函数****************/ void delay_nms(u16 time) { u16 i=0; while(time--) { i=12000; //自己定义 while(i--) ; } } /***********************************/ /*************************************************************** ** 函数名称: CarUpstandInit ** 功能描述: 全局变量初始化函数 ***************************************************************/ void CarUpstandInit(void) { BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0; //左右脉冲值 初始化 BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0; //叠加脉冲数 初始化 BST_fCarSpeed = BST_fCarSpeedOld = 0; //平衡小车车速 初始化 BST_fCarPosition = 0; //平衡小车位移量 初始化 BST_fCarAngle = 0; //平衡小车车速 初始化 BST_fAngleControlOut = BST_fSpeedControlOut = BST_fBluetoothDirectionNew = 0; //角度PWM、车速PWM、蓝牙控制PWM 初始化 BST_fLeftMotorOut = BST_fRightMotorOut = 0; //左右车轮PWM输出值 初始化 BST_fBluetoothSpeed = 0; //蓝牙控制车速值 初始化 BST_fBluetoothDirectionL =BST_fBluetoothDirectionR= 0; //蓝牙控制左右旋转标志位 初始化 BST_fBluetoothDirectionSL =BST_fBluetoothDirectionSR= 0; //蓝牙控制左右转向标志位 初始化 BST_u8MainEventCount=0; //用于5ms定时器子程序SysTick_Handler(void)中总中断计数位 BST_u8SpeedControlCount=0; //用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位 BST_u8SpeedControlPeriod=0; //用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位 fchaoshengbo=0;//用于5ms定时器子程序SysTick_Handler(void)中超声波平衡融入计数位 } void ResetPID() { if(BST_fCarAngle_P != PID_Original[0]) { BST_fCarAngle_P = PID_Original[0]; } if(BST_fCarAngle_D != PID_Original[1]) { BST_fCarAngle_D = PID_Original[1]; } if(BST_fCarSpeed_P != PID_Original[2]) { BST_fCarSpeed_P = PID_Original[2]; } if(BST_fCarSpeed_I != PID_Original[3]) { BST_fCarSpeed_I = PID_Original[3]; } } /*************************************************************** ** 函数名称: AngleControl ** 功能描述: 角度环控制函数 ***************************************************************/ void AngleControl(void) { if(flagbt==1) { BST_fCarAngle_P=0; BST_fCarAngle_P=y1*1.71875; } if(flagbt==2) { BST_fCarAngle_D=0; BST_fCarAngle_D=(z1-64)*0.15625; } dac=accel[2]; dgy=gyro[2]; if(Pitch==0||Pitch<-50||Pitch>50) //MPU6050状态指示灯 STM32核心板 PB13 绿色灯亮起为不正常 { //Pitch=1; GPIO_ResetBits(GPIOB, GPIO_Pin_13); //MPU6050状态指示灯 STM32核心板 PB13 绿色灯亮起为不正常 } else {GPIO_SetBits(GPIOB, GPIO_Pin_13);} //MPU6050状态正常时LED灯熄灭 BST_fCarAngle = Roll - CAR_ZERO_ANGLE; //DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度 BST_fAngleControlOut = BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ; //角度PD控制 } /*************************************************************** ** 函数名称: SetMotorVoltageAndDirection ** 功能描述: 电机转速 ***************************************************************/ void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage) { u16 u16LeftMotorValue; u16 u16RightMotorValue; //当左电机PWM输出为负时 PB14设为正 PB15设为负 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转） if(s16LeftVoltage<0) { GPIO_SetBits(GPIOA, GPIO_Pin_4 ); GPIO_ResetBits(GPIOA, GPIO_Pin_5 ); s16LeftVoltage = (-s16LeftVoltage); } //当左电机PWM输出为正时 PB14设为负 PB15设为正 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转） else { GPIO_SetBits(GPIOA, GPIO_Pin_5 ); GPIO_ResetBits(GPIOA, GPIO_Pin_4 ); s16LeftVoltage = s16LeftVoltage; } //当右电机PWM输出为负时 PB1设为正 PB0设为负 （PB1 0 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转） if(s16RightVoltage<0) { GPIO_SetBits(GPIOB, GPIO_Pin_1); GPIO_ResetBits(GPIOB, GPIO_Pin_0 ); s16RightVoltage = (-s16RightVoltage); } //当右电机PWM输出为正时 PB12设为负 PB13设为正 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转） else { GPIO_SetBits(GPIOB, GPIO_Pin_0 ); GPIO_ResetBits(GPIOB, GPIO_Pin_1 ); s16RightVoltage = s16RightVoltage; } u16RightMotorValue= (u16)s16RightVoltage; u16LeftMotorValue = (u16)s16LeftVoltage; TIM_SetCompare3(TIM2,u16LeftMotorValue); //TIM4与 u16RightMotorValue对比，不相同则翻转波形，调节PWM占空比 TIM_SetCompare4(TIM2,u16RightMotorValue); //TIM4与 u16LeftMotorValue对比，不相同则翻转波形，调节PWM占空比 #if 0 /*判断车辆 是否悬停或跌倒，调试用*/ if(Pitch>10||Pitch<-10&BST_fBluetoothDirectionSR==0&BST_fBluetoothDirectionSL==0) { TIM_SetCompare3(TIM2,0); TIM_SetCompare4(TIM2,0); stopflag=1; } else stopflag=0; if(BST_fCarAngle > 50 || BST_fCarAngle < (-50)) { TIM_SetCompare3(TIM2,0); TIM_SetCompare4(TIM2,0); stopflag=1; } else stopflag=0; #endif } /*************************************************************** ** 函数名称: MotorOutput ** 功能描述: 电机输出函数 将直立控制、速度控制、方向控制的输出量进行叠加,并加 入死区常量，对输出饱和作出处理。 ***************************************************************/ void MotorOutput(void) //电机PWM输出函数 { //右电机转向PWM控制融合平衡角度、速度输出 BST_fLeftMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew + BST_fBluetoothDirectionNew;//+directionl - BST_fBluetoothDirectionNew; //左电机转向PWM控制融合平衡角度、速度输出 BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew - BST_fBluetoothDirectionNew;//-directionl+ BST_fBluetoothDirectionNew; //右电机转向PWM控制融合平衡角度、速度输出 if((s16)BST_fLeftMotorOut > MOTOR_OUT_MAX) BST_fLeftMotorOut = MOTOR_OUT_MAX; if((s16)BST_fLeftMotorOut < MOTOR_OUT_MIN) BST_fLeftMotorOut = MOTOR_OUT_MIN; if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX) BST_fRightMotorOut = MOTOR_OUT_MAX; if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN) BST_fRightMotorOut = MOTOR_OUT_MIN; SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut); } //采集电机速度脉冲 void GetMotorPulse(void) { //////////////////////////////////此部分为外部中断计算脉冲///////////////////////////////////// uint16_t u16TempLeft; uint16_t u16TempRight; u16TempLeft = TIM_GetCounter(TIM3); // TIM3定时器计算调用 u16TempRight= TIM_GetCounter(TIM4); // TIM4定时器计算调用 leftstop=u16TempLeft; rightstop=u16TempRight; //清零 TIM_SetCounter(TIM3,0);//TIM3->CNT = 0; TIM_SetCounter(TIM4,0);//TIM4->CNT = 0; BST_s16LeftMotorPulse=u16TempLeft; BST_s16RightMotorPulse=(-u16TempRight); BST_s32LeftMotorPulseSigma += BST_s16LeftMotorPulse; //脉冲值叠加 40ms叠加值 BST_s32RightMotorPulseSigma += BST_s16RightMotorPulse; //脉冲值叠加 40ms叠加值 } /*************************************************************** ** 函数名称: SpeedControl ** 功能描述: 速度环控制函数 ***************************************************************/ void SpeedControl(void) { BST_fCarSpeed = (BST_s32LeftMotorPulseSigma + BST_s32RightMotorPulseSigma)*0.5;//左右电机脉冲数平均值作为小车当前车速 BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0; //全局变量 注意及时清零 BST_fCarSpeedOld *= 0.7; BST_fCarSpeedOld +=BST_fCarSpeed*0.3; BST_fCarPosition += BST_fCarSpeedOld; //路程 即速度积分 1/11 3:03 BST_fCarPosition += BST_fBluetoothSpeed;//融合蓝牙给定速度 BST_fCarPosition += fchaoshengbo; //融合超声波给定速度 if(stopflag==1) { BST_fCarPosition=0; } //积分上限设限// if((s32)BST_fCarPosition > CAR_POSITION_MAX) BST_fCarPosition = CAR_POSITION_MAX; if((s32)BST_fCarPosition < CAR_POSITION_MIN) BST_fCarPosition = CAR_POSITION_MIN; if(flagbt==3) { BST_fCarSpeed_P=0; BST_fCarSpeed_P=(y2-128)*0.46875; } if(flagbt==4) { BST_fCarSpeed_I=0; BST_fCarSpeed_I=(z2-192)*0.15625; } BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出 } /*************************************************************** ** 函数名称: BluetoothControl ** 功能描述: 蓝牙控制函数 手机发送内容 前：0x01 后：0x02 左：0x04 右：0x03 停止：0x07 功能键：（旋转） 左旋转:0x05 右旋转：0x06 停转：0x07 ** 输　入: ** 输　出: switch (x) { case 0x00 : BST_fCarAngle_P=BST_fCarAngle_D=BST_fCarSpeed_P=BST_fCarSpeed_I=0; case 0x01 : BST_fBluetoothSpeed = 3000 ;chaoflag=1; break; //向前速度 250 case 0x02 : BST_fBluetoothSpeed = (-3000);chaoflag=1; break; //后退速度 -250 case 0x03 : BST_fBluetoothDirectionNew= -300; chaoflag=1;break ;//左旋 case 0x04 : BST_fBluetoothDirectionNew= 300; chaoflag=1;break ;//右旋转 case 0x05 : BST_fBluetoothDirectionNew= driectionxco; chaoflag=1;break ;//左旋 case 0x06 : BST_fBluetoothDirectionNew= -driectionxco; chaoflag=1;break ;//右旋转 case 0x07 : BST_fBluetoothDirectionL =0; BST_fBluetoothDirectionR = 0; BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0;fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0; break; //停 case 0x08 : BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0; fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0;break; //停旋转 case 0x09 : BST_fBluetoothSpeed = 0 ; break; case 0x0A : flagbt=1;break; case 0x0B : flagbt=2;break; case 0x0C : flagbt=3;break; case 0x0D : flagbt=4;break; default : BST_fBluetoothSpeed = 0;flagbt=0; BST_fBluetoothDirectionL=BST_fBluetoothDirectionR = 0;BST_fBluetoothDirectionSR=BST_fBluetoothDirectionSL=0;chaoflag=0;break; } ***************************************************************/ int num = 0; u8 startBit = 0; int int9num =0; void USART1_IRQHandler(void) { u8 uartvalue = 0; if (USART_GetFlagStatus(USART1, USART_FLAG_ORE) != RESET)//注意！不能使用if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)来判断 { USART_ClearFlag(USART1, USART_FLAG_ORE); //读SR其实就是清除标志 USART_ReceiveData(USART3); } if(USART_GetFlagStatus(USART1,USART_FLAG_RXNE)!=RESET) { USART_ClearITPendingBit(USART1, USART_IT_RXNE); uartvalue = USART1->DR; if(uartvalue == '$') { startBit = 1; num = 0; } if(startBit == 1) { inputString[num] = uartvalue; } if (startBit == 1 && uartvalue == '#') { newLineReceived = 1; startBit = 0; int9num = num; } num++; if(num >= 80) { num = 0; startBit = 0; newLineReceived = 0; } // if(uartvalue == 'a') // { // GPIO_ResetBits(GPIOB, GPIO_Pin_12 | GPIO_Pin_13);//灯亮 // } // else if(uartvalue == 'b') // { // GPIO_SetBits(GPIOB, GPIO_Pin_12 | GPIO_Pin_13);//灯灭 // } // else if(uartvalue == 'd') // { // GPIO_ResetBits(GPIOB, GPIO_Pin_5); //蜂鸣器不响 // } // // else if(uartvalue == 'c') // { // GPIO_SetBits(GPIOB, GPIO_Pin_5); //蜂鸣器响 // } } } /**********************超声波距离计算***************************/ void chaoshengbo(void) { static uint8_t obstacle_detected = 0; // 标记是否检测到障碍物 if(chaoflag==0) { juli=TIM_GetCounter(TIM1)*5*34/200.0; if(juli <= 4.00) //判断若距离小于8cm，小车输出向后PWM值。 { obstacle_detected = 1; fchaoshengbo = -200; // 后退PWM值 } else if(juli >= 5 && juli <= 8) { obstacle_detected = 0; // 清除障碍物标记 fchaoshengbo=200; } else { obstacle_detected = 0; // 清除障碍物标记 fchaoshengbo=0; //距离大于8cm ，超声波PWM输出为0 } } } void CarStateOut(void) { switch (g_newcarstate) { case enSTOP: //停止 { BST_fBluetoothSpeed = 0; fchaoshengbo=0; BST_fBluetoothDirectionNew=0; chaoflag=0; } break; case enRUN: //向前速度 100 { BST_fBluetoothDirectionNew= 0; //BST_fSpeedControlOutNew=0; BST_fBluetoothSpeed = 50 ; chaoflag=1; }break; case enLOW: //向前速度 150 { BST_fBluetoothDirectionNew= 0; //BST_fSpeedControlOutNew=0; BST_fBluetoothSpeed = 100 ; chaoflag=1; }break; case enMEDIUM: //向前速度 200 { BST_fBluetoothDirectionNew= 0; //BST_fSpeedControlOutNew=0; BST_fBluetoothSpeed = 150 ; chaoflag=1; }break; case enHIGH: //向前速度 250 { BST_fBluetoothDirectionNew= 0; //BST_fSpeedControlOutNew=0; BST_fBluetoothSpeed = 200 ; chaoflag=1; }break; case enLEFT://左转 { BST_fBluetoothDirectionNew= -300; chaoflag=1; }break; case enRIGHT: //右转 { BST_fBluetoothDirectionNew= 300; chaoflag=1; }break; case enBACK: //后退速度 -100 { BST_fBluetoothDirectionNew= 0; //BST_fSpeedControlOutNew=0; BST_fBluetoothSpeed = (-100); chaoflag=1; }break; case enTLEFT: //左旋 { BST_fBluetoothDirectionNew = -driectionxco; chaoflag=1; }break; case enTRIGHT: //右旋 { BST_fBluetoothDirectionNew = driectionxco; chaoflag=1; }break; default: BST_fBluetoothSpeed = 0; break; //停止 case enLED: //LED亮 { GPIO_ResetBits(GPIOB, GPIO_Pin_12 | GPIO_Pin_13);//灯亮 }break; case enLED1: //LED灭 { GPIO_SetBits(GPIOB, GPIO_Pin_12 | GPIO_Pin_13);//灯灭 }break; case enBEEP: //蜂鸣器响 { GPIO_SetBits(GPIOB, GPIO_Pin_5); //蜂鸣器响 }break; case enBEEP1: //蜂鸣器不响 { GPIO_ResetBits(GPIOB, GPIO_Pin_5); //蜂鸣器不响 }break; } } void ProtocolCpyData(void) { memcpy(ProtocolString, inputString, num+1); memset(inputString, 0x00, sizeof(inputString)); } /*************************************************************************** 串口协议数据解析 ***************************************************************************/ void Protocol(void) { switch (ProtocolString[1]) { case run_car: g_newcarstate = enRUN; break; case back_car: g_newcarstate = enBACK; break; case left_car: g_newcarstate = enLEFT;break; case right_car: g_newcarstate = enRIGHT;break; case stop_car: g_newcarstate = enSTOP; break; case low_car: g_newcarstate = enLOW; break; case medium_car: g_newcarstate = enMEDIUM; break; case high_car: g_newcarstate = enHIGH; break; case led_car: g_newcarstate = enLED; break; case led1_car: g_newcarstate = enLED1; break; case beep_car: g_newcarstate = enBEEP; break; case beep1_car: g_newcarstate = enBEEP1; break; } switch (ProtocolString[3]) { case '1': g_newcarstate = enTRIGHT; break; case '2': g_newcarstate = enTLEFT; break; } /*防止数据丢包*/ if(strlen((const char *)ProtocolString)<21) { newLineReceived = 0; memset(ProtocolString, 0x00, sizeof(ProtocolString)); return; } newLineReceived = 0; memset(ProtocolString, 0x00, sizeof(ProtocolString)); } 加上遇到障碍物小车就停止