Future diagnostics

As my last act of being on the team I merge the self-diagnosing robot code written by Kyle and I into the main code.