Trouble with 90° turn an compass heading

Hello @ all
Here ist my Program:

// Libraries

#include <Wire.h>
#include <CMPS10.h>
#include <NewPing.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Create instance of digital compass
CMPS10 compass;

// Motor pins
#define CMPS10Addr  0x60 // Addresse des CMPS10
#define leftMtrDirPin1 12// Motor Block A
#define leftMtrDirPin2 9
#define leftMtrSpdPin 3

#define rightMtrDirPin1 13 //Motor Block B
#define rightMtrDirPin2 8
#define rightMtrSpdPin 11

#define SONAR3_TRIGGER_PIN  26
#define SONAR3_ECHO_PIN     27
#define SONAR3_MAX_DISTANCE 50

NewPing sonar3(SONAR3_TRIGGER_PIN, SONAR3_ECHO_PIN, SONAR3_MAX_DISTANCE); // Sensor links

#define SONAR2_TRIGGER_PIN  28
#define SONAR2_ECHO_PIN     29
#define SONAR2_MAX_DISTANCE 50

NewPing sonar2(SONAR2_TRIGGER_PIN, SONAR2_ECHO_PIN, SONAR2_MAX_DISTANCE); // Sensor mitte

#define SONAR1_TRIGGER_PIN  30
#define SONAR1_ECHO_PIN     31
#define SONAR1_MAX_DISTANCE 50

NewPing sonar1(SONAR1_TRIGGER_PIN, SONAR1_ECHO_PIN, SONAR1_MAX_DISTANCE); // Sensor rechts

LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);  // Set the LCD I2C address

// Variables for direction control
float initial_heading;
float target_heading;
float diff_heading;
bool turn_init;
bool target_reached;

//Servo Initialisierung
Servo servo1;        //Servo Objekte erzeugen
Servo servo2;

const int servoPin1 = 33; // Servo vaagrechte bewegung
int servoPos1 = 70;

const int servoPin2 = 34; // Servo senkrechte bewegung
int servoPos2 = 140;

//Ultraschallsensoren initialisierung
const byte TRIGGER1 = 26; //Sensor Links
const byte TRIGGER2 = 28; // Sensor mitte
const byte TRIGGER3 = 30; // Sensor rechts

const byte ECHO1 = 27; //sensor links
const byte ECHO2 = 29; //sensor mitte
const byte ECHO3 = 31; //sensor rechts

// Left & right
#define LEFT 0
#define RIGHT 1

void setup(void) 

{
  // Start Serial
  Serial.begin(9600);
  Wire.begin(); // Connect to I2C
  lcd.begin(16, 2);
   
     servo1.attach(servoPin1); // Servo waagerecht
    servo2.attach(servoPin2);
 
   // Declare motor pins as outputs

  pinMode(leftMtrSpdPin, OUTPUT); // initialiesierung Motor Kanal A
  pinMode(leftMtrDirPin1, OUTPUT);    
  pinMode(leftMtrDirPin2, OUTPUT);
  pinMode(rightMtrDirPin1, OUTPUT);// initialiesierung Motor Kanal B
  pinMode(rightMtrDirPin2, OUTPUT);
  pinMode(rightMtrSpdPin, OUTPUT);  

  // Initialise direction variables
  turn_init = false;
  target_reached = false;

 }

void loop(void) 

{
  

    delay(30);
    unsigned int uS1 = sonar1.ping(); //Sensor rechts
    //Serial.print("Ping: ");
    //Serial.print(uS1 / US_ROUNDTRIP_CM);
    //Serial.println("cm");
    uS1 = uS1 / US_ROUNDTRIP_CM;
    if (uS1 == 0) {
        uS1 = 50;
    }

    delay(30);
    unsigned int uS2 = sonar2.ping(); // Sensor mitte
    //Serial.print("Ping: ");
    //Serial.print(uS2 / US_ROUNDTRIP_CM);
    //Serial.println("cm");
    uS2 = uS2 / US_ROUNDTRIP_CM;
    if (uS2 == 0) {
        uS2 = 50;
    }

    delay(30);
    unsigned int uS3 = sonar3.ping(); // Sensor links
    //Serial.print("Ping: ");
    //Serial.print(uS3 / US_ROUNDTRIP_CM);
    //Serial.println("cm");
    uS3 = uS3 / US_ROUNDTRIP_CM;
    if (uS3 == 0) {
        uS3 = 50;
    }
 
    // Drehung begrenzen
    if (servoPos1 < 20) servoPos1 = 20;
    if (servoPos1 > 140) servoPos1 = 140;
    if (servoPos2 < 100) servoPos2 = 100;
    if (servoPos2 > 170) servoPos2 = 170;

    // Position der Servos setzen
    servo1.write(servoPos1);
    delay(20);
    servo2.write(servoPos2);
    delay(20);
 
   // Nach Rechts
  if (uS3 < 16) {

    turn(90, RIGHT);

  }
   // Nach Links
  if (uS1 < 16) {

    turn(90, LEFT);

  }



}



// Turn of a given angle
void turn(float angle, bool turn_direction) {

  // Get initial heading
  if (!turn_init) {
    Serial.println("Starting turn ...");
    initial_heading = get_heading();


    if (turn_direction) {
     target_heading = initial_heading + angle;

    }

    else {
      target_heading = initial_heading - angle;
    }

if (target_heading > 359){                          //korrectur der 360°
  target_heading = target_heading-360;
}

    Serial.print("Initial heading: ");
    Serial.println(initial_heading);
    Serial.print("Target heading: ");
    Serial.println(target_heading);

    turn_init = true;
    target_reached = false;
    diff_heading = 0;

  }

   float heading = get_heading();


  if (turn_direction) {
    diff_heading = target_heading - heading;
    

}
  else {
    diff_heading = heading - target_heading;
  }
  
  
  
  while (diff_heading > 0 && !target_reached) {

    // Move & stop
    if (turn_direction) {
        digitalWrite(leftMtrDirPin1, LOW); //Nach Rechts drehen
      digitalWrite(leftMtrDirPin2, LOW);  
      digitalWrite(rightMtrDirPin1, HIGH);
      digitalWrite(rightMtrDirPin2, LOW);
      analogWrite(leftMtrSpdPin, 155);   
      analogWrite(rightMtrSpdPin, 155);
    }
    else {
          digitalWrite(leftMtrDirPin1, HIGH); // Nach links drehen
      digitalWrite(leftMtrDirPin2, LOW);  
      digitalWrite(rightMtrDirPin1, LOW);
      digitalWrite(rightMtrDirPin2, LOW); 
      analogWrite(leftMtrSpdPin, 155);   
      analogWrite(rightMtrSpdPin, 155);
         
    }
    delay(150);
    analogWrite(leftMtrSpdPin, 0);   
      analogWrite(rightMtrSpdPin, 0);
    delay(50);

    

    // Measure heading again
    float heading = get_heading();




    if (turn_direction) {
      
      diff_heading = target_heading - heading;
    }
    else {
      diff_heading = heading - target_heading;
    }
    
  if (diff_heading >359){                        //mein code
    
    diff_heading = diff_heading-360;
  }                                              //mein Code Ende
    
   Serial.print("Difference heading (degrees): "); Serial.println(diff_heading);
   //lcd.setCursor(0, 0);
   //lcd.print("Difference heading ");
   //lcd.setCursor(0, 1);
   //lcd.println(diff_heading);
   
    if (diff_heading < 0 && !target_reached) {
      target_reached = true;
      turn_init = false;
      diff_heading = 0;
    }

  }

  // Stop
  Serial.println("Stopping turn ...");
}

// Get heading from the compass
float get_heading() {
 
     /////////////////////////////////////////////////////////////
    //     Kompass Modul Abfrage und senden der Daten         ///
    ////////////////////////////////////////////////////////////

    byte byteHigh, byteLow; // byteHigh / byteLow für Bearing
    char pitch, roll;       // Pitch und Roll
    int bearing;            // Bearing

    Wire.beginTransmission(CMPS10Addr); // Kommunikation mit CMPS10
    Wire.write(2);                      // Start Register (2)
    Wire.endTransmission();
    Wire.requestFrom(CMPS10Addr, 4); // Abfrage von 4 Bytes vom CMPS10
    while (Wire.available() < 4) ; // Warten, bis 4 Bytes verfügbar
    byteHigh = Wire.read();       // High-Byte für Bearing speichern
    byteLow = Wire.read();        // Low-Byte für Bearing speichern
    pitch = Wire.read();          // Byte für Pitch speichern
    roll = Wire.read();           // Byte für Roll speichern
    bearing = ((byteHigh << 8) + byteLow) / 10; // Bearing berechnen


   sendData(bearing, pitch, roll);             // Daten versenden
  
  
  float headingDegrees = bearing;
 
  return headingDegrees;

}

void sendData(int bearing, int pitch, int roll) {
    String data = String(bearing) + ";" + String(pitch) + ";" + String(roll);

    lcd.setCursor(0, 0);
    lcd.print("bear-pitch-roll");
    lcd.setCursor(0, 1);
    lcd.print(data);

    // Serial.println(data);
    delay(100);
}

Unfortunetly I have a problem with the heading value of my compass module CMPS 10 (full Tilt compensated Module ) .

My robot ist turn Pefect the Pos. à 90 ° only then when the heading is between 0 an 270 ° (360°- 90° = max up 270° )

If I am, or better say, my compass heading higher than 270° the target is geting out oft he range ( for example heading = 286 + 90 = 376 ° ?!! )
The result is I can never reach the target position and the robot is endless turning . So I have to turn him off 

What can i do to compensate the target „error“ in my code ?
Ok i can compensate the error with:

If (target_heading >359){
target _heading=target_heading-360;
}

But in this case is the diff_heading out of the range…

You see with solving my first problem i get anothen one … and so far…

What can i do to fix the problem and turning my vehicle also above the heading bigger then 270° ?
I hope you can help me.

greatings
Chris

If I am, or better say, my compass heading higher than 270° the target is geting out oft he range ( for example heading = 286 + 90 = 376 ° ?!! )

Since you can't get a reading from the compass of 376 degrees, you can't know you are heading in the correct direction.

But, you CAN get a reading of 376 - 360 = 16 degrees. So, that is the heading you should be trying to reach.

I don't understand the problem with diff_heading. If you are facing 286, and want to be facing 16, the difference is 16 - 286 = -270, which, being negative, needs to be subtracted from 360, giving 90.

void loop(void)

{
 

    delay(30);

If you are trying to build a clumsy, slow robot, you are off to a good start. Make it slower and clumsier by increasing the delay() value.

Do you understand the issue ok you are not alone :slight_smile:

But this is exactly what the program do especially in the Fuktion :
Turn of a given Angle.

target_heading = initial_heading (current heading) + angle ( here 90°) what happens when i get 300° +90° ?
exactly i get 390° this is unrechable for my vehicle that correct.

So i can fix it when i use following Code :
If (target_heading >359){
target _heading=target_heading-360;
}
So far ok

But now i get a issu with the Function - turn_direction

Why ?

here ist the Code : diff_heading = heading (current heading (if i was started by 300° ...- exactly 300°++)) -target_heading
So when i fixed it with the correction Code i get this 359°++ = 0° and so on

But for example the current Position is 300° + 90° we get 30° correct ? correct :wink: in this case ist the target_heading = 30°
But ... and here it Comes 30°-300°(still my heading Position) = -270° !!!

If you look für the Programm you will see here ist my Problem then i can not go <0 ...

Ohh the other one "Problem" with the delay (30); it is a dirty Version of my Program but thanks for finding the mistake :wink: 1 Point für you

KR

Chris

target_heading = initial_heading (current heading) + angle ( here 90°) what happens when i get 300° +90° ?
exactly i get 390° this is unrechable for my vehicle that correct.

You should get 30, since the orientation value rolls over at 360.

It really isn't that difficult to have a block of code that deals with a +n degree turn that results in still being less than 360, a -n degree turn that results in still being above 0, a +n turn that goes over 360, and a -n turn that goes below 0. In the crossing situations, break the turn into two parts. Turn from m up to 360 (or down to 0). Then turn from 0 up to m+n, or from 360 down to m-n.

That looks dangerous to me: if you make a right turn to 360 and then start the 'next' turn to 1 then maybe the compass is showing 359 at that instant. It's going to try to turn 358 degrees left.

Add 720 to everything. Then it never wraps around past zero. If you find that you've made 2 left circles and you're getting close to 0, add 720 to everything again. Update your target point and the current point at the same time so they're always in the same 720 range.