update control code
Merge branch 'recording_phase' of git://github.com/zhangchi8620/Multi-Robot-PbD into recording_phase