ARRRGG!! --Code! (FR05 Head Sweep)
June 17, 2008
Man am I stumped...
What I want to do is pretty simple, but I just can't seem to figure out which direction to go - I have a FR05 on top of a servo in the standard "look right/ look left" arrangement. I also have an encoder on one of my wheels so the robot knows how far it has turned (the code for the encoder is solid and works great). I want the head to do a sweep of a room and decide which direction is the farthest away and then turn the body that way. --As the body is turning, I want the head to rotate the opposite way so the head seems to stay still while the body turn under it.
I have the code for the encoder
I can make the servo turn, no problem...
How do I code the "what is the farthest away" thing??!?
So far I have tried having the servo stop in 5 different spots, take a reading and then I have about 900 lines of "if w1>w2 then b1=1 : if w2>w1 then b1=2 : if w3>w1 then b1=3 : if w1 > w3 then b1=1 : if w3>w2 then b1=3 : if w2>w3 then b1=2..........." etc. This code would probably work but it is a mess, WAY too long and uses a ton of available variables. There is got to be an easier way! In addition, it would be great that, instead of the servo taking reading at only a few spots it would be awesome if I could tie the distance readings in with the actual servo positions. --For example: for sweep = 75 to 175 step 1, take reading, next sweep --I was also thinking the FR05 could take a reading every step and then just compare the reading to the last reading it took --The head might have to do a few sweeps back and forth to "zero in" on the fartherst away spot but I could do it with just 2 variables used over and over.
I'm not looking to be a Code-Mooch (have someone write it for me) I am just looking for a new way to look at this problem...