一、蓝牙初始化

1.串口3初始化函数——usart3.c
这一串代码很容易理解，就是通过串口3与蓝牙连接，手机连接蓝牙将信息发送给蓝牙模块，再传至STM32获取控制信息。

#include "usart3.h"	 

void uart3_init(u32 bound)
{
	//GPIO端口设置
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	RCC_APB1PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   // 时钟GPIOB，USART3
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
  
  //USART3_TX   PB.10
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	//USART3_RX	  PB.11
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOB, &GPIO_InitStructure);  
	//USART 初始化设置
	USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
	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_Rx | USART_Mode_Tx;
	
  USART_Init(USART3, &USART_InitStructure);
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启中断
	USART_Cmd(USART3, ENABLE);                    //使能串口 
}

/*
0x00:刹车
0x01:前进
0x02:后退
0x03:左转
0x07:右转
*/
u8 Fore,Back,Left,Right;
void USART3_IRQHandler(void)                	    // 串口1中断服务程序
{
  int Bluetooth_data;
  if(USART_GetITStatus(USART3,USART_IT_RXNE)!=0)  // 接收中断标志位拉高
  {
    Bluetooth_data=USART_ReceiveData(USART3);     // 保存接收到的指令
    if(Bluetooth_data==0x00)Fore=0,Back=0,Left=0,Right=0; // 刹车
    else if(Bluetooth_data==0x01)Fore=1,Back=0,Left=0,Right=0; // 前进
    else if(Bluetooth_data==0x05)Fore=0,Back=1,Left=0,Right=0; // 后退
    else if(Bluetooth_data==0x03)Fore=0,Back=0,Left=1,Right=0; // 左转
    else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=0,Right=1; // 右转
    else                    Fore=0,Back=0,Left=0,Right=0;
  }
}

// 发送一个
void USART3_Send_Data(char data)
{
  USART_SendData(USART3,data);
  while(USART_GetFlagStatus(USART3,USART_FLAG_TC)==0);  // 除非发送完成  
}

// 发送一串
void USART3_Send_String(char *String)
{
  u16 len,j;
  len=strlen(String);
  for(j=0;j<len;j++)
  {
    USART3_Send_Data(*String++);
  }
}

2.串口3头文件——usart3.h

#ifndef __USART_H
#define __USART_H
#include "sys.h" 

void uart3_init(u32 bound);
void USART3_IRQHandler(void);  
void USART_Send_Data(char data);
void USART_Send_String(char *String);
#endif

二、转向环控制

转向环约束控制，引入RC，不是严格的PD控制器，Kd针对的是转向环的约束，但Kp针对的是遥控的转向。

float 
  Turn_Kd=-0.6,
  Turn_Kp=-20;
  
/*****************  
转向环：系数*Z轴角速度+系数*遥控数据
******************/
int Turn(int gyro_Z,int RC)
{
  int PWM_out;
  
  // 不是严格的PD控制器，Kd针对的是转向环的约束，但Kp针对的是遥控的转向
  PWM_out = Turn_Kd*gyro_Z+Turn_Kp*RC;
  
  return PWM_out;
}

三、修改控制函数

1.二次开发接口引入

float Target_Speed=0;     // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
float Turn_Speed=0;       // 左右遥控数据

19行至37行是控制函数编写，这一块应该比较容易理解。当对应标志位置1时，进行对应的操作，同时还需进行限幅，防止超过PWM规定范围。

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  {
    if(PBin(5)==0)    // 二级判断
    { 
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
      // 1.采集编码器数据&MPU6050角度信息
      // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
      // 2.将数据压入闭环控制中，计算出控制输出量
			
      /*前后*/
      if((Fore==0)&&(Back==0))Target_Speed=0;   // 未接收到前进后退指令->速度清零，稳在原地
      if(Fore==1)Target_Speed++;  // 前进标志位为1->需要前进
      if(Back==1)Target_Speed--;  // 后退标志位为1->需要后退 
      Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);  // 限幅
      
      /*左右*/
      if((Left==0)&&(Right==0))Turn_Speed=0;
      if(Left==1)Turn_Speed++;   // 左转标志位为1->需要左转
      if(Right==1)Turn_Speed--;  // 右转标志位为1->需要右转
      Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);          // 限幅
      
      /*转向约束*/
      if((Left==0)&&(Right==0))Turn_Kd=-0.6;    // 若无左右转向指令，则开启转向约束
      else if((Left==1)||(Right==1))Turn_Kd=0;  // 若左右转向指令接收到，则去掉转向约束
      
      Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
	  Turn_out=Turn(gyroz,Turn_Speed);	
      
      PWM_out=Vertical_out;//最终输出
      
      // 3.把控制输出量加载到电机上，完成最终控制
      MOTO1 = PWM_out-Turn_out; // 左电机
      MOTO2 = PWM_out+Turn_out; // 右电机
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加载到电机上
    }
  }
}

四、主函数

在主函数中初始化串口3函数。

#include "stm32f10x.h"
#include "sys.h" 

int PWM_MAX=7200,PWM_MIN=-7200;	// PWM限幅变量
int MOTO1,MOTO2;

float Pitch,Roll,Yaw;	          // Pitch：俯仰角，Roll：横滚角，Yaw：偏航角
short gyrox,gyroy,gyroz;        // 角速度
short aacx,aacy,aacz;           // 加速度
int Encoder_Left,Encoder_Right; // 编码器数据（速度）

int main(void)	
{
	delay_init();
	NVIC_Config();
	uart1_init(115200);
	uart3_init(9600);
  	USART3_Send_String("AT+NAME hc_05_sxwl \r\n");
	OLED_Init();
	OLED_Clear();
	
	MPU_Init();
	mpu_dmp_init();
	MPU6050_EXTI_Init();
	
	Encoder_TIM2_Init();
	Encoder_TIM4_Init();
	Motor_Init();
	PWM_Init_TIM1(0,7199);
  	while(1)	
	{
		OLED_Float(0,0,Roll,3);
	} 	
}