Obstacle avoiding robot: turning with magnetometer

Hi there, I’m currently working on a project for school. The assignment is to make a robot that can roam around and hit nothing as mentioned in the title. So far I have build the complete robot and I stumbled on a little problem. The turning angle is far to unpredictable so I did some research and found out I could use a magnetometer.

Now my problem is that I’m stuck in my code I’m trying to use the data from the magnetometer to make my robot turn more reliable (about 90°).

Here is my code:

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include <Servo.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>


Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
Adafruit_DCMotor *DC1 = AFMS.getMotor(1);
Adafruit_DCMotor *DC2 = AFMS.getMotor(2);

//unique ID voor de sensor 
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

Servo myServo;
int pin = A0;
int distance = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("Engines ROAR!");
  
  AFMS.begin();
  myServo.attach(10);
  
  //Motor 1
  DC1->setSpeed(215);
  DC1->run(FORWARD);
  DC1->run(RELEASE);
  
  //Motor 2
  DC2->setSpeed(215);
  DC2->run(FORWARD);
  DC2->run(RELEASE);

  if(!mag.begin())
  {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while(1);
  }
}

float getheading(){
  /* Start sensor event */
  sensors_event_t event;
  mag.getEvent(&event);
  
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  float declinationAngle = 0.015;
  heading += declinationAngle;

  /* correctie */
  if(heading < 0){
    heading += 2*PI;
  }
  if(heading > 2*PI){
    heading -= 2*PI;
  }
  /*radiansto graden*/
  return heading * 180/M_PI;
}
void driveForward(){
  DC1->run(FORWARD);
  DC2->run(FORWARD);
  delay(100);
}
void driveBackward(){
  DC1->run(BACKWARD);
  DC2->run(BACKWARD);
  delay(100);
  stopRobot();
}
void stopRobot(void) {
  DC1->run(RELEASE);
  DC2->run(RELEASE);
  delay(300);
}
void driveLeft(float newHeading){
  while(getheading() > newHeading){
  DC1->run(FORWARD);
  DC2->run(BACKWARD);
  }
  stopRobot();
}
void driveRight(float newHeading){
  while(getheading() < newHeading){
  DC1->run(BACKWARD);
  DC2->run(FORWARD);
  }
  stopRobot();
}
void backwardLeft(){
  DC1->run(BACKWARD);
  DC2->run(FORWARD);
  delay(400);
  stopRobot();
}
void backwardRight(){
  DC1->run(FORWARD);
  DC2->run(BACKWARD);
  delay(400);
  stopRobot();
}

void loop() {
  uint8_t i;
  
  float distance = analogRead(pin);
  if(distance > 390){
    stopRobot();
    
    myServo.write(180);
    delay(1000);
    int left_a = analogRead(pin);

    myServo.write(120);
    delay(1000);
    int left_b = analogRead(pin);

    myServo.write(0);
    delay(1000);
    int right_a = analogRead(pin);

    myServo.write(60);
    delay(1000);
    int right_b = analogRead(pin);

    myServo.write(90);

    if(left_a > 450 && left_b > 400){
      backwardRight();
    }
    if(right_a > 450 && right_b > 400){
      backwardLeft();
    }
    if(left_b <= right_b){
      backwardLeft();
    }
    else if(left_b > right_b){
      backwardRight();
    }  
  }
  driveForward();
}

You haven't said what the problem is.

My problem is that I don’t know how to use the data from the sensor here:

float getheading(){
  /* Start sensor event */
  sensors_event_t event;
  mag.getEvent(&event);
  
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  float declinationAngle = 0.015;
  heading += declinationAngle;

  /* correctie */
  if(heading < 0){
    heading += 2*PI;
  }
  if(heading > 2*PI){
    heading -= 2*PI;
  }
  /*radialen naar graden*/
  return heading * 180/M_PI;
}

and use the data to make a turn right or left ( about 90°)

void draaiLinks(float newHeading){
  while(getheading() > newHeading){
  DC1->run(FORWARD);
  DC2->run(BACKWARD);
  }
  stopRobot();
}
void draaiRechts(float newHeading){
  while(getheading() < newHeading){
  DC1->run(BACKWARD);
  DC2->run(FORWARD);
  }
  stopRobot();
}

It would be something like
(pseudocode)

direction = getHeading();

draailinks(direction - 90.0);
or
draaiRechts(direction + 90.0);

You'll need to deal, in draailinks(), with two cases - the robots needs to turn towards a larger heading and the robot needs to turn towards a smaller heading. You'll also need to deal with cases like the robot is heading 320 degrees and needs to be heading 30 degrees. Just determining that the current heading is less then the desired heading is nowhere near good enough.