Made some changes, now only CAN-functionality is tested.