working on pid controller
fixed estop update