Współpraca serwomechanizmu 360 stopni z czujnikiem MPU6050 GY-521

[TheSpikeMan]

Dzień dobry.

Chciałbym prosić o poradę w rozwiązaniu problemu. Celem mojego projektu jest odczyt za pomocą czujnika MPU6050 GY-521 kątów w dwóch płaszczyznach, z wyświetleniem go w tzw. monitorze portu szeregowego z jednoczesnym sterowaniem serwomechanizmem, przystosowanym do obrotu 360 stopni. Prędkość obrotowa wału serwomechanizmu realizowana jest za pomocą potencjometru. Zasilanie realizuję następująco:

- zasilanie 12V, obsługujące płytkę arduino (Vin), zasila potencjometr sterujący serwem, linię sterowania serwem, czujnik, realizuje komunikację z czujnikiem.

- Zasilanie 5V (stabilizowane z 7-9V)- zasilanie samego serwa.

Niestety, jednoczesna praca czujnika i serwa nie jest możliwa. Do obsługi czujnika używam bibioteki i kodu z tej strony . Samodzielnie czujnik pracuje poprawnie. Serwo pracuje cały czas niezależnie od tego czy z czujnikiem czy bez. Problem polega na tym, że przy pracującym serwie przez chwile wyświetlane są dane na monitorze, ale trwa to kilka sekund. Potem kolejne dane nie są wyświetlane (serwo pracuje bez problemu). Kiedy wyłączę okno monitora szeregowego i jeszcze raz je włączę znowu to samo. Kod praktycznie zawiera to samo co w linku wyżej, jakoś 5 linijek dodałem do obsługi serwa (tylko na potrzeby rozwiązania problemu, nie korzystam z potencjometru, a prędkością obrotową serwa steruję tylko z poziomu komputera.). Nie chciałbym używać dwóch płytek do obsługi czujnika i serwa.

/*
   Kalman Filter Example for MPU6050. Output for processing.
   Read more: http://www.jarzebski.pl/arduino/rozwiazania-i-algorytmy/odczyty-pitch-roll-oraz-filtr-kalmana.html
   GIT: https://github.com/jarzebski/Arduino-KalmanFilter
   Web: http://www.jarzebski.pl
   (c) 2014 by Korneliusz Jarzebski
*/


#include <Wire.h>
#include <MPU6050.h>
#include <KalmanFilter.h>
#include <Servo.h> //biblioteka do obsługi serwa
Servo serwomechanizm; //tworzenie nowego obiektu serwa

MPU6050 mpu;

KalmanFilter kalmanX(10, 40, 40);
KalmanFilter kalmanY(10, 40, 40);

float accPitch = 0;
float accRoll = 0;

float kalPitch = 0;
float kalRoll = 0;

void setup() 
{
 Serial.begin(9600);

 // Initialize MPU6050
 while(!mpu.begin(MPU6050_SCALE_250DPS, MPU6050_RANGE_2G))
 {
   delay(500);
 }

 // Calibrate gyroscope. The calibration must be at rest.
 // If you don't want calibrate, comment this line.
 mpu.calibrateGyro();

 serwomechanizm.attach(11); //serwo podpięte pod pin 11 arduino
 serwomechanizm.writeMicroseconds(1600); //predkosc obrotowa serwa- impuls 1600 mikrosekund

delay(1000);
}

void loop()
{

 Vector acc = mpu.readNormalizeAccel();
 Vector gyr = mpu.readNormalizeGyro();

 // Calculate Pitch & Roll from accelerometer (deg)
 accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
 accRoll  = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;

 // Kalman filter
 kalPitch = kalmanY.update(accPitch, gyr.YAxis);
 kalRoll = kalmanX.update(accRoll, gyr.XAxis);

 Serial.print(kalPitch);
 Serial.print(":");
 Serial.print(kalRoll);
 Serial.print(":");

 Serial.println();

 delay(50);
}

Edit: Podłączyłem inny czujnik (Gy-80) i wszystko działa jak należy. Istota więc w czujniku. Mam podłączoną diodę równolegle z zasilaniem czujnika. Czy to ona może być źródłem problemów? Nie mogę jej odłączyć,żeby sprawdzić bo mam już polutowane.