Broke controller function into pieces

Separate TeleopTick() to call different functions depending on button
state (possibly temporary?). Also added tick count so debug information
is not sent every tick.