Polish step code

Improve steps to not end in long hallway
Optimize turning code

Improved efficiency of setFacing routine
Add lower chamber

Added lower chamber