欧美日韩亚洲av一区在线看,日韩一区二区电影欧美,91精品成人影院

#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>
/*****************************************/
u8 newLineReceived = 0;
u8 inputString[80] = {0};
u8 ProtocolString[80] = {0};
/*小車運行狀態(tài)枚舉*/
enum{
enSTOP = 0,
enRUN,
enBACK,
enLEFT,
enRIGHT,
enTLEFT,
enTRIGHT
}enCarState;
#define run_car '1'//按鍵前
#define back_car '2'//按鍵后
#define left_car '3'//按鍵左
#define right_car '4'//按鍵右
#define stop_car '0'//按鍵停
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};
/*****************多數(shù)據(jù)************************/
u8 BST_u8MainEventCount; //主循環(huán)判斷計數(shù) 在SysTick_Handler(void)中使用每1ms加1
u8 BST_u8SpeedControlCount; //速度控制循環(huán)計數(shù) 在SysTick_Handler(void)中使用每5ms加1
u8 BST_u8SpeedControlPeriod;
u8 BST_u8DirectionControlPeriod;
u8 BST_u8DirectionControlCount; //轉(zhuǎn)向控制循環(huán)計數(shù) 在SysTick_Handler(void)中使用每5ms加1
u8 BST_u8trig;
u8 ucBluetoothValue; //藍(lán)牙接收數(shù)據(jù)
float volt = 12.0;
/******電機(jī)控制參數(shù)******/
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;
/*-----角度環(huán)和速度環(huán)PID控制參數(shù)-----*///以下參考為重點調(diào)試參考，同電池電壓有關(guān)，建議充好電再調(diào)試
float BST_fCarAngle_P =91.3;// 91.3 //調(diào)大小時會左右擺，調(diào)大時會振動請調(diào)到基本能夠站立 P=91.3是用于給小車在運動過程使用
float BST_fCarAngle_D =0.21; // 0.001 0.002 0.004 0.008 0.0010 0.011 調(diào)小時反應(yīng)慢，調(diào)大時會干擾
float BST_fCarSpeed_P=5.1;
float BST_fCarSpeed_I=0.10;
const double PID_Original[4] ={91.3, 0.21, 5.1, 0.10};
char alldata[80];
char *iap;
/******速度控制參數(shù)******/
s16 BST_s16LeftMotorPulse; //左電機(jī)脈沖數(shù)
s16 BST_s16RightMotorPulse; //右電機(jī)脈沖數(shù)
s32 BST_s32LeftMotorPulseOld;
s32 BST_s32RightMotorPulseOld;
s32 BST_s32LeftMotorPulseSigma; //50ms左電機(jī)疊加值
s32 BST_s32RightMotorPulseSigma; //50ms右電機(jī)疊加值
float BST_fCarSpeed; //測速碼盤得出的車速
float BST_fCarSpeedOld;
float BST_fCarPosition; //測速碼盤通過計算得到的小車位移
/*-----懸停參數(shù)-----*/
int leftstop=0;
int rightstop=0;
int stopflag=0;
/******超聲波********/
float fchaoshengbo = 0; //超聲波輸出量
float juli = 0; //超聲波距離
/******藍(lán)牙控制參數(shù)******/
float BST_fBluetoothSpeed; //藍(lán)牙控制車速
float BST_fBluetoothDirectionNew; //用于平緩輸出車速使用
float BST_fBluetoothDirectionSL; //左轉(zhuǎn)標(biāo)志位由于PWM轉(zhuǎn)向輸出使用判斷輸出方式固需要標(biāo)志位
float BST_fBluetoothDirectionSR; //右轉(zhuǎn)標(biāo)志位由于PWM轉(zhuǎn)向輸出使用判斷輸出方式固需要標(biāo)志位
int chaoflag=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;
/************旋轉(zhuǎn)*****************/
float BST_fBluetoothDirectionL; //左旋轉(zhuǎn)標(biāo)志位由于PWM旋轉(zhuǎn)輸出使用判斷輸出方式固需要標(biāo)志位
float BST_fBluetoothDirectionR; //右旋轉(zhuǎn)標(biāo)志位由于PWM旋轉(zhuǎn)輸出使用判斷輸出方式固需要標(biāo)志位
int driectionxco=800;
//******卡爾曼參數(shù)************
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);
/**********延時子函數(shù)****************/
void delay_nms(u16 time)
{
u16 i=0;
while(time--)
{
i=12000; //自己定義
while(i--) ;
}
}
/***********************************/
/***************************************************************
** 函數(shù)名稱: CarUpstandInit
** 功能描述: 全局變量初始化函數(shù)
***************************************************************/
void CarUpstandInit(void)
{
BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0; //左右脈沖值初始化
BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0; //疊加脈沖數(shù) 初始化
BST_fCarSpeed = BST_fCarSpeedOld = 0; //平衡小車車速初始化
BST_fCarPosition = 0; //平衡小車位移量初始化
BST_fCarAngle = 0; //平衡小車車速初始化
BST_fAngleControlOut = BST_fSpeedControlOut = BST_fBluetoothDirectionNew = 0; //角度PWM、車速PWM、藍(lán)牙控制PWM 初始化
BST_fLeftMotorOut = BST_fRightMotorOut = 0; //左右車輪PWM輸出值初始化
BST_fBluetoothSpeed = 0; //藍(lán)牙控制車速值初始化
BST_fBluetoothDirectionL =BST_fBluetoothDirectionR= 0; //藍(lán)牙控制左右旋轉(zhuǎn)標(biāo)志位初始化
BST_fBluetoothDirectionSL =BST_fBluetoothDirectionSR= 0; //藍(lán)牙控制左右轉(zhuǎn)向標(biāo)志位初始化
BST_u8MainEventCount=0; //用于5ms定時器子程序SysTick_Handler(void)中總中斷計數(shù)位
BST_u8SpeedControlCount=0; //用于5ms定時器子程序SysTick_Handler(void)中50ms速度平衡融入計數(shù)位
BST_u8SpeedControlPeriod=0; //用于5ms定時器子程序SysTick_Handler(void)中50ms速度平衡融入計數(shù)位
fchaoshengbo=0; //用于5ms定時器子程序SysTick_Handler(void)中超聲波平衡融入計數(shù)位
}
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];
}
}
/***************************************************************
** 函數(shù)名稱: AngleControl
** 功能描述: 角度環(huán)控制函數(shù)
***************************************************************/
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];
BST_fCarAngle = Roll - CAR_ZERO_ANGLE; //DMP ROLL滾動方向角度與預(yù)設(shè)小車傾斜角度值的差得出角度
BST_fAngleControlOut = BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ; //角度PD控制
}
/***************************************************************
** 函數(shù)名稱: SetMotorVoltageAndDirection
** 功能描述: 電機(jī)轉(zhuǎn)速
***************************************************************/
void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
{
u16 u16LeftMotorValue;
u16 u16RightMotorValue;
if(s16LeftVoltage<0) //當(dāng)左電機(jī)PWM輸出為負(fù)時 PB14設(shè)為正 PB15設(shè)為負(fù) （PB14 15 分別控制TB6612fng驅(qū)動芯片，邏輯0 1可控制左電機(jī)正轉(zhuǎn)反轉(zhuǎn)）
{
GPIO_SetBits(GPIOB, GPIO_Pin_14 );
GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
s16LeftVoltage = (-s16LeftVoltage);
}
else
{
GPIO_SetBits(GPIOB, GPIO_Pin_15 ); //當(dāng)左電機(jī)PWM輸出為正時 PB14設(shè)為負(fù) PB15設(shè)為正（PB14 15 分別控制TB6612fng驅(qū)動芯片，邏輯0 1可控制左電機(jī)正轉(zhuǎn)反轉(zhuǎn)）
GPIO_ResetBits(GPIOB, GPIO_Pin_14 );
s16LeftVoltage = s16LeftVoltage;
}
if(s16RightVoltage<0)
{ //當(dāng)右電機(jī)PWM輸出為負(fù)時 PB12設(shè)為正 PB13設(shè)為負(fù) （PB12 13 分別控制TB6612fng驅(qū)動芯片，邏輯0 1可控制左電機(jī)正轉(zhuǎn)反轉(zhuǎn)）
GPIO_SetBits(GPIOB, GPIO_Pin_13 );
GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
s16RightVoltage = (-s16RightVoltage);
}
else //當(dāng)右電機(jī)PWM輸出為正時 PB12設(shè)為負(fù) PB13設(shè)為正（PB12 13 分別控制TB6612fng驅(qū)動芯片，邏輯0 1可控制左電機(jī)正轉(zhuǎn)反轉(zhuǎn)）
{
GPIO_SetBits(GPIOB, GPIO_Pin_12 );
GPIO_ResetBits(GPIOB, GPIO_Pin_13 );
s16RightVoltage = s16RightVoltage;
}
u16RightMotorValue= (u16)s16RightVoltage;
u16LeftMotorValue = (u16)s16LeftVoltage;
TIM_SetCompare3(TIM2,u16LeftMotorValue); //TIM2與 u16RightMotorValue對比，不相同則翻轉(zhuǎn)波形，調(diào)節(jié)PWM占空比
TIM_SetCompare4(TIM2,u16RightMotorValue); //TIM3與 u16LeftMotorValue對比，不相同則翻轉(zhuǎn)波形，調(diào)節(jié)PWM占空比
#if 1 /*判斷車輛是否懸�；虻�，調(diào)試用*/
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
}
/***************************************************************
** 函數(shù)名稱: MotorOutput
** 功能描述: 電機(jī)輸出函數(shù)
將直立控制、速度控制、方向控制的輸出量進(jìn)行疊加,并加
入死區(qū)常量，對輸出飽和作出處理。
***************************************************************/
void MotorOutput(void) //電機(jī)PWM輸出函數(shù)
{
//右電機(jī)轉(zhuǎn)向PWM控制融合平衡角度、速度輸出
BST_fLeftMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew + BST_fBluetoothDirectionNew;//+directionl - BST_fBluetoothDirectionNew; //左電機(jī)轉(zhuǎn)向PWM控制融合平衡角度、速度輸出
BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew - BST_fBluetoothDirectionNew;//-directionl+ BST_fBluetoothDirectionNew; //右電機(jī)轉(zhuǎn)向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) //采集電機(jī)速度脈沖
{
//////////////////////////////////此部分為外部中斷計算脈沖/////////////////////////////////////
uint16_t u16TempLeft;
uint16_t u16TempRight;
u16TempLeft = TIM_GetCounter(TIM3); // TIM3定時器計算調(diào)用
u16TempRight= TIM_GetCounter(TIM4); // TIM4定時器計算調(diào)用
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疊加值
}
/***************************************************************
** 函數(shù)名稱: SpeedControl
** 功能描述: 速度環(huán)控制函數(shù)
***************************************************************/
void SpeedControl(void)
{
BST_fCarSpeed = (BST_s32LeftMotorPulseSigma + BST_s32RightMotorPulseSigma );// * 0.5 ; //左右電機(jī)脈沖數(shù)平均值作為小車當(dāng)前車速
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; //融合藍(lán)牙給定速度
BST_fCarPosition += fchaoshengbo; //融合超聲波給定速度
if(stopflag==1)
{
BST_fCarPosition=0;
}
//積分上限設(shè)限//
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輸出
}
/***************************************************************
** 函數(shù)名稱: BluetoothControl
** 功能描述: 藍(lán)牙控制函數(shù)
手機(jī)發(fā)送內(nèi)容
前：0x01 后：0x02
左：0x04 右：0x03
停止：0x07
功能鍵：（旋轉(zhuǎn)）
左旋轉(zhuǎn):0x05 右旋轉(zhuǎn)：0x06
停轉(zhuǎn)：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 ;//右旋轉(zhuǎn)
case 0x05 : BST_fBluetoothDirectionNew= driectionxco; chaoflag=1;break ;//左旋
case 0x06 : BST_fBluetoothDirectionNew= -driectionxco; chaoflag=1;break ;//右旋轉(zhuǎn)
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; //停旋轉(zhuǎn)
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 USART3_IRQHandler(void)
{
u8 uartvalue = 0;
if (USART_GetFlagStatus(USART3, USART_FLAG_ORE) != RESET)//注意！不能使用if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)來判斷
{
USART_ClearFlag(USART3, USART_FLAG_ORE); //讀SR其實就是清除標(biāo)志
USART_ReceiveData(USART3);
}
if(USART_GetFlagStatus(USART3,USART_FLAG_RXNE)!=RESET)
{
USART_ClearITPendingBit(USART3, USART_IT_RXNE);
uartvalue = USART3->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;
}
}
}
/**********************超聲波距離計算***************************/
void chaoshengbo(void)
{
if(chaoflag==0)
{
juli=TIM_GetCounter(TIM1)*5*34/200.0;
if(juli <= 4.00) //判斷若距離小于8cm，小車輸出向后PWM值。
{
fchaoshengbo= (-300);
}
else if(juli >= 5 & juli <= 8)
{
fchaoshengbo=500;
}
else
{
fchaoshengbo=0; //距離大于8cm ，超聲波PWM輸出為0
}
}
//寄生此上報數(shù)據(jù)
//增加自動上報 10ms進(jìn)一次，故10*50ms = 500ms
//SendAutoUp();
}
/*計算上報的數(shù)據(jù)*/
void CalcUpData()
{
float ls, rs, sLence;
short s_Acc, s_Gyro;
if(g_autoup == 1)
{
//CLI();
ls = BST_fLeftMotorOut;
rs = BST_fRightMotorOut;
s_Acc = accel[1];
s_Gyro = gyro[0];
sLence = juli;
//SEI();
dac=(s_Acc/16384.0f)*9.8f;
dgy=((s_Gyro-128.1f)/131.0f);
ls=ls/3.91;
rs=rs/3.91;
memset(manydisplay, 0x00, 80);
memcpy(manydisplay, "$LV", 4);
memset(lspeed, 0x00, sizeof(lspeed));
memset(rspeed, 0x00, sizeof(rspeed));
memset(daccel, 0x00, sizeof(daccel));
memset(dgyro, 0x00, sizeof(dgyro));
memset(csb, 0x00, sizeof(csb));
memset(vi, 0x00, sizeof(vi));
if((ls <= 1000) && (ls >= -1000))
sprintf(lspeed,"%3.2f",ls);
else
{
//sprintf(str,"$AutoUpError!ls=%3.2f#",ls);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((rs <= 1000) && (rs >= -1000))
sprintf(rspeed,"%3.2f",rs);
else
{
//sprintf(str,"$AutoUpError!rs=%3.2f#",rs);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((dac > -20) && (dac < 20))
sprintf(daccel,"%3.2f",dac);
else
{
//sprintf(str,"$AutoUpError!dac=%3.2f#",dac);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((dgy > -3000) && (dgy < 3000))
sprintf(dgyro,"%3.2f",dgy);
else
{
//sprintf(str,"$AutoUpError!dgy=%3.2f#",dgy);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((sLence >= 0) && (sLence < 10000))
sprintf(csb,"%3.2f",sLence);
else
{
//sprintf(str,"$AutoUpError!juli=%3.2f#",juli);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((volt >= 0) && (volt < 20))
sprintf(vi,"%3.2f",volt);
else
{
//sprintf(str,"$AutoUpError!volt=%3.2f#",volt);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
strcat(manydisplay,lspeed);
strcat(manydisplay,",RV");
strcat(manydisplay,rspeed);
strcat(manydisplay,",AC");
strcat(manydisplay,daccel);
strcat(manydisplay,",GY");
strcat(manydisplay,dgyro);
strcat(manydisplay,",CSB");
strcat(manydisplay,csb);
strcat(manydisplay,",VT");
strcat(manydisplay,vi);
strcat(manydisplay,"#");
memset(updata, 0x00, 80);
memcpy(updata, manydisplay, 80);
}
}
/*
自動上報
*/
void SendAutoUp(void)
{
g_uptimes --;
if ((g_autoup == 1) && (g_uptimes == 0))
{
CalcUpData();
UART3_Send_Char(updata); //返回協(xié)議數(shù)據(jù)包
}
if(g_uptimes == 0)
g_uptimes = 5000;
}
int StringFind(const char *pSrc, const char *pDst)
{
int i, j;
for (i=0; pSrc[i]!='\0'; i++)
{
if(pSrc[i]!=pDst[0])
continue;
j = 0;
while(pDst[j]!='\0' && pSrc[i+j]!='\0')
{
j++;
if(pDst[j]!=pSrc[i+j])
break;
}
if(pDst[j]=='\0')
return i;
}
return -1;
}
void CarStateOut(void)
{
switch (g_newcarstate)
{
case enSTOP: //停止
{
BST_fBluetoothSpeed = 0;
fchaoshengbo=0;
BST_fBluetoothDirectionNew=0;
chaoflag=0;
} break;
case enRUN: //向前速度 800
{
BST_fBluetoothDirectionNew= 0;
//BST_fSpeedControlOutNew=0;
BST_fBluetoothSpeed = 800 ;
chaoflag=1;
}break;
case enLEFT://左轉(zhuǎn)
{
BST_fBluetoothDirectionNew= -300;
chaoflag=1;
}break;
case enRIGHT: //右轉(zhuǎn)
{
BST_fBluetoothDirectionNew= 300;
chaoflag=1;
}break;
case enBACK: //后退速度 -800
{
BST_fBluetoothDirectionNew= 0;
//BST_fSpeedControlOutNew=0;
BST_fBluetoothSpeed = (-800);
chaoflag=1;
}break;
case enTLEFT: //左旋
{
BST_fBluetoothDirectionNew = -driectionxco;
chaoflag=1;
}break;
case enTRIGHT: //右旋
{
BST_fBluetoothDirectionNew = driectionxco;
chaoflag=1;
}break;
default: BST_fBluetoothSpeed = 0; break; //停止
}
}
void ProtocolGetPID(void)
{
memset(piddisplay, 0x00, sizeof(piddisplay));
memcpy(piddisplay, "$AP", 4);
if(BST_fCarAngle_P >= 0 && BST_fCarAngle_P <= 100)
{
sprintf(charkp,"%3.2f",BST_fCarAngle_P);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarAngle_D >= 0 && BST_fCarAngle_D <= 100)
{
sprintf(charkd,"%3.2f",BST_fCarAngle_D);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarSpeed_P >= 0 && BST_fCarSpeed_P <= 100)
{
sprintf(charksp,"%3.2f",BST_fCarSpeed_P);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarSpeed_I >= 0 && BST_fCarSpeed_I <= 100)
{
sprintf(charksi,"%3.2f",BST_fCarSpeed_I);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
strcat(piddisplay,charkp);
strcat(piddisplay,",AD");
strcat(piddisplay,charkd);
strcat(piddisplay,",VP");
strcat(piddisplay,charksp);
strcat(piddisplay,",VI");
strcat(piddisplay,charksi);
strcat(piddisplay,"#");
UART3_Send_Char(piddisplay); //返回協(xié)議數(shù)據(jù)包
}
void ProtocolCpyData(void)
{
memcpy(ProtocolString, inputString, num+1);
memset(inputString, 0x00, sizeof(inputString));
}
/***************************************************************************
串口協(xié)議數(shù)據(jù)解析
***************************************************************************/
void Protocol(void)
{
//USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);//禁能接收中斷
//判斷數(shù)據(jù)包有效性
switch (ProtocolString[1])
{
case run_car: g_newcarstate = enRUN; UART3_Send_Char(returntemp);break;
case back_car: g_newcarstate = enBACK; UART3_Send_Char(returntemp);break;
case left_car: g_newcarstate = enLEFT; UART3_Send_Char(returntemp);break;
case right_car: g_newcarstate = enRIGHT; UART3_Send_Char(returntemp);break;
case stop_car: g_newcarstate = enSTOP; UART3_Send_Char(returntemp);break;
default: g_newcarstate = enSTOP; break;
}
/*防止數(shù)據(jù)丟包*/
if(strlen((const char *)ProtocolString)<21)
{
newLineReceived = 0;
memset(ProtocolString, 0x00, sizeof(ProtocolString));
//USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//使能接收中斷
UART3_Send_Char("$ReceivePackError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if (ProtocolString[3] == '1') //左搖
{
g_newcarstate = enTLEFT;
UART3_Send_Char(returntemp); //返回協(xié)議數(shù)據(jù)包
}
if (ProtocolString[3] == '2') //右搖
{
g_newcarstate = enTRIGHT;
UART3_Send_Char(returntemp); //返回協(xié)議數(shù)據(jù)包
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復(fù)制代碼