Проверьте пожалуйста мой склепаный из нескольких чужих скетч

Ребята проверьте что я тут налепил.... скетчь компилируется но еще не проверял в железе. особенно интересует грамотно ли получаю данные с магнитометра и далее тело программы до моторов.

Код (Text):

// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>
#include "I2Cdev.h"
// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

// Out setup routine, here we will configure the microcontroller and compass.

int IN3 = 5;
int IN4 = 4;
int ENB = 3;
int sensor;              // переменная для хранения показания сенсора
int dir = 180;  // заданное направление
int tol = 20;  // допустимая точность удерживания направления

void setup()
{
  pinMode (ENB, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN4, OUTPUT);
 
// Initialize the serial port.
Serial.begin(9600);

Serial.println("Starting the I2C interface.");
Wire.begin(); // Start the I2C interface.

Serial.println("Constructing new HMC5883L");
compass = HMC5883L(); // Construct a new HMC5883 compass.
 
Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3); // Set the scale of the compass.
if(error != 0) // If there is an error, print it out.
  Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
  Serial.println(compass.GetErrorText(error));
}

// Our main program loop.
void loop()
{
// Retrive the raw values from the compass (not scaled).
MagnetometerRaw raw = compass.ReadRawAxis();
// Retrived the scaled values from the compass (scaled to the configured scale).
MagnetometerScaled scaled = compass.ReadScaledAxis();
// Values are accessed like so:
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(scaled.ZAxis, scaled.XAxis);
// Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
// Mine is: 2? 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
float declinationAngle = 0.0457;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
  heading += 2*PI;
 
// Check for wrap due to addition of declination.
if(heading > 2*PI)
  heading -= 2*PI;
 
// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;

// Output the data via the serial port.
Output(raw, scaled, heading, headingDegrees);

// Normally we would delay the application by 66ms to allow the loop
// to run at 15Hz (default bandwidth for the HMC5883L).
// However since we have a long serial out (104ms at 9600) we will let
// it run at its natural speed.
// delay(66);
  //int heading = compass.heading();  // вынимаем из показаний величину направление компаса
  if (abs(heading - sensor)>320) sensor = heading; // отсекаем проблемный переход из 359° в 1°
  sensor = (int)digitalLowPass(sensor, heading, 0.7);  // отфильтровываем показания компаса (сглаживаем)
  int dif = sensor - dir;    // вычисляем разницу между реальным направлением и заданным
  int absdif = abs(dif);
  if (absdif <= tol) Stop();        // если разница меньше допустимой погрешности, то останавливаемся
    else if (((absdif > 180) && (dif != absdif)) || ((absdif < 180) && (dif == absdif))) TurnLeft();  // иначе определяем кратчайшее направление поворота
          else TurnRight();
//  delay(25);
 
} // конец основной программы, начинаем все сначала

//
// Подпрограммы
//
// Цифровой низкочастотный фильт
double digitalLowPass(double last_smoothed, double new_value, double filterVal)
{
  return (new_value * (1 - filterVal)) + (last_smoothed * filterVal);
}


// Output the data down the serial port.
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  Serial.print("Raw:\t");
  Serial.print(raw.XAxis);
  Serial.print("  ");
  Serial.print(raw.YAxis);
  Serial.print("  ");
  Serial.print(raw.ZAxis);
  Serial.print("  \tScaled:\t");
 
  Serial.print(scaled.XAxis);
  Serial.print("  ");
  Serial.print(scaled.YAxis);
  Serial.print("  ");
  Serial.print(scaled.ZAxis);

  Serial.print("  \tHeading:\t");
  Serial.print(heading);
  Serial.print(" Radians  \t");
  Serial.print(headingDegrees);
  Serial.println(" Degrees  \t");
 
 
  }
 
  void Stop(){  // подпрограмма Остановка
      digitalWrite (IN3, LOW);
      digitalWrite (IN4, LOW);
      analogWrite(ENB,0);
      delay(200);
}

void TurnRight(){ // подпрограмма Поворот направо
      digitalWrite (IN3, HIGH);
      digitalWrite (IN4, LOW);
      analogWrite(ENB,255);
      delay(200);
}

void TurnLeft(){ // подпрограмма Поворот налево
      digitalWrite (IN3, LOW);
      digitalWrite (IN4, HIGH);
      analogWrite(ENB,255);
      delay(200);
}

 

Для начала переведите и причешите комментарии, сам код и опишите алгоритм и состав оборудования.
Думаю, никто этот хлам разгребать не будет. Уважайте тех к кому обращаетесь за помощью.

 

ок попробую)))