本帖最后由 HexAI 于 2018-8-17 19:44 编辑

位置式PID

增量式PID


MPU6050初始化

读取与积分（上点时持续收集10000个数据来统计零漂并在后期减去）
使用的为STM32的硬件 I2C + 中断，软件I2C或者轮询I2C太占CPU

滤波和姿态融合都没贴上去，然后PID只截取了部分代码，不包括一些核心算法

速度公式如下，
Underpan.Speed_Wheel_1 =        -1.0f        *        Underpan.Speed_x
                                                -SQRT_3        *        Underpan.Speed_y
                                                +Ratio        *        Underpan.Speed_a;
Underpan.Speed_Wheel_2 =        +2.0f        *        Underpan.Speed_x
                                                +ZERO        *        Underpan.Speed_y
                                                +Ratio        *        Underpan.Speed_a;
Underpan.Speed_Wheel_3 =        -1.0f                *        Underpan.Speed_x
                                                +SQRT_3        *        Underpan.Speed_y
                                                +Ratio        *        Underpan.Speed_a;
如果前进的话，那么
Speed_x = 0;
Speed_y = 随便一个你想要的速度，当然你有定位器的话可以根据横坐标和PID进行纠正;
Speed_a = PID_Regular_Cacl(&PID_Underpan_Yaw);
利用PID来纠正角度误差

你没有正交全向轮定位系统，有的话可以如下来获得基于上电时刻的实时坐标
初始化：


坐标更新 每1ms



同时屏幕上可以实时显示坐标，电机速度电流位置，PID参数并可实时调节