Adds a testcase to check if heading & speed is right. Program unusable for the moment.