Basic functionality for mode switch.

And removed loop4051 variable, now also uses b.