Update floor_controller.cpp