Магнитный компас.

HMC5883L compass;

define LM1 7 // Левый мотор 1 контакт
define LM2 6 // Левый мотор 2 контакт
define RM1 5 // Правый мотор 1 контакт
define RM2 4 // Правый мотор 2 контакт
define BUTTON_PIN 2 // кнопка пуска
define LED_PIN 13
define SENS_LEFT 8 // левый сенсор 8 контакт
define SENS_RIGH 9 // правый сенсор

boolean buttonWasUp = true; // была ли кнопка отпущена?
boolean workEnabled = false; // работают ли движки?
boolean ledEnabled = false; // включен ли свет?
float heading;

void setup() {
pinMode(LM1, OUTPUT);
pinMode(LM2, OUTPUT);
pinMode(RM1, OUTPUT);
pinMode(RM2, OUTPUT);
pinMode(LED_PIN, OUTPUT);
pinMode(BUTTON_PIN, INPUT_PULLUP);
pinMode(SENS_LEFT, INPUT_PULLUP);
pinMode(SENS_RIGH, INPUT_PULLUP);
Serial.begin(9600);
Wire.begin();

compass = HMC5883L(); // создаем экземпляр HMC5883L библиотеки
setupHMC5883L(); // инициализация HMC5883L

}

void setupHMC5883L(){
// инициализация HMC5883L, и проверка наличия ошибок
int error;
error = compass.SetScale(0.88); // чувствительность датчика из диапазона: 0.88, 1.3, 1.9, 2.5, 4.0, 4.7, 5.6, 8.1
if(error != 0) Serial.println(compass.GetErrorText(error)); // если ошибка, то выводим ее

error = compass.SetMeasurementMode(Measurement_Continuous); // установка режима измерений как Continuous (продолжительный)
if(error != 0) Serial.println(compass.GetErrorText(error)); // если ошибка, то выводим ее
}

float getHeading(){
// считываем данные с HMC5883L и рассчитываем направление
MagnetometerScaled scaled = compass.ReadScaledAxis(); // получаем масштабированные элементы с датчика
float heading = atan2(scaled.YAxis, scaled.XAxis); // высчитываем направление

//* корректируем значения с учетом знаков
if(heading < 0) heading += 2*PI; if(heading > 2*PI) heading -= 2*PI;

return heading* RAD_TO_DEG; // переводим радианы в градусы
}

void povorot(float azimut, int ugol) // функция поворота по компасу
{
if (ugol < 0) { if (azimut + ugol < 0) {azimut = azimut + ugol + 360;} do { float heading = getHeading(); } while (heading <= azimut); } else { if (azimut + ugol > 360) {azimut = azimut + ugol - 360;}
do
{
float heading = getHeading();
} while (heading >= azimut);
}
}
//------------------STOP---------------------
void stop_motors() // Функция остановки моторов
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, LOW);
digitalWrite(RM1, LOW);
digitalWrite(RM2, LOW);
}
//---------------FORWARD-----------------------
void forward() // Функция движения ВПЕРЁД
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, HIGH);
digitalWrite(RM1, HIGH);
digitalWrite(RM2, LOW);
}
//----------------BACK------------------------
void back() // Функция движения НАЗАД
{
digitalWrite(LM1, HIGH);
digitalWrite(LM2, LOW);
digitalWrite(RM1, LOW);
digitalWrite(RM2, HIGH);
}
//----------------LEFT------------------------
void left() // Функция поворота ВЛЕВО
{
digitalWrite(LM1, HIGH);
digitalWrite(LM2, LOW);
digitalWrite(RM1, HIGH);
digitalWrite(RM2, LOW);
}
//----------------RIGHT------------------------
void right() //Функция поворота ВПРАВО
{
digitalWrite(LM1, LOW);
digitalWrite(LM2, HIGH);
digitalWrite(RM1, LOW);
digitalWrite(RM2, HIGH);
}
//-------------------------------------------
void loop()
{
float heading = getHeading();
Serial.println(heading);
delay(500);

// определить момент «клика» несколько сложнее, чем факт того,
// что кнопка сейчас просто нажата. Для определения клика мы
// сначала понимаем, отпущена ли кнопка прямо сейчас...
boolean buttonIsUp = digitalRead(BUTTON_PIN);

// ...если «кнопка была отпущена и (&&) не отпущена сейчас»...
if (buttonWasUp && !buttonIsUp) {
// ...может это «клик», а может и ложный сигнал (дребезг),
// возникающий в момент замыкания/размыкания пластин кнопки,
// поэтому даём кнопке полностью «успокоиться»...
delay(10);
// ...и считываем сигнал снова
buttonIsUp = digitalRead(BUTTON_PIN);
if (!buttonIsUp) { // если она всё ещё нажата...
// ...это клик! Переворачиваем переменную работы
workEnabled = !workEnabled;
ledEnabled = !ledEnabled;
digitalWrite(LED_PIN, ledEnabled);
}
}

// запоминаем последнее состояние кнопки для новой итерации

buttonWasUp = buttonIsUp;