Finalizations to code

Finalized the code, and added a bunch of comments. Also added some error messages to make error management easier. Ready to rock on Monday!

Report done, final testing done

Finished the report today, and finished the system testing. Some small errors in calibration were found and the necessary changes were made. Also did full system test which worked out well. All thats left to do is the board, and possibly paint the robot if there is time.