Гироскоп на mpu6050 HELP!!! РЕШЕНО!!!

Спасибо за комплименты...

 

Решено!!!! может кому и пригодится. Прошу Эль-Дэ-Балаболам которые кроме "а по пи...деть" на форуме пользы никакой не приносят - ПРОХОДИТЕ МИМО.
сам скетч

Код (C++):

#include <Wire.h>
#include "Kalman.h"
#include "PID_v1.h"

Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;

int16_t gyroY;

double accYangle;
double gyroYangle = 180;
double compAngleY = 180;
double kalAngleY;

double in, out, setpoint;
PID pid(&in, &out, &setpoint, 30, 410 , 0.8, DIRECT);

uint32_t timer;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  i2cWrite(0x6B,0x00); // Disable sleep mode
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }  
  kalmanY.setAngle(180);

  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255,255);
  pid.SetSampleTime(100);
  setpoint = 181.9;
 
  timer = micros();
}

double ang;
int pwm;

void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);
  gyroY = ((data[10] << 8) | data[11]);
 
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
 
  double gyroYrate = -((double)gyroY/181.0);
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
 
  in = kalAngleY;
  if(in > 170 && in < 190){ // Вот тут и выставляется зазор мертвой зоны не качания.
   analogWrite(11, 0);
   analogWrite(10, 0);
   goto pause;
  }
  pid.Compute();
  if (out < 0) {
    pwm = -1 * out;
    analogWrite(11, 0);
    analogWrite(10, pwm);
  } else {
    pwm = out;
    analogWrite(10, 0);
    analogWrite(11, pwm);
  }
  Serial.println(kalAngleY);
pause:

  delay(1); // The accelerometer's maximum samples rate is 1kHz
}


void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}

uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data = Wire.read();
  return data;
}

 

Вставьте код в тэги, пожалуйста

 

Сделано!

 

Низабуть сюда написать, када тебя сажать будут.

 

Рад за вас. Пара реплик -
В самом начале программы надо было объявить выходные пины, типа

#define Left_Valve 10 // Левый клапан
#define Right_Valve 11 // Правый клапан
А в программе вместо номеров пинов (10,11) вставлять их имена (Left_Valve, Right_Valve).
Это вроде как считается хорошим тоном программирования, а то сразу не поймешь где у вас выход. И главное - Как говорят - гладко было на бумаге, да буераки и овраги.
Вы пытаетесь совместить динамический гироскоп и статический вектор силы тяжести Земли. Гироскоп хорош на временах в минуты. А работа на таком подъемнике может длиться часами. При этом погрешность гироскопа будет нарастать. Вы инициализируете гироскоп на заведомо горизонтальной поверхности в начале работы, а что он будет считать горизонтом через пару часов ?
Тут обязательно надо что-то абсолютное, типа "строительного уровня" (или отвеса).
ПС. Я это чисто теоретизирую. Так что извиняйте.

 

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