循迹小车怎样做才能跑直线呢

　　（一）概述
　　记得去年这个时候，我参加了省上的机器人大赛，当时有熊猫乐园、机器人排爆、机器人扎气球、循迹小车等题目，我选的是循迹小车。由于我比较菜，前前后后历时一个多月才完成这个项目。期间遇到的困难有两个：一是如何循迹，二是小车如何跑直线。为了解决这两个问题，查阅大量资料后，分别采用PID巡线和DMP姿态解算予以解决。
　　（二）解决过程
　　（1）巡线的解决方案
　　主控板采用STM32F103C8T6，5路ADC采样（ADC的型号是ST188），ADC采样值作为PID处理函数的参数。
　　ADC采样代码
　　void Get5SensorsVal（uint16_t *p）
　　{
　　int i;
　　//ADC采样值滤波处理
　　ADC_value_filter（）;
　　for（i=0;i《5;i++）
　　{
　　*p++=ADC_filterVal［i］;
　　}
　　}
　　（2）小车走直线的解决方案
　　给小车配备MPU6050用于检测当前小车运动姿态。MPU6050有三个轴的数据：pitch， roll， yaw。采集到这三个轴的数据，经过DMP解算后传给控制小车前进的函数。这个函数写在定时器中断里，每几毫秒执行一次（具体是多少记不清了）。
　　DMP解算函数
　　u8 mpu_dmp_get_data（float *pitch，float *roll，float *yaw）
　　{
　　float q0=1.0f，q1=0.0f，q2=0.0f，q3=0.0f;
　　unsigned long sensor_timestamp;
　　unsigned char more;
　　long quat［4］;
　　if（dmp_read_fifo（gyro， accel， quat， &sensor_timestamp， &sensorss，&more））
　　return 1;
　　if（sensorss&INV_WXYZ_QUAT）
　　{
　　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; // pitch
　　*roll = atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3; // roll
　　*yaw = atan2（2*（q1*q2 + q0*q3），q0*q0+q1*q1-q2*q2-q3*q3） * 57.3; //yaw
　　}
　　else return 2;
　　return 0;
　　}
　　（三）后期优化
　　基本功能实现后，加入了蓝牙模块，可以连接手机蓝牙APP看到5路ADC采样的值，便于PID调参。