初步重构了代码
重构完成，还没测试的
将舵机程序模块化，得到了颜色程序