EEPW论坛

直立时小车保持静止

       小车要在直立时保持静止，有难度，除非PID整定的比较好，但是我调的效果一般。

效果如下：

视频地址：http://player.youku.com/player.php/sid/XMTM1MjUxOTk4MA==/v.swf

三.小车绕8字行走

要使得小车实现绕8字行走，设置代码，使得小车前进的同时，又转弯，就可以实现简单的绕8字行走。走8字，前进速度以及转弯速度必须要控制好，根据自身的代码修改转弯的时间，以自身的代码为例子，需要修改的有一下文件：

1.MiniBalance.c中，TIM1_UP_TIM16_IRQHandler函数中增加为

void TIM1_UP_TIM16_IRQHandler(void)  
{  
//fivems++;  
	if(TIM1->SR&0X0001)//5ms
	{   
		if(FiveMsCount<250) { Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=1; } else if(FiveMsCount==250) { Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0; } else if(FiveMsCount<500) { Flag_Qian=1,Flag_Hou=0,Flag_Left=1,Flag_Right=0; } else if(FiveMsCount>=500) 
            {  
                FiveMsCount=0;  
                Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;  
            }  
            FiveMsCount++;  
		  TIM1->SR&=~(1<<0); readEncoder(); Led_Flash(400); Get_battery_volt(); key(100); Get_Angle(Way_Angle); Balance_Pwm =balance(Angle_Balance,Gyro_Balance); Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm; Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm; Xianfu_Pwm(); if(Turn_Off(Angle_Balance,Voltage)==0) Set_Pwm(Moto1,Moto2); } } 

	

2.int turn(int encoder_left,int encoder_right,float gyro)函数修改为

Turn_Amplitude=300/Way_Angle; 

300这个值越大，半径越小，速度越快，反之则相反效果，速度不是很快转弯效果才比较好，所以这里我们改为300

小车绕8字行走的效果如下：

四.多传感器数据融合

MPU6050 自带了数字运动处理器， 即 DMP， 结合 MPU6050 的 DMP， 可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到 yaw、roll 和 pitch。我们是做平衡小车而不是四轴飞行器，所以，仅仅使用pitch就行了。其他两个角度直接在程序中屏蔽了，因为反三角函数运算在STM32F1中还是需要不少时间的。这里使用到了模拟IIC和内置的DMP，解算出四元数。

使用 MPU6050 的 DMP 输出的四元数是 q30 格式的， 也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的30 次方，然后再进行计算，

计算公式为：

q0=quat[0] / q30; //q30 格式转换为浮点数

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

//计算得到俯仰角/横滚角/航向角

pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角

roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //横滚角

yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角

void Read_DMP(void)  
  
{   
  
 unsigned long sensor_timestamp;  
  
 unsigned char more;  
  
 long quat[4];  
  
   dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);    
  
   if (sensors & INV_WXYZ_QUAT )  
  
   {      
  
     q0=quat[0] / q30;  
  
     q1=quat[1] / q30;  
  
     q2=quat[2] / q30;  
  
     q3=quat[3] / q30;  
  
     Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;    
  
     printf("%f\r\n",Pitch);  
  
   }  
  
}  

连接串口转接线，通过上位机软件可以读取角度值如图

注意设置好波特率，我这里用到的是115200

     多传感器数据融合实验结束