Made some logical changes. Trying code on robot after this.