技术日志——第六篇

今天真的是完成了很多的任务，但是也发现了很多的问题，最终的效果是这样的：

点击播放测试视频

整车的模型不稳定

打滑现象非常严重！

由于目前整车都是用3D打印的，因为有很多结构还没有确定，不方便直接出图加工，之前测试强度不够高的时候感觉强度还不错，没有什么问题，但是今天的测试强度比较大，整车震动比较强烈，强度就明显不够了，有两个不稳定的地方：

一、这个是最明显的地方，就是下图中的四个角的地方在运动过程中，垂直向上弯曲了，所以当麦克纳姆轮每转过一个辊子，车并不是往前走，而是四个角的位置往上翘，这样就打滑了，真的非常非常影响小车的运行

二、第二点就如下图标注的一样，我已经发现了稍微有一点明显的变形，中间的位置下沉，压的两边的轮子往外走，同时有一点向斜上方变形，这样就导致电机轴并不平行于地面了，我还分析不出来影响在哪里，但是感觉还是有影响的。

但其实我已经喜欢上了3D打印直接打印整车，一个原因是我自己本身就有3D打印机，比较方便，想做什么就可以做，回学校后学校里也有很多3D打印机，效率比较高。另一个原因是，比赛前有零件坏了，我可以在很短的时间内做出来新的并且安装上，如果是激光切割或者CNC加工，就需要等了，而且3D打印的话我可以在比赛前备用很多组零件，一个坏了就换另一个，比较的方便。

但是强度不够是一个问题，我思考一下能否略微更改结构来让小车更加稳定一些，比如加一些加强的结构。

更改后的模型

对结构做了部分更改，其实改动还蛮大的，更加费材料了，不过问题并不大。

打印估计要持续很多天，先做其他的工作叭。

总体控制程序

这里详细地阐述一遍小车的控制程序，我的所有控制程序的基础，就是麦克纳姆轮底盘的速度合成公式，这里再放一遍这个公式。

根据这个公式可以写出来以下的速度合成程序

速度合成程序

速度合成程序就是对公式进行程序化，我这里的加减号和公式略有区别，主要是根据我自己电机的安装来调整的，已经测试过没有问题

void Calculated_combined_velocity(void)
{	
	target_V1=(Vy+Vx-w);//计算组合速度
	target_V2=(Vy-Vx+w);
	target_V3=(Vy+Vx+w);
	target_V4=(Vy-Vx-w);
	float t_V[4]={custom_abs(target_V1),custom_abs(target_V2),custom_abs(target_V3),custom_abs(target_V4)};//将组合速度放到数组t_V里，且都是绝对值
	float temp;
  for(int j=0;j<3;j++)//外层循环限制 
  {
    for(int i=0;i<3-j;i++)//内存循环 
    if(t_V[i]>t_V[i+1])//如果前一个数比后一个数大 
    {
      temp=t_V[i]; //把小的数赋值给前面，大的数赋值给后面 
      t_V[i]=t_V[i+1];
      t_V[i+1]=temp;
    }
  } 
	printf("t_V: %.2f %.2f %.2f %.2f\r\n",t_V[0],t_V[1],t_V[2],t_V[3]);
	if(t_V[3]>=300)//如果最大的速度超过了300，会等比例的缩小
	{
		float k_scaling;
		k_scaling=t_V[3]/300;
		target_V1=target_V1/k_scaling;
		target_V2=target_V2/k_scaling;
		target_V3=target_V3/k_scaling;
		target_V4=target_V4/k_scaling;
		
	}
	printf("t_V1=%.2f , t_V2=%.2f , t_V3=%.2f , t_V4=%.2f\r\n",target_V1,target_V2,target_V3,target_V4);
}

转速上限问题

不得不再提一个点，转速上限问题

电机空载测试的时候，每个电机都有转速上限，大概都是300，但是实际上加上负载后会更低，先就按照300来计算，假设我三个速度合成起来之后，超过了300，那根据公式计算出来的占空比或许会超过100%，那结果就是合速度的方向不正确了

为了解决这个问题，写了一段程序：

首先是定义变量：

float Max_Vx=150,Max_Vy=180,Max_w=200;
float bas_Vx=40,bas_Vy=40,bas_w=60;

上面的这个变量，类似于Vx、Vy、w的混合比例，其中含有Max的是最大值，含有bas的是基础值，因为电机想要转动有最低占空比，所以我给每一个方向上的速度设置了最低转速，而各个速度必须有一个上限，否则单个计算出来的速度可能会非常大，再经过缩放，调整到300转以下时，比例非常的不协调

然后我是如何将速度限制在300以内呢，为了不改变速度的方向，只能通过整体缩放来实现

关键的代码就是上面那段的后半部分：

	float t_V[4]={custom_abs(target_V1),custom_abs(target_V2),custom_abs(target_V3),custom_abs(target_V4)};//将组合速度放到数组t_V里，且都是绝对值
	float temp;
  for(int j=0;j<3;j++)//外层循环限制 
  {
    for(int i=0;i<3-j;i++)//内存循环 
    if(t_V[i]>t_V[i+1])//如果前一个数比后一个数大 
    {
      temp=t_V[i]; //把小的数赋值给前面，大的数赋值给后面 
      t_V[i]=t_V[i+1];
      t_V[i+1]=temp;
    }
  } 
	printf("t_V: %.2f %.2f %.2f %.2f\r\n",t_V[0],t_V[1],t_V[2],t_V[3]);
	if(t_V[3]>=300)//如果最大的速度超过了300，会等比例的缩小
	{
		float k_scaling;
		k_scaling=t_V[3]/300;
		target_V1=target_V1/k_scaling;
		target_V2=target_V2/k_scaling;
		target_V3=target_V3/k_scaling;
		target_V4=target_V4/k_scaling;
		
	}
	printf("t_V1=%.2f , t_V2=%.2f , t_V3=%.2f , t_V4=%.2f\r\n",target_V1,target_V2,target_V3,target_V4);
}

首先把叠加后的各个速度的绝对值放到一个数组t_V里，再将这个数组冒泡排序，获得最大值，如果这个最大值超过了300，就将这个数除以300得到比例系数k的值，再将四个速度分别除以k得到最终的速度。

这样就可以在不改变速度方向的情况下，将速度限制在300以内了。

而Vx，Vy和w是如何得到的呢？

陀螺仪姿态矫正程序

HWT101陀螺仪是自带硬件归零的，但是我发现它的归零在每次开机的时候还是有偏差，所以我还是直接在程序里写了一个软件归零，其效果是每次开机的位置为标准的位置。

开机陀螺仪归零程序

首先拟定新变量：

int initial_Angle_number=10;
int count_Angle=0;
float initial_Angle[10];
bool is_Count_Angle_ok=false;
bool is_middle_Angle_ok=false;
float middle_Angle;
float temp_Angle;

把串口中断回调函数里的2号串口（就是陀螺仪）的回调函数改成了如下模样：

  if(huart->Instance == USART2)
	{
		if(RxByte==0x53)	start_u2=true;
		if(start_u2==true)
		{
			RxBuff[Rx_Count++]=RxByte;
			if(Rx_Count>=7)
			{
				Rx_Count=0;
				start_u2=false;
				
				lastAngle=Angle;
				
				if(((((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0)!=0)	Angle = (((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0;
				
				if(Angle >=180 && Angle <=360)	Angle=Angle-360;//从0~360映射到-180~180
				if(is_middle_Angle_ok)	Angle=Angle-middle_Angle;//将中间位置更改为middle_Angle测出的角度
				//printf("count = %d initial = %d\r\n",count_Angle,initial_Angle_number);
				if(count_Angle<initial_Angle_number)
				{
					initial_Angle[count_Angle]=Angle;
					count_Angle++;
					//printf("Angle = %f\r\n",Angle);
				}
				else
				{
					is_Count_Angle_ok=true;
					//printf("Angle = %f\r\n",Angle);
				}

			}
		}
		if(Rx_Count>=254)
		{
			Rx_Count=0;
		}
		while(HAL_UART_Receive_IT(&huart2,&RxByte,1)==HAL_OK);
	}

其中关键的部分是这里：

if(Angle >=180 && Angle <=360)	Angle=Angle-360;//从0~360映射到-180~180
				if(is_middle_Angle_ok)	Angle=Angle-middle_Angle;//将中间位置更改为middle_Angle测出的角度
				//printf("count = %d initial = %d\r\n",count_Angle,initial_Angle_number);
				if(count_Angle<initial_Angle_number)
				{
					initial_Angle[count_Angle]=Angle;
					count_Angle++;
					//printf("Angle = %f\r\n",Angle);
				}
				else
				{
					is_Count_Angle_ok=true;
					//printf("Angle = %f\r\n",Angle);
				}

上述代码的总体意思为，首先要把角度从0到360映射到-180到180，之后进入一个判断if(is_middle_Angle_ok)，这个意思是中间的角度取值完毕后，就可以用后面的式子得到校准中间位置后的Angle值；之后的意思就是把十个数放到一个叫initial_Angle的数组里，然后停止操作。

后面的一步就是把int main里的while分成两部分：

	while(1)
	{
		HAL_Delay(100);
		printf("is_Middle_Angle is not Ready!\r\n");
		if(is_Count_Angle_ok)
		{
			for(int j=0;j<initial_Angle_number-1;j++)//外层循环限制 
			{
				for(int i=0;i<initial_Angle_number-1-j;i++)//内存循环 
				if(initial_Angle[i]>initial_Angle[i+1])//如果前一个数比后一个数大 
				{		
					temp_Angle=initial_Angle[i]; //把小的数赋值给前面，大的数赋值给后面 
					initial_Angle[i]=initial_Angle[i+1];
					initial_Angle[i+1]=temp_Angle;
				}
			}
			float sum_initial_Angle=0;
			for(int i=3;i<initial_Angle_number-3;i++)
			{
				sum_initial_Angle+=initial_Angle[i];//取中间几个数相加
			}
			middle_Angle=sum_initial_Angle/(initial_Angle_number-6);//取平均得到middle_Angle
			printf("middle_Angle = %.2f \r\n ",middle_Angle);
				
			is_middle_Angle_ok=true;
			break;
		}
		
	}
		

	while(1)
	{
		set_velocity(1,target_V1);
		set_velocity(2,target_V2);
		set_velocity(3,target_V3);
		set_velocity(4,target_V4);		
	}

第二部分就是设定电机速度的函数，没有问题。

第一部分是处理最先获得的10个数，从回调函数里可以看出来，当取得10个数后，会翻转is_Count_Angle_ok变量，这样第一个while停止循环，进入if。
进入if后把最先获得的10个数进行冒泡排序，然后去掉前三个和后三个，类似于比赛的时候去掉最低分和最高分，把剩下的取平均，作为middle_Angle，也就是中间角度，这样子中间角度获得，之后翻转is_middle_Angle_ok变量，反馈给串口回调函数，再break出第一个while，进入第二个while

至此已经将开机时刻的位置设为基准位置，也就是零点。

之后是在Auto_Control头文件里的控制程序

陀螺仪控制程序

陀螺仪的校准计划用一个PID控制，但现在只有P，因为刚搭建好，I和D也不确定是否需要加，加上之后可能会更麻烦，就只有P

而且现在的控制程序，所有的参数我都是凭感觉取的，不合适的话凭感觉改，也不知道改的对不对，也没有深刻考虑过各个参数对整车的影响，而且最关键的是，我不知道我这么写的控制程序是否合理

定义变量：

float kp=4,ki=0,kd=0;
float dAngle;
float excursion_w=0;
float Add_Angle=0;
float Add_Max=100;//积分上限

计算角度速度程序，

void Calculate_angular_velocity(void)
{
	dAngle=Angle-lastAngle;
	excursion_w=dAngle/dt;//对Angle进行求导
	
	Add_Angle+=Angle;//对Angle进行积分
	if(Add_Angle>=Add_Max)	Add_Angle=Add_Max;//抗积分饱和
	if(Add_Angle<=-Add_Max)	Add_Angle=-Add_Max;
	
	w=kp*Angle+ki*Add_Angle+kd*excursion_w;//调整w
	if(Angle>1)	w+=bas_w;
	else if(Angle<-1)	w-=bas_w;
	else w=0;
	if(w>=Max_w)	w=Max_w;//把w限制在Max_w之内
	else if(w<=-Max_w)	w=-Max_w;
	
	printf("Angle = %.2f w = %.2f \r\n",Angle,w);
}

根据PID的公式获得w的值，然后如果Angle角度偏差大于1或者小于-1，就分别添加对应的基础值，如果在-1和1之间默认角度已经比较准确了，就设置w为0避免来回跳动。再往后就是把w限制在Max_w的范围里

到此为止已经可以获得w了

Vx和Vy的获得方式如下：

Vx、Vy的计算程序

Vx和Vy的计算程序是比较直观简单的，但是其实也不是很可靠，从控制视频里看出，这样计算出来的速度非常的不合理，速度时快时慢

void Calculate_the_direction_velocity(int target_x,int target_y)
{
	Vx=dis2-target_x;//大部分时间是满速的，有一个最小速度bas_Vx和bas_Vy，但注意这不是最终速度
	Vy=dis1-target_y;
	
	if(custom_abs(dis2-target_x)<500 && custom_abs(dis2-target_x)>300)
	{
		if(Vx>=0)	Vx=200;
		else	Vx=-200;
	}
	if(custom_abs(dis2-target_x)<300 && custom_abs(dis2-target_x)>150)
	{
		if(Vx>=0)	Vx=120;
		else	Vx=-120;
	}
	if(custom_abs(dis2-target_x)<150)
	{
		Vx=dis2-target_x;
	}
	
	if(custom_abs(dis1-target_y)<500 && custom_abs(dis1-target_y)>300)
	{
		if(Vy>=0)	Vy=200;
		else	Vy=-200;
	}
	if(custom_abs(dis1-target_y)<300 && custom_abs(dis1-target_y)>150)
	{
		if(Vy>=0)	Vy=120;
		else	Vy=-120;
	}
	if(custom_abs(dis1-target_y)<150)
	{
		Vy=dis1-target_y;
	}
	
	
	if(Vx>0)	Vx+=bas_Vx;//设定速度基础
	else if(Vx<0)	Vx-=bas_Vx;
	if(Vy>0)	Vy+=bas_Vy;
	else if(Vy<0)	Vy-=bas_Vy;
	
	if(Vx>=Max_Vx)	Vx=Max_Vx;//设定速度限制
	else if(Vx<=-Max_Vx)	Vx=-Max_Vx;
	if(Vy>=Max_Vy)	Vy=Max_Vy;
	else if(Vy<=-Max_Vy)	Vy=-Max_Vy;
	printf("dis 1= %.2f , dis2 = %.2f , Vx = %.2f , Vy = %.2f\r\n",dis1,dis2,Vx,Vy);
}

这个函数的前两行：

	Vx=dis2-target_x;//大部分时间是满速的，有一个最小速度bas_Vx和bas_Vy，但注意这不是最终速度
	Vy=dis1-target_y;

实际上是用来大致获得速度方向用的，这个速度并不是最后的速度，只是获得一个正负值

再往后的话我没有做计算，直接分成了三段，距离目标300毫米到500毫米之间，速度是200,150到300之间速度是120,150以内速度会和距离成正比

实际效果确实不怎么样

再往后就是提供基础速度和限制最高速度了

移动到指定位置的程序

这个函数我是要放在TIM2的定时中断里，每50ms调用一次，也就是说，每50ms更新一次速度

bool Is_x_ok=false;
bool Is_y_ok=false;
bool Is_Angle_ok=false;
extern bool mission_complete;

void run_to(int target_x,int target_y)//输入目标位置，小车会走到目标位置并发出任务完成标志信号
{
	
	Calculate_angular_velocity();	//获取到w
	Calculate_the_direction_velocity(target_x,target_y);//获取到Vx和Vy

	
	if(custom_abs(dis1-target_x)<20)
	{
		Is_x_ok=true;
		Vx=0;	
	}		
	else Is_x_ok=false;
	if(custom_abs(dis2-target_y)<20)
	{
		Is_y_ok=true;
		Vy=0;	
	}	
	else Is_y_ok=false;
	if(custom_abs(Angle)<2)
	{
		Is_Angle_ok=true;
		w=0;
	}
	else Is_Angle_ok=false;
	
	//Is_x_ok=true;
	
	if(Is_x_ok && Is_y_ok && Is_Angle_ok)
	{
		Is_x_ok=false;
		Is_y_ok=false;
		Is_Angle_ok=false;
		mission_complete=true;
	}
	
	if(mission_complete)	
	{
		Vx=0;
		Vy=0;
		w=0;
		mission_complete=false;
	}
	
	Calculated_combined_velocity();//叠加
	
}

函数有两个输入参数，target_x和target_y，分别表示x轴坐标的目标距离和y轴坐标的目标距离

输入这两个目标后，调用之前的函数获取w和Vx、Vy的值，再进行判断，如果目标和实际距离差距到一定程度后，会将相应的判断变量进行翻转，当三个判断变量（Is_x_ok、Is_y_ok、Is_Angle_ok）都是true的时候，翻转变量**mission_complete*，方便通知后面的程序“我的任务已经完成了”

叠加函数放到了最后

总结

这个底盘的控制程序和我之前用arduino写的几乎一样，但是之前跑的其实就不太行，尤其是陀螺仪的校准，小车经常会出现边来回扭边往目的地走的状况，也不知道是我的参数调整有问题，还是我的控制程序本身就有不合理的地方。

所以，计划下一步任务：

• 更改并调试程序到好用为止
• 打印3D模型