I have this crazy problem, I don't know why I couldn't come with a proper solution.
Story: I want to turn the robot to a some degree (30 degrees) according to sensor readings. for example if right is blocked turn 30 degrees left and check if front is free move forward else check if left is free turn 30 degrees left.
I am using a compass to read the current bearing. if moving right add 30 degress to current compass reading and turn right until new compass reading is between previous comapss reading + 30 and previous compass reading + 35.
Why 35? because, the compass reading is not accurate and it always gives one or two degrees up and down, for example: if the angle is 45, sometimes it gives 46 or 47 or 44. so, I am not worried about 5 degrees up and down.
if moving left. read current bearing and subtract 30 degrees from current bearing. turn left until new compass reaches our new angle.
here is the code.
void turnRightTheRobot(int angle){
int currentBearing = getBearing();
int newBearing = currentBearing + angle;
if(newBearing > 359) {
newBearing -= 359;
}
while(1) {
int x = getBearing();
if(x >= newBearing && x <= newBearing +5) {
moveForward(leftMotorSpeed, rightMotorSpeed );
break;
}
else {
turnRight(leftMotorSpeed, rightMotorSpeed);
}
}
}
the problem raises here
if(x >= newBearing && x <= newBearing +5)
for example---> if the current compass reading is 350, want to turn 30 degrees to right, the compass reading should be between 20 and 25 to stop. However with my solution 350 is always greater than 20 and never smaller than 25. I am stuck here.
I appreciate if you guys can help me. PLEASE.
PS. I have also tried with OR like
if(x == newBearing || x == newBearing +1 || x == newBearing +2 || x == newBearing +3 || x == newBearing +4)
Didn't work. :(. some times it turns and works fine, but mostly turns and never stops
As I said, if the current bearing is greater than 330, it will turn right infinity, because the new bearing would be 1-30 degrees and it is always smaller than previous compass reading (greater than 330).
if (x >= newBearing && x <= newBearing +5)
is there any other option to rethink about the above code.
It seems that I solved the problem, just wanted to post my code for some people facing same problem,
I don't know whether this approach is right or not, but it works. I hope it does not affect other parts of my application.
you are more than welcome to comment and suggest a better approach.