Actually fixed code

The last commit had a mistake and this commit actually fixes the program
as well as advancing slightly on the micromaster design