Selenaut,
Fundamentally, I’m still puzzled by your flow control. However, you may just have given me a critical hint. I don’t see how your code could cause each of the turn subroutines to loop independently. My interpretation of the [Reference is that the instructions in {braces} after an IF statement are executed once if the condition is TRUE and not at all if the condition is FALSE. The looping in your code appears to include all of what’s in the “loop” function.
If you want each turn to loop independently (and that might cut down the overshoot, as the position checks would be more frequent (assuming the compass is fast enough)), you need to use a looping flow control, such as WHILE or DO…WHILE, for each of the turns. I don’t think you need to make the execution of the loops dependent on some turn number, but they do need to depend on the target heading not having been reached.
Also, I think you’ll have a problem if the initial heading is in the range of 270 to (274 or 275). Subtracting 360 and adding 85 will give you a “range” of -5 to (-1 or 0). No matter which way the compass is pointing, you should always get a value that’s at least 0. (I don’t know whether your compass returns 0 or 360 if it is pointed at magnetic north.) While you’re fixing this, why not save a step and just calculate the range, if the initial heading is => 270, as initial heading - 270? If you don’t want to use modulo, how about:
if (initialHeading =>270) then {targetHeading = initialHeading - 270} else {targetHeading = initialHeading + 90}
Have Fun,
Eric
](Arduino - Home)
Thanks for the idea! I can’t believe I didn’t think of that. This makes it easier for me (let alone everyone else) to understand it. I’ll get to coding, and post it when I’m done. 
Aaaannnndddd… Done.
#include <Wire.h>
float range;
float range2;
float range3;
int irdistance = 1;
int irsensed = 1;
byte turning = 0;
byte turning2 = 0;
byte turning3 = 0;
byte turning4 = 0;
int HMC6352Address = 0x42;
int slaveAddress;
byte headingData[2];
int i, headingValue;
float headingStart;
float headingTurn;
byte done = 0;
float Heading;
/////////////////////////////////////////////////////////////
void setup() {
pinMode(13, OUTPUT);
Wire.begin();
Serial.begin(9600);
Serial.print("Beginning setup.");
digitalWrite(13, HIGH);
Serial.print(".");
delay(10);
digitalWrite(13, LOW);
Serial.println(".");
//Note: the following four lines of code are not mine.
// Shift the device's documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
slaveAddress = HMC6352Address >> 1; // This results in 0x21 as the address to pass to TWI
delay (50);
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
digitalWrite(13,HIGH);
headingStart = getHeading();
if (headingStart >= 270) {
range = headingStart - 270;
}
else {
range = headingStart + 90;
}
if (headingStart >= 180) {
range2 = headingStart - 180;
}
else {
range2 = headingStart + 180;
}
if (headingStart >= 90) {
range3 = headingStart - 90;
}
else {
range3 = headingStart + 270;
}
digitalWrite(13,LOW);
Serial.println("Setup completed.");
goForward();
}
/////////////////////////////////////////////////////////////
void loop() {
irsensed = analogRead(0); //Gets distance of object in front of the robot.
irdistance = irsensed;
if (done == 0) {
if (irdistance >= 488 && irdistance <= 536 && turning == 0) { // if an object is about 20 cm away and turning is not true
digitalWrite(13,HIGH);
turning = 1;
Serial.println("Beginning turning sequence...");
digitalWrite(13,LOW);
}
}
while (turning == 1) {
goRight();
headingTurn = getHeading();
if (headingTurn >= range) {
digitalWrite(13,HIGH);
turning++;
turning2 = 1;
goLeft();
Stop();
Serial.println("First turn completed.");
digitalWrite(13,LOW);
goForward();
delay(500);
Stop();
digitalWrite(13,HIGH);
}
}
while (turning2 == 1) {
goLeft();
headingTurn = getHeading();
if (headingTurn <= headingStart) {
turning3 = 1;
turning2 = 0;
goRight();
Stop();
digitalWrite(13,LOW);
goForward();
delay(500);
Stop();
digitalWrite(13,HIGH);
}
}
while (turning3 == 1) {
goLeft();
headingTurn = getHeading();
if (headingTurn <= range3) {
turning4 = 1;
turning3 = 0;
goRight();
Stop();
digitalWrite(13,LOW);
goForward();
delay(500);
Stop();
digitalWrite(13,HIGH);
}
}
while (turning4 == 1) {
goRight();
headingTurn = getHeading();
if (headingTurn >= headingStart) {
done = 1;
turning4 = 0;
goLeft();
Stop();
digitalWrite(13,LOW);
goForward();
delay(500);
Stop();
digitalWrite(13,HIGH);
delay(1000);
digitalWrite(13,LOW);
}
}
}
/////////////////////////////////////////////////////////////
int getHeading() {
Serial.print("getting heading.");
Wire.beginTransmission(slaveAddress);
Serial.print(".");
Wire.send("A"); // The "Get Data" command
Serial.print(".");
Wire.endTransmission();
Serial.print(".");
delay(10); // The HMC6352 needs at least a 70us (microsecond) delay
// after this command. Using 1ms just makes it safe
// Read the 2 heading bytes, MSB first
// The resulting 16bit word is the compass heading in 10th's of a degree
// For example: a heading of 1345 would be 134.5 degrees
Serial.print(".");
Wire.requestFrom(slaveAddress, 2); // Request the 2 byte heading (MSB comes first)
Serial.print(".");
i = 0;
Serial.print(".");
delay(5);
while (Wire.available() && i < 2)
{
Serial.print(".");
headingData[i] = Wire.receive();
Serial.print(".");
i++;
Serial.println(".");
Serial.println("Heading found.");
}
headingValue = headingData[0]*256 + headingData[1]; // Put the MSB and LSB together
Heading = headingValue / 10;
Serial.println(float (Heading));
return Heading;
}
/////////////////////////////////////////////////////////////
void goForward() {
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
}
void goRight() {
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
}
void Stop() {
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
}
void goLeft() {
digitalWrite(3, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
}
Selenaut,
Well, that’s a far happier sound than the “AAARRRRRRRRRRRRGGGGHHHH” with which you started this thread!
A few other thoughts and concerns:
- It probably doesn’t matter (because you don’t appear to use the result), but it appears that you calculate range2 to be the opposite direction from the original heading. To avoid confusion, I suggest you delete the range2 calculation.
- Have you tested this with a variety of initial compass headings? I'm still not comfortable with the wraparound. If you were to change the condition at which the turn is ended from:
currentHeading > targetHeading {or, if turning left: currentHeading < targetHeading}
to:
(currentHeading > (targetHeading-5)) && (currentHeading < (targetHeading + 5))
I believe that would address the problem near 360. To improve the precision, I would try substituting, in sequence, “4”, “3”, “2”, and “1” for the “5” until I found that the bot was overshooting and making an additional rotation (or more than 1) to hit the specified range.
- Slowing the rotation, such as by using the [[analogWrite function](http://arduino.cc/en/Reference/AnalogWrite), would reduce the chance of overshoot. The issue is that the compass takes some time to read, so the code has to get to the read at the same time the bot is in the target range. If you wanted to get fancy, you could make the rotation speed a function of the difference between the present and target locations (that, by the way, is how a servo works), so the robot would start at a high rotational speed, then decelerate as it approached its target.
- Another way to address overshoot is to turn briefly (say, whatever time it takes for the robot to turn ~5 degrees), stop, read the compass, and, depending on the reading, move forward or make another ~5 degree turn.)
- If you do use the [[analogWrite function](http://arduino.cc/en/Reference/AnalogWrite), I believe you should use it only for the pins you would otherwise have set HIGH. That way, you have a pulsed power signal on that pin and the other pin is a constant ground.
- Despite having encouraged you to put the division by 10 inside the heading function, I now suggest you consider whether it would be easier to refrain from doing it at all. There's no physical law mandating using a 0-360 range for headings. You could just use 0-3600 and then this would all be integer arithmetic. That would not only save you some memory (integers are smaller than floats), it would remove some calculations and reduce the time it takes for others. It would also allow the use of the modulo function, which is limited to integers. Rather than using the if... then... else statements for determining target headings, you could then use modulo.
- Your flow control is now closer to what I had in mind, but not quite there. If I have some time in the near future, I'll try to do a better job of communicating my idea.
- Perhaps as a matter of tradition*, I would use boolean, rather than byte, variables for things that are either true or false. According to the Reference, a boolean variable occupies a byte of memory, so the memory usage is the same. Using a boolean, however, would simplify some things. For example, instead of:
if(turning == 1)
you could use
if(turning)
- Speaking of "turning", is there some reason you increment "turning" when the first turn is completed, but reset "turning2", "turning3", and "turning4" to 0 when those turns are completed?
- We're only a few hours from the end of the fifth calendar decade in which I've programmed, although I admit it wasn't until the last couple of years of the '60s that I learned. One advantage of having started that long ago is that I have a well-developed appreciation of the value of being stingy with memory allocation and considering the relative execution times of various approaches.
Have Fun,
Eric
](analogWrite() - Arduino Reference)](analogWrite() - Arduino Reference)
Thanks for all of the help so far! Sorry I haven’t replied in a while, I’ve been working on some other things. I will surely try the wraparound idea, as well as the other ideas.