bug fix and add position controller\nalmost finished