Finished findAngle(), added Robot.h