今年做的是光电直立，程序主要是参考官网上面给的清华的方案。原理什么的就不具体研究了，方案还有论文里面都非常的详细。

主要有三个部分：1角度控制，2速度控制，3方向控制。其中还有利用互补滤波来计算角度以及利用PID算法来进行控制。

首先是角度控制，利用陀螺仪积分来计算角度，同时由于积分会导致误差的积累，所有需要利用加速度计进行修正，这里利用的是互补滤波的算法。在加速度计计算角度的时候，我们采用的方法是，获取一个直立时候的初始值，然后让车模向左边倾斜至水平，认为此时加速度计的数值a1对应角度是90°（或者-90°也可以），之后在让车模向右边倾斜至水平，认为此时加速度计的数值a2对应角度是-90°（之前-90度对应这里是90°）。然后利用加速度计计算角度的时候就可以利用180 / (a1 - a2) * a3（a3是当前加速度计的输出）计算角度（公式这里要注意一个正负的问题，具体可以试验一下，这里就不具体描述了），这么计算角度会有一定的误差，不过考虑直立并不是要求角度非常准确，所有就没有研究其他的方式结算角度。计算角度的时候，利用互补滤波和角速度的积分进行融合，这里融合也有两种方式，一种是加速度计算出来的角度直接和陀螺仪积分出来的角度进行融合，就是ratio_accel * angle_accel + ratio_gyro * angle _gyro，这里的两个比例（ratio）个人认为应该满足相加和为1，但是由于没有尝试过，这里就暂且留个疑问。第二个种方法就是清华的方案了，他是利用加速度计计算的角度，与之前计算出来的角度做差，然后将这个差值除以一个时间常数（不太理解为什么），之后再处理过的偏差直接加到角速度的输出上面，然后角速度的输出上面有一个比例，通过调这个数来完成融合，调试的时候需要利用上位机，陀螺仪的比例过大会导致角度计算迟钝，而陀螺仪比例太小（相对来说就是加速度计的比例过大）会导致计算出来的角度输出不够平滑，最终能够确定一个比较合适比例。然后根据利用PID算法来控制角度，并直接将其转换成电压。这里利用的是PD控制，D(微分)对应角速度，这里也不是非常的理解PID,PI,PD etc之间有什么区别。最后将电压转换成PWM输出到车轮上面，就可以完成直立，可以考虑加一点死区电压，因为两个电机是不可能完全一致的。在调整PD的时候，先调整P使得车模能够围绕着直立的状态左右摇摆，但是要注意的是，单纯的P是不能让车直立起来的，然后是增加D，D能够抑制抖动，但是D过大会导致车模的高频振动。在调整好PD之后车模就能够直立起来，不过因为没有速度的闭环控制，所有会出现往一个方向一直匀速（或者有一点加速度）的前进这个是没有问题的。在调完后面的速度控制之后，就能较好的直立起来。

先写到这里，剩下的之后在写