Made some headway on software - there are some large gaps in communicating with the drive system, no integration with IMU, and lack of loop process to iterate through points with PID - BUT! most of the initialization and handling of data is completed