你这个问题还是比较多的,
首先,一个功能一个功能的都调试通过了,再把每个功能都整合在一起。
整合时,逻辑的判断等问题,也会有,这个就要你一步步进行硬件仿真调试了
你这个要先确定代码已经运行了,再去控制电机吧。
先用个简简单单的demo来测试一下硬件环境