MSP для Робота или RC машины

Mastino
Offline
Зарегистрирован: 03.12.2011

Приветствую всех!
Один из моих новых проектов машина с компасом, а в будущем может и с 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;

}
}
}

 

 

 

Shizopara
Offline
Зарегистрирован: 04.05.2011

 Я так понял машинка с ДВС.
А на большой скорости не сильно "рыскает"? Я так понимаю в коде задется только лево или право, без указания угла поворота колес. А что происходит в этой строчке?
RCrudder = (pulseIn (6, HIGH, 100000))/10;
это время удержания колес повернутыми? Простите если ламерский вопрос.

До какого примерно угла наклона удается скомпенсировать показания компасса?

Mastino
Offline
Зарегистрирован: 03.12.2011

Shizopara пишет:

Я так понял машинка с ДВС.
А на большой скорости не сильно "рыскает"? Я так понимаю в коде задется только лево или право, без указания угла поворота колес.

Да, пока что только влево и право или прямо. Еще нету настоящего PID. Завтра будут первый испытание на улице..(может и видео выложу). Пока что только дома толкал...:)

Shizopara пишет:

А что происходит в этой строчке?
RCrudder = (pulseIn (6, HIGH, 100000))/10;
это время удержания колес повернутыми? Простите если ламерский вопрос.

эта строчка читает PWM сигнал, который идёт из RC приемника в arduino.

Shizopara пишет:

До какого примерно угла наклона удается скомпенсировать показания компасса?


 

скомпенсировать показания компасса невозможно.. можно только поставить лимит чтения. То есть принимать показание компаса если наклон до 10 градусов.

Евгений74
Offline
Зарегистрирован: 08.05.2012

Вот то же приобрел компас компас – HMC5883L и использую его с Arduino UNO по той же статье, что и вы. Но вот незадача на ардуино иде 1.0. и 1.01. ругается на библиотеку HMC5883L. Прошил через иде 021 все нормально. Очень интересует решение вашей задачи, как удалось?
 

leshak
Offline
Зарегистрирован: 29.09.2011

Евгений74 пишет:

Вот то же приобрел компас компас – HMC5883L и использую его с Arduino UNO по той же статье, что и вы. Но вот незадача на ардуино иде 1.0. и 1.01. ругается на библиотеку HMC5883L. Прошил через иде 021 все нормально. Очень интересует решение вашей задачи, как удалось?
 

Выход - искать более свежую версию библиотеки. Если брали ее где-то "с сайта производителя", псомотрите, может там там ссылочка на домашнюю страницу проекта есть. Если там нет - загляните в файлы самой библиотеки. Частенько в самом верху есть имя автора и линк где взять свежую версию.

Если и тут "облом" и более свежей просто не существует, то 90% библиотек "отучаются от ругани" путем нахождения в них строчки вида

#include "WConstants.h"

и замены ее #include "Arduino.h"

Или, более феншуйно, что-бы она работала и со старой версией IDE и новой, вот на такой кусок

#if ARDUINO >= 100
    #include "Arduino.h"   
#else
extern "C" {
    #include "WConstants.h"
}
#endif

 

Mastino
Offline
Зарегистрирован: 03.12.2011

Евгений74 пишет:

Вот то же приобрел компас компас – HMC5883L и использую его с Arduino UNO по той же статье, что и вы. Но вот незадача на ардуино иде 1.0. и 1.01. ругается на библиотеку HMC5883L. Прошил через иде 021 все нормально. Очень интересует решение вашей задачи, как удалось?
 

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