Control logic tested, nearly works

The wall following seems to need some tuning.