Added motor kill, added timeouts for movements

Also simplified the calculateMovements function.