Final Code

Fixed FSM to center its emergency range with respect to the set point