Well, I thought the radar was well, but I spent 2 days witha error. The error was about the angles, it always went to 270º even if I change that value, after 3 days of this, I noticed that I was using =+ and not += so, that was all the stuff. Now it works well.
An advice for everyone, be carfeull with this! it sounds idiot, but anyone who is not full concentrated or with many noise arround may make this mistake and is difficult to find once written.