Updated controller
Using xacro to generate maze more easily