Added switch reading to get position # and store that in registers, modifed a couple other subroutines