Go Down

Topic: Calculate difference in heading direction to course direction?? (Read 777 times) previous topic - next topic

agz

Hi all,


I'm having a bit of trouble wrapping my head around what seems like a simple task.


I have a GPS module and a compass module supplying me with:

- The direction I'm facing
&
- The course direction to an input location vs my location

What I'm wanting to do is update the position of a motor+encoder to point me in the direction of the input location whilst being aware of the direction it is already facing.

For example if I'm facing 180 degrees south, but need to be heading 310 degrees, the motor will point in the direction that will get me on course.

I have the motor already being able to rotate to its position based on a compass reading, the problem im facing is determining the course correcting direction based on the two inputs (my direction vs location course direction).

Any guidance is very much appreciated, thanks all.

wildbill

I'm not sure I understand the problem. You know the course to the desired destination and you can tell the motor to point to a particular bearing, so it seems that that's enough.

Is the problem that you can only set the motor position relative to the boat's heading?

jremington

It is simple enough to calculate the heading error, as follows. Don't forget to correct for compass wrap.
Code: [Select]

// routine to calculate heading error in degrees, taking into account compass wrap

int heading_error(int bearing, int current_heading)
{
 int error = current_heading - bearing;
 if (error >  180) error -= 360;
 if (error < -180) error += 360;
 return error;
}

// routine to correct for compass wrap

int wrap360(int direction) {
while (direction > 359) direction -= 360;
while (direction <   0) direction += 360;
return direction;
}


But you haven't told us how the motor/encoder works, or is connected to the rest of the system.

agz

Thank you for the replies,

The problem I think is a wildbill has said, if I input the course direction to update the motor say 300 NW but it is not aware that I am facing 180 S, the motor will update to what it thinks is NW but really be something like SE due to my facing direction.

I'm a bit unsure if what jremington has posted is the solution to my issue, can someone please confirm?



agz

Here is my current code which is set to update the motor position based on my facing direction.

Code: [Select]

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Servo.h>

static const uint32_t GPSBaud = 115200;
static const int RXPin = 8, TXPin = 9;

Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);
Servo servo_test;        //initialize a servo object for the connected servo

int pinFeedback = 5;

float tHigh = 0;
float tLow = 0;
int tCycle = 0;
float dc = 0;
float angle = 0; //Measured angle from feedback --- Servo Feedback
float dcMin = 2.9; //From Parallax spec sheet
float dcMax = 97.1; //From Parallax spec sheet
float Kp = .7; //Proportional Gain, higher values for faster response, higher values contribute to overshoot.
float Ki = .2; //Integral Gain, higher values to converge faster to zero error, higher values produce oscillations. Higher values are more unstable near a target_angle = 0.
float iLimit = 5; //Arbitrary Anti-wind-up
float Kd = 1; //Derivative Gain, higher values dampen oscillations around target_angle. Higher values produce more holding state jitter. May need filter for error noise.
float prev_error = 0;
float prev_pError = 0;
float error = 0;
float pError = 0;
float iError = 0;
int headingDegrees;

unsigned long targetMillis = 0;
const long intervalQuick = 850;
const long intervalSlow = 5000;
long intervalCalculated = 0;



void setup()
{
  Serial.begin(9600);
  ss.begin(GPSBaud);

  delay(2000);

  Serial.println();
  Serial.println(F("Sats HDOP  Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("           (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to Melbourne  ----  RX    RX        Fail"));
  Serial.println(F("----------------------------------------------------------------------------------------------------------------------------------------"));
}

void loop()
{
  static const double MELB_LAT = -37.814, MELB_LON = 144.96332;

  printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
  printFloat(gps.hdop.hdop(), gps.hdop.isValid(), 6, 1);
  printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
  printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
  printInt(gps.location.age(), gps.location.isValid(), 5);
  printDateTime(gps.date, gps.time);
  printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
  printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
  printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
  printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.deg()) : "*** ", 6);

  unsigned long distanceKmToMelbourne =
    (unsigned long)TinyGPSPlus::distanceBetween(
      gps.location.lat(),
      gps.location.lng(),
      MELB_LAT,
      MELB_LON) / 1000;
  printInt(distanceKmToMelbourne, gps.location.isValid(), 9);

  double courseToMelbourne =
    TinyGPSPlus::courseTo(
      gps.location.lat(),
      gps.location.lng(),
      MELB_LAT,
      MELB_LON);

  printFloat(courseToMelbourne, gps.location.isValid(), 7, 2);

  const char *cardinalToMelbourne = TinyGPSPlus::cardinal(courseToMelbourne);

  printStr(gps.location.isValid() ? cardinalToMelbourne : "*** ", 6);

  printInt(gps.charsProcessed(), true, 6);
  printInt(gps.sentencesWithFix(), true, 10);
  printInt(gps.failedChecksum(), true, 9);
  Serial.println();

  displayCompassInfo();

  smartDelay(1000);

  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));

  unsigned long currentMillis = millis();
  bool updateGPS = (currentMillis >= targetMillis);

  // only read the GPS/Compass periodialy, to give the servo more processing time
  if (updateGPS) {
    targetMillis = currentMillis + intervalCalculated;

    displayCompassInfo();  // updates heading degrees
  }


  // update the servo
  moveServo( headingDegrees );


  float differenceFactor = abs( headingDegrees - angle ) / 360;  // 180 degrees should be the maximum value that the compass and servo can differ by, to scale this between 0 and 1
  intervalCalculated = intervalSlow * differenceFactor + intervalQuick * (1 - differenceFactor);

}


static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do
  {
    while (ss.available())
      gps.encode(ss.read());
  } while (millis() - start < ms);
}

static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i = flen; i < len; ++i)
      Serial.print(' ');
  }
  smartDelay(0);
}

static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i = strlen(sz); i < len; ++i)
    sz[i] = ' ';
  if (len > 0)
    sz[len - 1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}

static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }

  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}

static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i = 0; i < len; ++i)
    Serial.print(i < slen ? str[i] : ' ');
  smartDelay(0);
}


void displayCompassInfo()
{

  sensors_event_t event;
  mag.getEvent(&event);


  float newHeading = atan2(event.magnetic.y, event.magnetic.x);


  float declinationAngle = 0.2027;
  newHeading += declinationAngle;

  // Correct for when signs are reversed.
  if (newHeading < 0)
    newHeading += 2 * PI;

  // Check for wrap due to addition of declination.
  if (newHeading > 2 * PI)
    newHeading -= 2 * PI;

  // Convert radians to degrees for readability.
  headingDegrees = newHeading * 180 / M_PI;

  Serial.println("");
  Serial.println("");
  Serial.println("");
  Serial.println("Heading (degrees): "); Serial.println(headingDegrees);

  Serial.println("");
  Serial.println("");
  Serial.println("");

  delay(500);
}


void moveServo(int targetAngle) {
  //  Serial.println(targetAngle);

  while (1) //From Parallax spec sheet
  {
    tHigh = pulseIn(pinFeedback, HIGH);
    tLow = pulseIn(pinFeedback, LOW);
    tCycle = tHigh + tLow;
    if ( tCycle > 1000 && tCycle < 1200)
    {
      break; //valid tCycle;
    }
  }

  dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, you are trying to determine the percentage of the HIGH in the pulse

  angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

  //Keep measured angles within bounds
  if (angle < 0)
  {
    angle = 0;
  }
  else if (angle > 359)
  {
    angle = 359;
  }

  if (targetAngle < 0)
  {
    targetAngle = 360 + targetAngle; //handles negative targetAngles;
  }

  error = targetAngle - angle;

  if (error > 180)
  {
    error = error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
  }
  if (error < -180)
  {
    error = 360 - error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
  }

  // PID controller stuff, Adjust values of Kp, Ki, and Kd above to tune your system
  float pError = Kp * error;
  float iError = Ki * (error + prev_error);

  if  (iError > iLimit)
  {
    iError = iLimit;
  }
  if (iError <  -iLimit)
  {
    iError = -iLimit;
  }

  prev_error = error;
  float dError = Kd * (pError - prev_pError);
  prev_pError = pError;

  error = error / 2; //max 180 error will have max 90 offset value

  int val = 93 - (Kp * error) - iError - dError; // 93 is the middle of my servo's "no motion" dead-band

  servo_test.write(val); //Move the servo
}


Delta_G

Thank you for the replies,

The problem I think is a wildbill has said, if I input the course direction to update the motor say 300 NW but it is not aware that I am facing 180 S, the motor will update to what it thinks is NW but really be something like SE due to my facing direction.
But you said:

Quote
I have a GPS module and a compass module supplying me with:

- The direction I'm facing
&
- The course direction to an input location vs my location
So if the GPS module can tell you which direction you are facing then how is the thing going to get confused about which direction it is facing? 
|| | ||| | || | ||  ~Woodstock

Please do not PM with technical questions or comments.  Keep Arduino stuff out on the boards where it belongs.

jremington

GPS can't tell you which direction you are facing, but a compass can, if it is properly calibrated. They never are, out of the box.

GPS can tell which direction you are traveling, and that requires movement in a particular direction.

Who knows what the motor and encoder are doing. The OP hasn't bothered to inform us.

agz

Sorry jremington I thought the code might of cleared up what the motor is doing.

The motor is only taking an input (angle) rotating to that particular position and holding that position until the angle changes in which case it will then update its position, so essentially it is just pointing in a direction.

It is not part of a robot/ rover, boat or anything like that so it not controlling wheels, a sail, or anything else other than it's own position.


If there is any other information that you want to clear up my issue please tell me what would help and I will try post it.

Thanks all

agz

I've attached a diagram of the full (complete) circuit, the only thing missing is my servo has 4 pin outs the additional pin being a feedback line for the hall effect encoder (attached to pin 5).

I have a feeling that part of the confusion may be that motor might be part of a larger system or interact with a series of other motors or controls, but really it's not doing any of that. The only output is the position of the motor.


jremington

Your posts are missing some really fundamental issues that are required to understand the problem.

How does the motor/encoder know which direction you are facing?

How does the rest of the system know which direction the dial on the motor/encoder is pointing? In other words, what does it mean that the dial is pointing at 0 degrees?

What is the compass connected to, or is it free to point in any direction?

Have you calibrated the compass in place? If not, the readings are most likely garbage.

The diagram is useless, because it does not reveal the mechanical connections required to explain the points above. That said, you MUST NOT power any motor or servo from the Arduino 5V output. It won't work well, if at all, and you can even destroy the Arduino doing that.

wildbill

I'm assuming that you can position your motor/encoder relative to the boat and that zero degrees points it to the bow. Given that, I think this tells you where to point it:

Code: [Select]
int MotorBearing(int CurrentDirection,int DesiredDirection)
{
      return (360-(CurrentDirection-DesiredDirection)) %360;
}

agz

Right, I'm beginning to understand where I have failed to clarify my issue, thank you elaborating on that.


Ill attach some images of a temporary testing rig and updated code.


Quote
How does the motor/encoder know which direction you are facing?
My current system is a handheld format so based on the direction that it is being held will change the direction of the compass module which is located and fixed in place behind the motor.

Quote
How does the rest of the system know which direction the dial on the motor/encoder is pointing? In other words, what does it mean that the dial is pointing at 0 degrees?
This comes from the feedback pin and is accessed through the variable 'angle'.

Quote
What is the compass connected to, or is it free to point in any direction?
The compass module and motor are both fixed within the current temporary enclosure and is free to point based on how/ what direction it's held.

Quote
Have you calibrated the compass in place? If not, the readings are most likely garbage.
I updated the code to calibrate it, tested it to another compass and readings are fairly accurate.


Quote
That said, you MUST NOT power any motor or servo from the Arduino 5V output. It won't work well, if at all, and you can even destroy the Arduino doing that.
Yes, my circuit diagram was wrong, my apologies.

 

agz

Code: [Select]

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <Servo.h>

static const uint32_t GPSBaud = 115200;
static const int RXPin = 8, TXPin = 9;



HMC5883L compass;
TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);
Servo servo_test;        //initialize a servo object for the connected servo

int pinFeedback = 5;

float tHigh = 0;
float tLow = 0;
int tCycle = 0;
float dc = 0;
float angle = 0; //Measured angle from feedback --- Servo Feedback
float dcMin = 2.9; //From Parallax spec sheet
float dcMax = 97.1; //From Parallax spec sheet
float Kp = .7; //Proportional Gain, higher values for faster response, higher values contribute to overshoot.
float Ki = .2; //Integral Gain, higher values to converge faster to zero error, higher values produce oscillations. Higher values are more unstable near a target_angle = 0.
float iLimit = 5; //Arbitrary Anti-wind-up
float Kd = 1; //Derivative Gain, higher values dampen oscillations around target_angle. Higher values produce more holding state jitter. May need filter for error noise.
float prev_error = 0;
float prev_pError = 0;
float error = 0;
float pError = 0;
float iError = 0;
int headingDegrees;

unsigned long targetMillis = 0;
const long intervalQuick = 850;
const long intervalSlow = 5000;
long intervalCalculated = 0;



void setup()
{
  Serial.begin(9600);
  ss.begin(GPSBaud);
  
  while (!compass.begin())
  {
    Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
    delay(500);
  }

  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(132, 320);

  delay(2000);

  Serial.println();
  Serial.println(F("Sats HDOP  Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("           (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to Melbourne  ----  RX    RX        Fail"));
  Serial.println(F("----------------------------------------------------------------------------------------------------------------------------------------"));
}

void loop()
{
  static const double MELB_LAT = -37.814, MELB_LON = 144.96332;

  printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
  printFloat(gps.hdop.hdop(), gps.hdop.isValid(), 6, 1);
  printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
  printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
  printInt(gps.location.age(), gps.location.isValid(), 5);
  printDateTime(gps.date, gps.time);
  printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
  printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
  printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
  printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.deg()) : "*** ", 6);

  unsigned long distanceKmToMelbourne =
    (unsigned long)TinyGPSPlus::distanceBetween(
      gps.location.lat(),
      gps.location.lng(),
      MELB_LAT,
      MELB_LON) / 1000;
  printInt(distanceKmToMelbourne, gps.location.isValid(), 9);

  double courseToMelbourne =
    TinyGPSPlus::courseTo(
      gps.location.lat(),
      gps.location.lng(),
      MELB_LAT,
      MELB_LON);

  printFloat(courseToMelbourne, gps.location.isValid(), 7, 2);

  const char *cardinalToMelbourne = TinyGPSPlus::cardinal(courseToMelbourne);

  printStr(gps.location.isValid() ? cardinalToMelbourne : "*** ", 6);

  printInt(gps.charsProcessed(), true, 6);
  printInt(gps.sentencesWithFix(), true, 10);
  printInt(gps.failedChecksum(), true, 9);
  Serial.println();

  displayCompassInfo();

  smartDelay(1000);

  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));

  unsigned long currentMillis = millis();
  bool updateGPS = (currentMillis >= targetMillis);

  // only read the GPS/Compass periodialy, to give the servo more processing time
  if (updateGPS) {
    targetMillis = currentMillis + intervalCalculated;

    displayCompassInfo();  // updates heading degrees
  }


  // update the servo
  moveServo( headingDegrees );


  float differenceFactor = abs( headingDegrees - angle ) / 360;  // 180 degrees should be the maximum value that the compass and servo can differ by, to scale this between 0 and 1
  intervalCalculated = intervalSlow * differenceFactor + intervalQuick * (1 - differenceFactor);

}


static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do
  {
    while (ss.available())
      gps.encode(ss.read());
  } while (millis() - start < ms);
}

static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i = flen; i < len; ++i)
      Serial.print(' ');
  }
  smartDelay(0);
}

static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i = strlen(sz); i < len; ++i)
    sz[i] = ' ';
  if (len > 0)
    sz[len - 1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}

static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }

  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}

static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i = 0; i < len; ++i)
    Serial.print(i < slen ? str[i] : ' ');
  smartDelay(0);
}


void displayCompassInfo()
{
 Vector norm = compass.readNormalize();

  // Calculate heading
  float newHeading = atan2(norm.YAxis, norm.XAxis);

  
  float declinationAngle = 0.2027;
  newHeading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (newHeading < 0)
  {
    newHeading += 2 * PI;
  }

  if (newHeading > 2 * PI)
  {
    newHeading -= 2 * PI;
  }

  // Convert to degrees
  headingDegrees = newHeading * 180/M_PI;

 
  Serial.print(" Degress = ");
  Serial.print(headingDegrees);
  Serial.println();

  delay(100);
}


void moveServo(int targetAngle) {
  //  Serial.println(targetAngle);

  while (1) //From Parallax spec sheet
  {
    tHigh = pulseIn(pinFeedback, HIGH);
    tLow = pulseIn(pinFeedback, LOW);
    tCycle = tHigh + tLow;
    if ( tCycle > 1000 && tCycle < 1200)
    {
      break; //valid tCycle;
    }
  }

  dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, you are trying to determine the percentage of the HIGH in the pulse

  angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

  //Keep measured angles within bounds
  if (angle < 0)
  {
    angle = 0;
  }
  else if (angle > 359)
  {
    angle = 359;
  }

  if (targetAngle < 0)
  {
    targetAngle = 360 + targetAngle; //handles negative targetAngles;
  }

  error = targetAngle - angle;

  if (error > 180)
  {
    error = error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
  }
  if (error < -180)
  {
    error = 360 - error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
  }

  // PID controller stuff, Adjust values of Kp, Ki, and Kd above to tune your system
  float pError = Kp * error;
  float iError = Ki * (error + prev_error);

  if  (iError > iLimit)
  {
    iError = iLimit;
  }
  if (iError <  -iLimit)
  {
    iError = -iLimit;
  }

  prev_error = error;
  float dError = Kd * (pError - prev_pError);
  prev_pError = pError;

  error = error / 2; //max 180 error will have max 90 offset value

  int val = 93 - (Kp * error) - iError - dError; // 93 is the middle of my servo's "no motion" dead-band

  servo_test.write(val); //Move the servo
}

agz


agz

#2

wildbill, with the info I just posted do you think your suggestion can be the answer?

Go Up