Одноосевой стабилизатор на MPU6050 и серво

Нам понадобятся:

1. Arduino UNO
2. Сервопривод SG-90
3. Макетная плата
4. Гироскоп MPU6050
5. Много проводочков 😊

Сервопривод подключаем на 9 порт (желтый провод — на 9, красный — 5В, черный — земля). Гироскоп подключаем на 5В, землю и аналоговые входы А4 и А5. SDA на А4, SCL на А5.

В Arduino заливаем следующий код. Код разработан самостоятельно, в Интернете этого кода нет.

#include <Wire.h>
#include <Servo.h>

const int MPU_addr=0x68; int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int minVal=265; int maxVal=402;
int servoPovorotUgol = 0;
int servoPovorotUgolChernovik = 0;
double UgolNaklonaPoX; double y; double z;

Servo servoX;

void setup(){
  servoX.attach(9, 500, 500);
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(9600);
}
void loop(){
  servoX.write(0);
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  int xAng = map(AcX,minVal,maxVal,-90,90);
  int yAng = map(AcY,minVal,maxVal,-90,90);
  int zAng = map(AcZ,minVal,maxVal,-90,90);

  UgolNaklonaPoX= round(RAD_TO_DEG * (atan2(-yAng, -zAng)+PI));
  y= round(RAD_TO_DEG * (atan2(-xAng, -zAng)+PI));
  z= round(RAD_TO_DEG * (atan2(-yAng, -xAng)+PI));

  Serial.print("Угол по оси Х = ");
  Serial.print(UgolNaklonaPoX);
  
  
  if(UgolNaklonaPoX > 270 && UgolNaklonaPoX < 360){
    servoPovorotUgolChernovik = 360 - UgolNaklonaPoX;
    servoPovorotUgol = 90 - servoPovorotUgolChernovik;
  }
  else if(UgolNaklonaPoX > 0 && UgolNaklonaPoX < 90){
    servoPovorotUgol = 90 + UgolNaklonaPoX;
  }
  
  servoX.write(servoPovorotUgol);
  Serial.println("*");
  Serial.print("Команда на серво:");
  Serial.println(servoPovorotUgol);
  Serial.println("-----------------------------------------");
  delay(100);
}

Видео с демонстрацией работы сервопривода можно посмотреть здесь.