added fight-state-machine. Enter Critical section when read out the ultrasonic should be done.