Additional obstacle avoidance bits

PID renamed to match sonar names
RC input variables added to workloop