Just finished first pass at debugging. Cleaned up quite a bit of code and debugged with the specific intent to make wall tracking work. Found at least one bug where we had hard-coded z-components.