自平衡莱洛三角形
已启动：     电池电压： V
期望角度TA 摇摆电压SV 摇摆角度SA 速度环P1 速度环I1 速度环P2 速度环I2







0
0
Shaft
Velocity		 motor
voltage q		 target
velocity		 pendulum
angle		 target
angle		 pitch kalAngleZ gyroZrate