Add some data logging to enable system identification

[jkleiber]

Summary
Having data of maneuvers would be helpful for us for system modeling. This could be another way of determining the feedforward parameters in #10.

The objective here is mainly to make it possible to collect data. Once we collect data we can do curve fitting in python to characterize the system.

Work Scope

• Add data logging to each subsystem where tuning is of interest (drive, wrist, pivot, telescope)

Verification

• Code review
• Robot testing

@FujiBookPro assigning this to you since you mentioned that you're interested in simulation.