changed some things around, made logic more concise, changed branching flow so that driving always happens on new forward and then it saves for next loop