角度PID
- 微分项
Angle_Pram[KD] * gyroy：根据角速度的变化率来调整电机PWM，角速度变化越快，调整的幅度也越大。
速度PID
Encoder_Least = (Encoder1 + Encoder2 + Encoder3 + Encoder4) - Trg_Speed;：计算速度偏差，即四个编码器的读数之和与目标速度Trg_Speed的差值。
Encoder *= 0.8;：将之前的滤波结果乘以0.8，实现低通滤波的效果，减少噪声。Encoder += Encoder_Least * 0.2;：将新的速度偏差乘以0.2后加到滤波结果上，更新滤波后的速度读数。
Encoder_Integral += Encoder;：将滤波后的速度读数加到积分项上，实现积分控制。
积分限幅
if(Encoder_Integral > 20) Encoder_Integral = 20;：如果积分项超过20，将其限制在20。if(Encoder_Integral < -20) Encoder_Integral = -20;：如果积分项低于-20，将其限制在-20。这是为了防止积分饱和。
sudu_out = Speed_Pram[KP] * Encoder + Speed_Pram[KI] * Encoder_Integral;：根据比例项和积分项计算速度控制器的输出。这里使用了Speed_Pram[KP]作为比例增益，Speed_Pram[KI]作为积分增益。
转向环
Direct_Out = Direct_Pram[KD] * gyro_z;：在这种情况下，控制器仅使用微分项（D项），其中Direct_Pram[KD]是微分增益，gyro_z是陀螺仪的读数。这个微分项有助于机器人在直线行驶时快速响应并抑制任何不必要的转向或旋转。
最后实现基本运动
Angle_PID_Out = Angle_PID(Speed_PID_Out+Mid_Angle); 这个是串级pid，将速度环的输出作为直立环的输入
然后叠加转向环和轮子差速。最后得到PWM。
