Turn to heading, code seems messy

HI there,

I’ve only been programming the arduino for a couple of weeks, but through a bunch of trial and error (mostly error) and reading some of the fantastic posts on this forum I’ve managed to create a simple program that takes a heading (between 0-360) entered by the user through serial and returns a direction to turn (for quickest route) and the amount of degrees needed to turn.

eg -

current heading = 180’
new heading = 200’
Turn anti-clockwise 20’ (quickest route and degrees to turn)

In the future I plan to use this for controlling motors etc.

However, my code seems very messy. Plus, I’ve tried to place the clockwise/anti-clockwise if statements in to functions and I start getting some crazy returns (large negative numbers, mostly).

Any help with tidying things up and putting things in to functions would be so much appreciated!

The code -

int heading = 180; //initial heading value

void setup() 
{
  Serial.begin(9600); //start serial communication at 9600 Baud rate
  Serial.println ("Please enter an Angle to turn to"); // let the user know they have to input an angle 
}

void loop() 
{ 
  while(Serial.available() < 1);
  char buffer[] = {' ',' ',' '};                   // set up the buffer to receive up to 3 ASCII characters 
  Serial.readBytesUntil('\n', buffer, 3);          //read the incoming data until: a new line is detected (enter key presses), or 3 characters read
  int newHeading = atoi(buffer); 
  Serial.println("Heading entered...");  //let the user know where the servo is going
  Serial.println(newHeading);           //let the user know where the servo is going
  
  // assume new and heading are both 0..359 normalized (e.g. compass readings)

  if (heading < newHeading) heading += 360;  // denormalize ...
  int left = heading - newHeading;   // calculate left turn, will allways be 0..359  
  // take the smallest turn
  if (left < 180)   // Anti-Clockwise
  {
  Serial.println("Turn anti-clockwise");
  Serial.println(left);
  heading = newHeading;
  }
  else // Clockwise
  {
  left = left - 360;
  left = left - left*2;
  Serial.println("Turn clockwise");
  Serial.println(left);
  heading = newHeading;
  }
  if (heading < 0){
    heading = heading + 360;
  }
  Serial.println("New heading...");
  Serial.println(heading);
}

Parts like this…

left = left - 360;
  left = left - left*2;

…just seem over long, plus I’m sure that if I tidied up code somewhere else I wouldn’t need to use these lines at all.

One post I tried to draw a lot from was here - http://forum.arduino.cc/index.php?topic=94131.0

Many thanks in advance (and apologies for my sloppy coding skills).

Smirks

I would suggest breaking your code into different functions - each doing just one thing. For example

void loop() {
   getDataFromSerial();
   workOutDirectionToTurn();
}

The Arduino code attached to Reply #4 in this Thread should illustrate the concept.

By separating things it is easy to concentrate on the code for the piece that interests you without being confused by other parts.

...R

Hey Robin2,

Thanks for the advice! I have gone through and set everything I could to a function (which was tough/fun as never really used a function before, so took some researching).

Now I have this -

int heading = 180;
int denormalized = 0;
int turn = 0;

void setup() 
{
  Serial.begin(9600); //start serial communication at 9600 Baud rate
  Serial.println ("Please enter an Angle to turn to"); // let the user know they have to input an angle 
}

void loop() 
{ 
  while(Serial.available() < 1);
  char buffer[] = {' ',' ',' '};                      // set up the buffer to receive up to 3 ASCII characters 
  Serial.readBytesUntil('\n', buffer, 3);             //read the incoming data until: a new line is detected (enter key presses), or 3 characters read
  int newHeading = atoi(buffer); 
  
  printNewHeading (newHeading);                       // calls a function to print the new heading inputed by the user
  deNormer(heading, newHeading);                      // calls the deNormer function, which sets the value of denormalize
  turnDirSet(denormalized, newHeading);               // calls the turnDirSet function, which is used to find the value of turn (itself used to find if the compass should turn clockwise or anticlockwise)
  TurnDirFinder(turn);                                // calls the TurnDirFinder function, which is used to find the direction of turn (clockwise or anti-clockwise)
  
  if (newHeading < 0){                                // if statement to set heading value to the newHeading entered by the user
    heading = newHeading + 360;
  }
  else heading = newHeading;
    
  Serial.println("New heading...");
  Serial.println(heading);
}

void printNewHeading(int newHeading){
  Serial.println("Heading entered...");              //let the user know where the servo is going
  Serial.println(newHeading);                        //let the user know where the servo is going
}

int deNormer(int heading, int newHeading){           // assume new and heading are both 0..359 normalized (e.g. compass readings)
  if (heading < newHeading) {
    return denormalized = heading + 360; 
  } // denormalize ...
    else return denormalized = heading;
}

int turnDirSet(int denormalized, int newHeading){    // set turn value, which is used later to calculate turn directon
  turn = denormalized - newHeading; 
}

int TurnDirFinder(int turn){                         // Finds the direction that the compass should turn to meet the new heading command
  if (turn < 180)                                    // if Anti-Clockwise
  {                                                  
    Serial.println("Turn anti-clockwise");           // Do anti-clockwise action, using turn as the number of degrees to turn
    Serial.println(turn);
  }
  else                                               // if Clockwise
  {
    int heading = turn - 360;                        // Do clockwise action, using turn as the number of degrees to turn
    heading = heading - heading*2;
    Serial.println("Turn clockwise");
    Serial.println(heading);
  }
}

Once I take out the Serial.print lines it seems a fair bit cleaner and will be easier to change later if required, which is nice. I managed to remove and shorten a few lines too.

Two bits that I can’t get my head round though is no matter what approaches I take I can’t put this bit i to a function without it saying in the next Serial.print that heading = 0

if (newHeading < 0){                                // if statement to set heading value to the newHeading entered by the user
    heading = newHeading + 360;
  }
  else heading = newHeading;

I’m obviously missing something obvious (maybe in what data to send or how to return it).

Also -

int heading = turn - 360;                        // Do clockwise action, using turn as the number of degrees to turn
    heading = heading - heading*2;

Is there a shorter/cleaner way of writing that equation?

Any ideas?

Many thanks,

Smirks

int heading = turn - 360;                        // Do clockwise action, using turn as the number of degrees to turn
    heading = heading - heading*2;

change to

int heading = 360 - turn ;

since a-2a == -a

And

if (newHeading < 0){                                // if statement to set heading value to the newHeading entered by the user
    heading = newHeading + 360;
  }
  else heading = newHeading;

change to

  heading = newHeading < 0 ? newHeading + 360 : newHeading ;

ie use a conditional expression.

You might want a normalise function:

int normalize (int angle)
{
  while (angle < 0)
    angle += 360 ;
  while (angle >= 360)
    angle -= 360 ;
  return angle ;
}

Which normalizes any degree angle to the range 0…359

great, you reduced your 42 lines of code down to 61. :smiley:

Two bits that I can’t get my head round though is no matter what approaches I take I can’t put this bit i to a function without it saying in the next Serial.print that heading = 0

you may wish to review the scope of your variables. If you are using them in different functions, like you wish to, they must be declared globally.

if (newHeading < 0){                                // if statement to set heading value to the newHeading entered by the user
    heading = newHeading + 360;
  }
  else heading = newHeading;

newHeading is of local scope to the loop() function.

BulldogLowell: great, you reduced your 42 lines of code down to 61. :D

lol! Aye, good point. I'm still tweaking and bringing the count down, but heh yea is longer. I guess I've increased length slightly but also decreased my future confusion and made it cleaner? I hope... ;)

You were right about the variable being declared in the wrong place. Fixed it and now the function works a treat!

Markt -

Thanks a lot for your advice. I made the changes you suggest and they work greatt, although I have actually been able to remove the 'if statement' regarding the heading altogether as it was fixing an error I had made in older code that is now not there.

Now I can simply put...

heading = newHeading;

Do you suggest the normalise function to catch incorrect user inputs? Or is it an important piece of code to add full stop?

Many thanks once again for all the help! What a great community!

PS - now down to 46 lines of code XD

smirkza: Do you suggest the normalise function to catch incorrect user inputs? Or is it an important piece of code to add full stop?

good idea, yeah and another thing to practice with.

learning functions is crucial, and you did that well, so a few extra lines invested now pay back dividends in your future projects!

I’d do it like this:

int heading = 180; //initial heading value

void setup() {
  Serial.begin(9600); //start serial communication at 9600 Baud rate
  Serial.println ("Please enter an Angle to turn to"); // let the user know they have to input an angle 
}

int normalize(int degrees) {
  // If it's negative, add 360 until it isn't
  while (degrees < 0)
    degrees += 360;
  // If it's > 359 use modulo to restrict it to 0-359
  degrees %= 360;
  return degrees;
}

void loop() {
  int newHeading;
  int leftTurnDegrees;
  char buffer[4]; // buffer to receive up to 3 ASCII characters and a null terminator

  Serial.readBytesUntil('\n', buffer, 3);  //read the incoming data until: a new line is detected (enter key presses), or 3 characters read
  newHeading = normalize(atoi(buffer)); 

  Serial.println("Heading entered...");  //let the user know where the servo is going
  Serial.println(newHeading);           //let the user know where the servo is going

    // assume new and heading are both 0..359 normalized (e.g. compass readings)
  leftTurnDegrees = normalize(heading - newHeading);   // calculate left turn, will allways be 0..359  

  // take the smallest turn
  if (leftTurnDegrees < 180) {  // Anti-Clockwise
    Serial.println("Turn anti-clockwise");
    Serial.println(leftTurnDegrees);
  }
  else  { // Clockwise
    Serial.println("Turn clockwise");
    Serial.println(360 - leftTurnDegrees);
  }

  heading = newHeading;
  Serial.println("New heading...");
  Serial.println(heading);
}