Added the collisiontest which still seems to be working just fine (except this time without too many boost / brake calls) though still needs some tinkering.