remove some codes that is not used
changed some time interval
Merge pull request #7 from chiwunau/use_pid_control

Use pid control