Arduino.ru

Приветствую всех!
Один из моих новых проектов машина с компасом, а в будущем может и с GPS.
Данная система идеально подходит и для робота. Код написан примитивным и понятным “языком”. Одна из основных проблем с которой я столкнулся - работа компаса.
Он правильно показывает когда стоит ровно по горизонтали и вертикали.., как только его наклон меняется, меняется и показания самого компаса.. С помощью кода я пробовал как можно лучше исправить эту проблему, но все же результат оставляет желать лучшего..
Нашел хорошую статью (https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-...) в которой описывается устранение этой проблемы с помощью дополнительного модуля(https://www.loveelectronics.co.uk/products/136/3-axis-accelerometer---ad...).
Но может есть способы обойтись без него?

Что делает код:
Я управляю rc машиной с помощью пулта(в лево, в право,вперед и стоп). Когда я выбрал мною желаемое направление, и больше не трогаю руль управления, код начинает свою работу.
Он знает в каком направление ему надо ехать, и в каком он идет сейчас. Сравнивает эти данные и корректирует направление машины. То есть если машина сошла с направления влево, идет сигнал из arduino в серво - поворачивай направо и так далее.

Используемые мною компоненты:
компас – HMC5883L
Arduino UNO

Жду ответов от всех у кого есть мысли как улучшить код!
P.S. Почему MSP а не ESP потому что Magnetic Stabilization Program а не Electronic Stabilization Program как у VW или AUDI:)

#include <Servo.h> 
#include <Wire.h>
#include <HMC5883L.h>
Servo rudder; 
HMC5883L compass;
int error = 0;
unsigned long RCrudder = 6;
int currentdirection;
int maindirection;
int state = 1;
int firststandart = 0;
int zstandart = 0;
int zlimit = 5;
int P = 5;
int left = 20;
int right = 160;
int forward = 90;
long rcrudderneutral;

void setup()
{
  pinMode (RCrudder, INPUT);
  Serial.begin(9600);
  rudder.attach(3);  
  Serial.println("Starting the I2C interface.");
  Wire.begin(); 
  Serial.println("Constructing new HMC5883L");
  compass = HMC5883L(); 
  Serial.println("Setting scale to +/- 1.3 Ga");
  error = compass.SetScale(1.3); 
  if(error != 0) 
  Serial.println(compass.GetErrorText(error));
  Serial.println("Setting measurement mode to continous.");
  error = compass.SetMeasurementMode(Measurement_Continuous); 
  if(error != 0) 
  Serial.println(compass.GetErrorText(error));
}


void loop()
{
if(state == 1) {
////////////////calibration///////////////////////////
compas();
zstandart = firststandart;
RCrudder = (pulseIn (6, HIGH, 100000))/10;
rcrudderneutral = RCrudder;
Serial.println("calibration");

//////////////////////////////////////////////////////  
if(millis() >= 5000) {
state = 0;  
}
}
else
{
RCrudder = (pulseIn (6, HIGH, 100000))/10;
if(RCrudder >= (rcrudderneutral-5) && RCrudder <= (rcrudderneutral+5)) {  
pidcontroll();
}
else if(RCrudder > (rcrudderneutral+6) || RCrudder < (rcrudderneutral-6))
{
maualcontroll();
}
}
}


void pidcontroll()
{
compas();
if ((currentdirection - maindirection > P) || (maindirection - currentdirection > P)) {

if (currentdirection > maindirection) 
  {
  if((currentdirection-maindirection) < 180)
    rudder.write(right);
  else
    rudder.write(left);
  }
else 
  {
  if((maindirection-currentdirection) < 180)
    rudder.write(left);
  else
    rudder.write(right);
  }
}
else
{
rudder.write(forward);
}

}


void maualcontroll()
{
  RCrudder = map(RCrudder, 97, 209, 0, 179);
  rudder.write(RCrudder);
  compas();
  maindirection = currentdirection;
}

void compas()
{
    // 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(raw.YAxis, raw.XAxis);
   
  // Your mrad result / 1000.00 (to turn it into radians).
  float declinationAngle = 96.87  / 1000.0;
  // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
  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; 
  
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 

firststandart = raw.ZAxis;
if(state == 0) {
if((raw.ZAxis >= zstandart - zlimit) && (raw.ZAxis <= zstandart + zlimit)) { //zstandart 515
currentdirection = headingDegrees;

}
}
}