modify programs for 2 robots

 - add cmd5(for all demo motion)
 - add sp5_2's control program