refactoring CdqrExpDev, PidControl, and ServoMotor

in addition, modify defines.h and main.c