小车陀螺仪：输出波形图二阶滤波.PPT

* Arduino 智能平衡小车 模块篇 小车主视图： 小车主控制板：Arduino uno r3 小车陀螺仪：MPU6050 可以输出当前模块X Y Z轴的角度 角速度 更集成温度传感器 能读取温度 小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC #include I2Cdev.h #include MPU6050_6Axis_MotionApps20.h #include Wire.h“ Wire.begin(); //加入 I2C 总线序列 mpu.initialize(); //初始化MPU6050 小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC 开始（S）和结束（P）标志 开 始信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由高变低就表示一个开始信号。同时IIC总线上的设备检测到这个开始信号它就知道处理器要发送数据了。 停止信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由低变高就表示一个停止信号。同时IIC总线上的设备检测到这个停止信号它就知道处理器已经结束了数据传输，我们就可以各忙各个的了，如休眠等。 小车陀螺仪：MPU6050 上位机读取数据程序 使用通讯方式为IIC 如果要读取MPU-60X0 寄存器的值，首先由主设备产生开始信号（S），然后发送从设备地址位和一个写数据位，然后发送寄存器地址，才能开始读寄存器。紧接着，收到应答信号后，主设备再发一个开始信号，然后发送从设备地址位和一个读数据位。然后，作为从设备的MPU-60X0 产生应答信号并开始发送寄存器数据。通信以主设备产生的拒绝应答信号（NACK）和结束标志（P）结束。拒绝应答信号（NACK）产生定义为SDA 数据在第9 个时钟周期一直为高。 小车陀螺仪：MPU6050 上位机读取数据程序 void loop() { accelgyro.getMotion6(ax, ay, az, gx, gy, gz); gyro_y=-(gy-10.58)*rgyro; Serial.print(a/g:\t); Serial.print(ax); Serial.print(\t); Serial.print(ay); Serial.print(\t); Serial.print(az); Serial.print(\t); Serial.print(gx); Serial.print(\t); Serial.print(gy); Serial.print(\t); Serial.println(gz);Serial.print(\t); Serial.print(gyro_y);Serial.print(\t); // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } Arduion IIC接口： Sda：A4 Sck：A5 小车陀螺仪：MPU6050 通过IDE自带串口工具 小车陀螺仪：输出波形图 当得到六个原始量（ax ay az gx gy gz）以后，我们可以通过三种方式得到当前角度并通过上位机得到输出波形 方式一：低阶滤波（一阶滤波，二阶滤波） 方式二：高阶滤波（卡曼滤波） 方式三：DMP直接得到角度（启动MPU6050内部运算，读取DMP存储器中数据） 小车陀螺仪：输出波形图 一阶滤波/二阶滤波： void getangle() { accelgyro.getMotion6(ax, ay, az, gx, gy, gz);//读取原始6个数据 angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角 gyroGy=-gy/131.00;//计算角速度 （1）Yijielvbo(angleAx,gyroGy);//一阶滤波 （2）Erjielvbo(angleAx,gyroGy);//二阶滤波 （3）Kalman_Filter(angleAx,gyroGy); //卡尔曼滤波 } （1）void Yijielvbo(float angle_m, float gyro_m) { angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } //一阶滤波 （2）void Erjielvbo(float angle_m,float