1.串口

1.1指令

根据ASCII码，将几块连续字符码分配给不同类型的指令。这里有三种指令。

变量定义

static	int uart_receive=0;                 //蓝牙接收相关变量
static u8 Flag_PID,i,j,Receive[50];         //蓝牙接收相关变量
static float Data;                          //蓝牙接收相关变量
uart_receive=USART2->DR; 
Usart2_Receive=uart_receive;

指令1，选择工作模式。

//*************************调试界面下方按键*************************************//	
if(uart_receive>=97&&uart_receive<=105)   //键值a至i
{
	switch(uart_receive)
	{
		case 0x61: Debug_key=1;  break;    //指令a==直线模式
		case 0x62: Debug_key=2;  break;    //指令b==圆周模式
		case 0x63: Debug_key=3;  break;    //指令c==稳态模式
		case 0x64: Debug_key=4;  break;    //指令d==清除
		case 0x65: Debug_key=0;  break;    //指令e==自定义模式
		case 0x66: Debug_key=0;  break;    //指令f==自定义模式
		case 0x67: Debug_key=7;  break;    //指令g==读零
		case 0x68: Debug_key=8;  break;    //指令h==自定义模式
		case 0x69: Debug_key=0;  break;    //指令i==自定义模式
	}
}		
//*****************************************************************************//

指令2，选择客户端控制模式。

//*******************控制界面下方按键选择控制方式********************************//
if(uart_receive>=73&&uart_receive<=75) 
{
	switch(uart_receive)
	{
		case 0x49: Control_way=1; break;    //指令I==重力模式
		case 0x4A: Control_way=2; break;    //指令J==遥感模式
		case 0x4B: Control_way=3; break;    //指令K==按键模式
	}

}
//*****************************************************************************//

指令3，把圆分成八份，每π/4一个方向指令。

//***********************控制界面下方方向控制************************************//
if((uart_receive>=65&&uart_receive<=72)||(uart_receive==90)) 
{
	switch(uart_receive)
	{
		case 0x41: Flag_Direction=1; break;    //指令A==向前
		case 0x42: Flag_Direction=2; break;    //指令B==右前
		case 0x43: Flag_Direction=3; break;    //指令C==向右
		case 0x44: Flag_Direction=4; break;    //指令D==右后
		case 0x45: Flag_Direction=5; break;    //指令E==向后
		case 0x46: Flag_Direction=6; break;    //指令F==左后
		case 0x47: Flag_Direction=7; break;    //指令G==向左
		case 0x48: Flag_Direction=8; break;    //指令H==左前
		default:     Flag_Direction=0; break;    //指令Z==停止
	}
}
//*****************************************************************************//

1.2调参

为了表示浮点数，显示的数值都是放大了 100 倍的。比如显示的 10000，实际该参数是 100。

调参代码

//*************************调节界面9通道参数*************************************//
if(Usart2_Receive==0x7B) Flag_PID=1;  //指令｛ == 起始位
if(Usart2_Receive==0x7D) Flag_PID=2;  //指令 ｝== 停止位
if(Flag_PID==1)  Receive[i]=Usart2_Receive  ,  i++;//记录参数
if(Flag_PID==2)                                    //解析数据
{
	if(Receive[3]==0x50)  PID_Send=1;     		//指令P==获取设备参数
	else if(Receive[3]==0x57)  Flash_Send=1;   	//指令W==掉电保存参数
	else if(Receive[1]!=0x23)                  		//指令#==更新PID参数
	{
		for(j=i;j>=4;j--)
		{
			Data+=(Receive[j-1]-48)*pow(10,i-j);
		}
		switch(Receive[1])
		{
			case 0x30:  Basic_Amplitude=Data/100;break;      	//0==振幅
			case 0x31:  Position_KP=Data/100   ;break;      	//1==X方向KP
			case 0x32:  Position_KD=Data/100   ;break;      	//2==X方向KD
			case 0x33:  Position_Kp=Data/100   ;break;      	//3==Y方向Kp
			case 0x34:  Position_Kd=Data/100   ;break;      	//4==Y方向Kd
			case 0x35:  break;//Data_Period=Data/100;break; 	//5==预留可调周期
			case 0x36:  break;//Data_Phase=Data/100;break;  	//6==预留可调相位差
			case 0x37:  break;//Data_delta=Data/100;break; 	//7==预留可调振幅比例
			case 0x38:  break;                              			//8==预留
				 }
			 }
			 //相关标志位清零			 
			 Flag_PID=0;   
			 i=0;
			 j=0;
			 Data=0;
			 memset(Receive, 0, sizeof(u8)*50);
			}
//*****************************************************************************//

通信协议

示意图

2.运动学分析、算法

控制原理：陀螺仪测量角度，角度转换为二维位置坐标，以设定的X，Y轴运动拟合函数为期望值，通过位置式PID修订电机PWM实现控制。

关键词：角度，坐标，PID，简谐运动，李萨如图形

2.1单摆

简谐运动
定义：物体受力大小与位移成正比，而方向相反

示意图（手写笔记）

2.2李萨如图形

定义：李萨如图形由在互相垂直的方向上的两个频率成简单整数比的
简谐振动所合成的规则的、稳定的闭合曲线；
要点：互相垂直 · 简谐运动 · 频率成整数比

拟合函数代码

Target_X=Amplitude_x*sin(Alpha);       //X方向目标值函数
Target_Y=Amplitude_y*sin(Alpha+Phase); //Y方向目标值函数

手写笔记

2.3测量(X，Y轴投影)

测量代码

Measure_X=(float)tan((Roll-ZHONGZHI_B)/180*2*PI)*Height;
Measure_Y=(float)tan((Pitch-ZHONGZHI_A)/180*2*PI)*Height;

运算需将角度转弧度

示意图

2.4PID算法

代码

Motor_X=Position_PID_X(Measure_X,Target_X);
Motor_Y=Position_PID_Y(Measure_Y ,Target_Y);

PID封装函数

/**************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置值，目标值
返回值   ：电机PWM
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e(k)-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次偏差  
∑e(k)代表e(k)以及之前的偏差的累计值;其中k为1,2,,k
pwm代表输出
**************************************************************************/
int Position_PID_X (float value,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=value-Target;                                  		//计算偏差
	 Integral_bias+=Bias;	                               	//求出偏差的积分
	 Pwm=Position_KP*Bias+                                 	//PID控制器比例项
	     Position_KI*Integral_bias+                        	//PID控制器积分项
	     Position_KD*(Bias-Last_Bias);                     	//PID控制器微分项 
	 Last_Bias=Bias;                                       		//保存上一次偏差
	 return Pwm;                                           		//输出PWM
}

int Position_PID_Y (float value,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=value-Target;                                   		//计算偏差
	 Integral_bias+=Bias;	                               	//求出偏差的积分
	 Pwm=Position_Kp*Bias+                                 	//PID控制器比例项
	     Position_Ki*Integral_bias+                        	//PID控制器积分项
	     Position_Kd*(Bias-Last_Bias);                     	//PID控制器微分项  
	 Last_Bias=Bias;                                       		//保存上一次偏差
	 return Pwm;                                           		//输出PWM
}