wip - completed avoid obstacle behavior

First working version of Tribot with Avoid Obstacle Behavior.
The PID currently only increases in its output value, it does not
reduce the output value despite having a low error value.