Главная » Форумы » Программирование

Гироскоп и Аксель

Нет ответов
Ср, 17/02/2016 - 00:58
Dragal
Offline
Зарегистрирован: 04.09.2015

Здравствуйте! 

Я собираюсь замерять угол подъема горизонтально плиты, которая шарнирно закреплена одной стороной. Поднимается она гидроцилиндрами. 

Я науился снимать показания с гиро и акселя, и переводить их в градусы. Но возникла одна проблема

#include <l3g4200d.h>
#include <lis331dlh.h>
#include <lis3mdl.h>
#include <LPS331.h>
#include <troyka-imu.h>
#include <Wire.h>
Gyroscope gyro;
Accelerometer accel;
float rotationThreshold = 1;   //Минимальная угловая скорость, которую можно не учитывать 
float pitchacc;
float currentAngle;
float pitch;
boolean DR;
float t;
float tt;
void setup() {
  Serial.begin (9600);
  gyro.begin();
  gyro.setRange(RANGE_500);
  accel.begin(); 
  DR = true;
  pitch = 0;
}
void loop() { 
  Serial.print ( DR); // Проверял действуюет ли первый if для задания начального угла гироскопа
  Serial.print ("\t\t");
  // Тут только для первого лупа задаю начальное значие угла отклонения для гироскопа, который равен показанию акселерометра
  if (DR = true)
  {
    currentAngle = atan2(accel.readY_G(),accel.readZ_G())*180/3.14;
    t = micros();
    DR = false;
  }
  float gyroRate = gyro.readX_DegPerSec();
  tt = micros();
  Serial.print (tt-t);
  Serial.print ("\t\t");
  //Игнорировать показания гироскопа, если они меньше заданного лимита 
  if (gyroRate >= rotationThreshold || gyroRate <= -rotationThreshold) {
  currentAngle += gyroRate*(tt-t)/1000;
  Serial.print ( currentAngle);
  Serial.print ("\t\t");
  }
  pitchacc = atan2(accel.readY_G(),accel.readZ_G())*180/3.14;
  Serial.print (pitchacc);
  Serial.print ("\t\t");
  pitch = (pitchacc*0.05 + 0.95* currentAngle);
  Serial.println(pitch);
  t = tt;  
}
Вопрос в другом: 
1)
Когда я беру модуль в руки и начинаю двигать, скачут не только показания акселя, но и силно разнятся показания гироскопа(см. первую картинку). Я бы даже скзал, акселемрометр работает стабильней...  При применении в практике это сильно будет влиять на результат. 
2) (см. картинку вторую) Почену Serial.print выдает третий столбик, а именно угла с комплементарным фильтром, не всегда, а только тогда, когда показания гиро и акселе разняться (начиная от разницы в 5 градусов) 
Буду очень признателен
 
 
Верх