Nesten ferdig med control_functions.c og floor_control.c