comments and code formatting
bug about sensor is fixed
PD Control