improved controller2, should be ready for intersection integration