PID + direction
change pid calculate way
merge