Cleaned up initializing radar states into own function.  Moved initialize radar and movment states into intialize function. Slightly more readible. Fixed blocking call problem in radar state 0.