循迹小车制作心得

（一）概述
记得去年这个时候，我参加了省上的机器人大赛，当时有熊猫乐园、机器人排爆、机器人扎气球、循迹小车等题目，我选的是循迹小车。由于我比较菜，前前后后历时一个多月才完成这个项目。期间遇到的困难有两个：一是如何循迹，二是小车如何跑直线。为了解决这两个问题，查阅大量资料后，分别采用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调参。
效果图：