Integrate Lidar drivers into program

[TytanRock]

This will probably need to be done for #4.

https://github.com/De-La-Salle-Robotics/Robot-2019/commit/0ffe0943491e68aecb6fac7a5512e5d326c7c6cb writes the drivers

What needs to be done:

• [x] Write code for drivers
• [x] Test code for drivers