请问平衡车基本功能已实现，但是平衡效果不是很好应该如何解决？

我现在做出来的效果能平衡了，但是他总是在50CM范围前后来回平衡，不能像我看到的一些发烧友做的那种在那点上很小的移动平衡。不过我选择的这个平衡车，重心偏高，而且轮子是履带式的（坦克那种），车身也比较重，马达用的是铁芯马达，马达输出基本要达到30%才能带动负载转动。我直立环是用角度和角速度做的；速度环是用光耦开关做的（PI控制），不过我这种检测光耦的方法只能是检测光耦开关的导通次数，且根据当前马达本身输出的正反转来确定方向。现在效果不怎么好，请帮忙分析下问题出现在哪里？ 谢谢！！！