SparkFun Forums

[esklar81]

Selenaut,

Several, but I'll start with a firm suggestion that you define a few drive functions so my brain doesn't hurt so much and I can follow what you're doing. For example:

Code: Select all

void botForward () {
      digitalWrite(3, HIGH);                           //And these
      digitalWrite(5, LOW);                            //four are to
      digitalWrite(6, LOW);                            //get them to
      digitalWrite(9, HIGH);                           //go forward.
      }
void botStop () {
      digitalWrite(3, LOW);                        //stop both motors
      digitalWrite(5, LOW);
      digitalWrite(6, LOW);
      digitalWrite(9, LOW);
      }
void botRight () {
      digitalWrite(3, HIGH);                       //turns right
      digitalWrite(5, LOW);
      digitalWrite(6, HIGH);
      digitalWrite(9, LOW);
      }

Then, replace each of the blocks of digitalWrite commands with a call to the appropriate drive function. That will make the code much shorter and much easier to read. (When you get around to learning about PWM speed control, you can change each drive function to take a speed argument.)

I also suggest you move the division by 10 inside the heading function. That will shorten the code a bit and reduce the risk that you'll forget to do the division. Note that moving the division inside will change the data type of what the function returns. You can avoid that by rounding or truncating the quotient inside the heading function. (It seems unlikely that tenths of a degree are relevant at this point.)

It's not clear to me what you were trying to do with the multiple turns and how they are supposed to differ from each other. If you've prepared a flow chart or some other description of what you're trying to do, please share it.

Also, you do not appear to have addressed compass wraparound for all of your turns.

Good Luck,
Eric
[/color]

[Selenaut]

To address the forward, stop, left, and right blocks, yes, I will surely do that.

As for the whole point of multiple turns, it is supposed to travel al of the way around the object and continue on in its original driving line, by taking a right turn, two left turns, and then another right turn. (In the beginning, the point was to use the compass readouts and trig to make the process much shorter, along with allowing for multiple obstacles and such, but that's another project entirely .)

For the "dividing by 10" problem, I'll make a general "Heading" float variable to handle this, and put it inside the compass heading function.

The wraparound is a bit dicey, but this gets the job done for all of the turns - somewhat. Like I said, it REALLY overshoots the 3rd turn. I need some help on that - it seems to me like I either got confused in my own reasoning or I just screwed up a number somewhere.

[esklar81]

To address the forward, stop, left, and right blocks, yes, I will surely do that.

If you do that and post the revised code, I'll makde another attempt to understand it.

As for the whole point of multiple turns, it is supposed to travel al of the way around the object and continue on in its original driving line, by taking a right turn, two left turns, and then another right turn. (In the beginning, the point was to use the compass readouts and trig to make the process much shorter, along with allowing for multiple obstacles and such, but that's another project entirely .)

Well, at least I've left "botLeft" as an exercise for the reader.

I still don't understand your "turning1", "turning2", etc. variables. As I understand things, you're always going to execute the 4 turns. If that's the case, why have them executed conditionally?

The wraparound is a bit dicey, but this gets the job done for all of the turns - somewhat.

Check out the "modulo" function. That will allow you to compute, based on the initial heading, the two intermediate headings you need. (It appears that the heading between the second and third turns and the heading after the fourth turn should all be the same as the initial heading.){/color]

Like I said, it REALLY overshoots the 3rd turn. I need some help on that - it seems to me like I either got confused in my own reasoning or I just screwed up a number somewhere

If you still have that problem after making the code changes you've said you'll make, I'll try to help you find the bug. (Making the changes should reduce the risk of errors in "duplicating" parts of your code internally.

Good Luck,
Eric

[Selenaut]

Here's the condensed code - and it is a LOT easier to read now (as well as a good deal shorter). There are only a few more steps to address all of the problems listed thus far.

Code: Select all

#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;
/////////////////////////////////////////////////////////////
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);                                      //(my own code continues here)Just for good measure.
  pinMode(3, OUTPUT);                              //These four
  pinMode(5, OUTPUT);                              //lines of code
  pinMode(6, OUTPUT);                              //are to set
  pinMode(9, OUTPUT);                              //the motors up.
  digitalWrite(13,HIGH);
  headingStart = getHeading() / 10;            //This sets up a variable called headingStart that gives a real heading and saves it as its initial heading.
  if (headingStart >= 270) {
    headingStart = headingStart - 360;
  }
  digitalWrite(13,LOW);
  Serial.println("Setup completed.");
  goForward();
}
/////////////////////////////////////////////////////////////
void loop() {
  irsensed = analogRead(0);                        //Gets distance of object in front of the robot.
  irdistance = irsensed;                           //Doublechecks. I added this in because the sensor sometimes would just return 0.
  if (done == 0) {                                 //fixed this. Its purpose is to make sure it doesn't keep going and going.
    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;                                //this tells itaself that it needs to start turning, hence the name of the variable.
      Serial.println("Beginning turning sequence...");
      range = headingStart + 85;                   //this sets up the threshold for the turn later in the code (see loop).
      digitalWrite(13,LOW);
    }
    if (turning == 1) {                            //If it is beginning its turn
      Stop();
      delay(10);
      goRight();
      headingTurn = getHeading() / 10;
      if (headingTurn >= range) {
        digitalWrite(13,HIGH);
        turning++;
        turning2 = 1;
        goLeft();
        Stop();
        Serial.println("First turn completed.");
        digitalWrite(13,LOW);
        goForward();
        delay(1500);
        Stop();
        digitalWrite(13,HIGH);
      }
    }
    if (turning2 == 1) {
      Stop();
      delay(10);
      goLeft();
      headingTurn = getHeading() / 10;
      if (headingTurn <= headingStart || headingTurn - 360 >= headingStart) {
        turning3 = 1;
        turning2 = 0;
        goRight();
        Stop();
        digitalWrite(13,LOW);
        goForward();
        delay(1500);
        Stop();
        digitalWrite(13,HIGH);
      }
    }
    if (turning3 == 1) {
      Stop();
      delay(10);
      goLeft();
      headingTurn = getHeading() / 10;
      if (headingTurn <= range - 180 || headingTurn >= range + 180) {
        turning4 = 1;
        turning3 = 0;
        goRight();
        Stop();
        digitalWrite(13,LOW);
        goForward();
        delay(1500);
        Stop();
        digitalWrite(13,HIGH);
      }
    }
    if (turning4 == 1) {
      Stop();
      delay(10);
      goRight();
      headingTurn = getHeading() / 10;
      if (headingTurn >= headingStart || headingTurn - 360 >= headingStart) {
        done = 1;
        turning4 = 0;
        goLeft();
        Stop();
        digitalWrite(13,LOW);
        goForward();
        delay(1500);
        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
  Serial.println(float (headingValue / 10));
  return headingValue;
}
/////////////////////////////////////////////////////////////
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]

Oh, and the reason I'm keeping the turns separate is so that it can loop through each one separately; the reason for this is to allow for more than one heading reading for each turn (the only way I can think of this working other than the one I have is to use goto - which I don't like).
BTW, if you get confused as to why the bot goes left, then goes right for a fery short amount of time, and then stops, it is so that the wheels don't "drift" while stopping.

[esklar81]

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

[Selenaut]

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.

[Selenaut]

Aaaannnndddd..... Done.

Code: Select all

#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);
}
  

[esklar81]

Selenaut,

Well, that's a far happier sound than the "AAARRRRRRRRRRRRGGGGHHHH" with which you started this thread!

A few other thoughts and concerns:

1. 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.
2. 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.
3. Slowing the rotation, such as by using the analogWrite function, 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.
4. 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.)
5. If you do use the analogWrite function, 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.
6. 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.
7. 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.
8. 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)
9. 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

[Selenaut]

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.