Added PIDController and implemented straight driving along rps coordinate. untested but compiles