Newbie needs help with arduino code

@chip

I've been developing with mutiple languages for a living so it's just a question of acquired skills and habits over the years.

And crunch, munch thanks for the munch treat ! ? 

Let's look at this piece of code :

1 int x;
2 if (x >= 1 ) {
3   if (x <=6 && x >=0 ){
4     //soft turn one way
5     }
6   if (x>=5){
7     //hard turn one way
8     }
9   }

a) First, a little cleanup :

As soon as you are in the block (lines 3-9) you know that x>=1, so at line 3 the "x >=0" test is useless because it is mandatorily true : you can get rid of it and change line 3 to just "if (x <=6) {".

Then the bug :

Supposing you want a soft turn for x = 1 to 5, and a hard turn for x = 6 or more :

b) The first condition test "if (x <=6) {"at line 3 is wrong because it will enable the soft turn also for x=6 : you have to use :

• either "if (x<=5) {"
• or "if (x < 6) {"

c) The second test "if (x>=5){" at line 6 is also wrong because it will enable the hard turn also for x=5 : you have to use :

• either "if (x > 5) {"
• or "if (x >= 6) {"

So that would give for example :

1 int x;
2 if (x >= 1 ) {
3   if (x <=5 ){
4     //soft turn one way
5     }
6   if (x > 5){
7     //hard turn one way
8     }
9   }

But a better way of "isolating" the 2 branches (soft vs. hard turn), which actually are  mutually exclusive, would be to use "else" like this for example :

1 int x;
2 if (x >= 1 ) {
3   if (x <=5 ){
4     //soft turn one way
5     }
6   else {
7     //hard turn one way
8     }
9   }

And you could also code it like that :

1 int x;
2 if (x > 5 ) {
3     //hard turn one way
4 }
5 else if (x >= 1 ) {
6     //soft turn one way
7 }

Does all that make sense ?

So to correct the bug you have 2 places to edit : for justshy... and for justover...

...and now....

?Bug-hunting exercise 2 :

Taking into account what happens in your main loop(), what will chip do if it's current actual heading differs from your desired Dheading by 2 (heading = Dheading + 2 or -2), and how does that fit with the code above ?

Posted by: @chip

... look at what I have been working on with the headingDelta and tell me what you think, I still have bugs turning left but I am getting close I think.

void alignToCompass()
{

  int headingDelta = (Dheading - heading);
  ==>>> int turnDelay = (headingDelta * 50); <<<== too soon !
  headingDelta is still a "raw" value between -359 and +359
  => the delay will be negative for a left hard turn
  if (headingDelta == 0) {
    return;
  }
  if (headingDelta > 180) { //true
    headingDelta = headingDelta - 360;
  }
  if (headingDelta < -180) { //false
    headingDelta = headingDelta + 360;
  }

  ==>>> At this point, headingDelta has been "normalised" to -180 to +180, this is where 
  i would put the code to setup the criteria of the following tests, for example :<<==
  bool bTurnRight = (headingDelta > 0); // false = turn left, true = turn right
  headingDelta = abs(headingDelta); //now headingDelta is always between +1 and +180
  int turnDelay = (headingDelta * 50); //now your delay is positive  ? 

  if (...etc...
code for left/right and soft/hard turns

Posted by: @zeferby

Bug-hunting exercise 2 :

Taking into account what happens in your main loop(), what will chip do if it's current actual heading differs from your desired Dheading by 2 (heading = Dheading + 2 or -2), and how does that fit with the code above ?

  htolset();
  if (htol > 3) {
    aligntocompass();
  }

I think this is the Bug I am looking for, seems as if (heading = Dheading +2 or -2) then we never go to the aligntocompass function unless it is above 2 + or - 2 . So ... I need to change this to ...

if (htol > 1) {

aligntocompass();

}

Correct ?

@chip

Re: bug 2

Yes with that code the aligntocompass() function will only be called for a delta/htol of 4 degrees or more.

if (htol > 3) {
    aligntocompass();
  }

Actually I think the htolset() code suffers from the "big right turn instead of small left one" syndrom.

You probably can just always call the aligntocompass() function and get rid of the htol variable (and the htolset() function), because i don't see it used anywhere else in your program.

That way you would already have a small adjustment as soon as you get headingDelta = 1

But if you want to keep a "no alignment for desired heading+/- X" feature, you can also #define a minimum value of the delta, used in aligntocompass() to exit if headingDelta < HEADING_LOW_THRESHOLD (remember to do this after the headingDelta is made positive !)

Re: bug 1 correction

The code in post 5824 looks good.  Just do a bit of cleanup in the beginning: this block

  if (justshyofheading  < 10) { //check if shy is less than 10, if so..
    if (justshyofheading < 0) {// check if it is below zero
      justshyofheading = justshyofheading - justshyofheading; // set it ot zero
    }
  }
  if (justoverheading  < 10) { // same as above
    if (justoverheading  < 0) {
      justoverheading = justoverheading - justoverheading;
    }
  }

is equivalent to :

  if (justshyofheading < 0) {// check if it is below zero
      justshyofheading = 0; // set it ot zero
    }
  if (justoverheading  < 0) {
      justoverheading = 0;
    }

Posted by: @chip

At the present time I only use the right encoder to determine distance travelled, you may notice some small variations in my headings on this map and that is due to magnetic interference from things like my refrigerator, stove and in floor metal heating duct work.

Have you tried dead reckoning just using the encoders?

Hopefully I will have some code making use of the HMC5883 IC to see how well it works at allowing the robot to travel in a straight line and I guess a computed curve in a more complex manoeuvre.

The magnetic interference seems to be a real issue?

Can you navigate the path using the sonar?

Posted by: @zeferby

About the compass values, instead of discriminating against the innocent 0 heading, you probably should check if the sensor event read is valid : as seen on the sensor library code, getEvent returns a bool value that you maybe sometimes could catch as false.

https://github.com/adafruit/Adafruit_LSM303DLHC/blob/ccc4e70113460333a70926e08150d2d13a32c35b/Adafruit_LSM303_U.h#L181

https://github.com/adafruit/Adafruit_LSM303DLHC/blob/ccc4e70113460333a70926e08150d2d13a32c35b/Adafruit_LSM303_U.cpp#L435

 

After  looking at both cpp.h files am I correct in assuming the second one is a modified version of the first one and might resolve my zero heading issue, and if so would I just replace my ccp.h file with this second one?

@chip

No the first one is the .h file that you #include at the beginning of your sketch :

#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);

It is a file containing the declarations that your program uses when you create the sensor object and when your code uses the sensor (begin, take readings, etc...)

The second file is the actual source code of the library.  You have absolutely nothing to do there as  it is automatically "linked" into your sketch when the Arduino IDE compiles your sketch, but you can read the source code to better understand how it works.

But my point was that the getEvent() function is declared as a boolean, so you should probably test if the result is true or false, and maybe keep the previous value if the function returns false, or retry a reading ?

For example :

int compassheading() {
  tft.println("in compassheading function");
  int newHeading;
  bool bSensorReadOk;

  /* Get a new sensor event */
  sensors_event_t event;
  bool bSensorReadOk = mag.getEvent(&event);

  if (bSensorReadOk) {
    // Calculate the angle of the vector y,x
    newHeading = (atan2(event.magnetic.y, event.magnetic.x) * 180) / M_PI;
    // Normalize to 0-360
    if (newHeading < 0)
    {
      newHeading = 360 + newHeading ;
      }//if (newHeading < 0)

    //Sensor was read Ok, let's update the heading global variable
    heading = newHeading;
    }//if (bSensorReadOk)
  else {
  //Sensor was NOT successfully read, we keep the previous value of heading intact
  //tft.print something ?
  }
  //always return the heading value (updated or not, depending on success above)
  return heading;
}

Beware I cannot test this as I don't have such sensor.

Posted by: @casey

Have you tried dead reckoning just using the encoders?

Yes I did try that and found it nearly impossible due to rpm's of both motors always are slightly different. I don't have PID motor controllers, those might help.

Posted by: @casey

Hopefully I will have some code making use of the HMC5883 IC to see how well it works at allowing the robot to travel in a straight line and I guess a computed curve in a more complex manoeuvre.

Thats great be sure to post your code so i can snatch it 🙂  🙂

Posted by: @casey

The magnetic interference seems to be a real issue?

Actually it's not that big a deal, I just change my heading to compensate for it.

Posted by: @casey

Can you navigate the path using the sonar?

It might be possible although my main goal was to get the compass working well enough go exactly where I tell it and I am happy to report that it is working GREAT ! *smiles*.

Here is my current ino if you would like to see the changes I made.

Thanks for your interest Casey.

Posted by: @zeferby

Beware I cannot test this as I don't have such sensor.

The code seems to be working with no problem.

Eric I really don't know how to thank you enough for all your help and guidance , the headingDelta code is working great, chip can turn cw & ccw now *smiles* and it stays exactly on course.

Here is todays ino file if you wan't to look it over.

@chip

Nice, now all you have to do is teach chip to open the fridge and bring the beer can to you !

Posted by: @chip

Thanks for your interest Casey.

Well you have a medium sized robot up and running so we can talk about more than just our hardware collection.

With reference to your statement that you just change your heading to compensate for magnetic interference may I suggest that an autonomous robot will not have its path hard coded. All you should need to do is assign the starting point and end point.  The robot then works out a path to take which it can also adapt if unexpected obstacles appear while carrying out the task. All this planning would be best done in the RPi leaving the Arduino with the simpler functions. If the robot has created a map of the house it can use a simple algorithm planning a path but not so simple to fit on the Arduino.