Replaced TeleopPeriodic() call in TestPeriodic()

Fixes double-ticking issue during test mode.
Also, formatting cleanup.