电机控制改为两个，并增加控制逻辑

电机控制改为两个，并增加控制逻辑，修正超声测距暂存数组下标的错误