MPU9250 Arduino Gimbal Stabilization

Projects Networking, Protocols, and Devices
1

Hi guys,i have to make one device kinda gimbal with yaw,pitch only,so i have MPU9250 and Arduino Nano,so i need to have Magnetometer oriented to north and stabilizing gumbal which will stabilize if i move gimbal ,please help me to achieve this

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

MPU6050 mpu;
SimpleKalmanFilter kalmanX(1, 1, 0.1); // Սկզբնական Kalman գործակիցը
Servo myServo;

// PID կարգավորիչի նախնական արժեքներ
float Kp = 12, Ki = 0.4, Kd = 0.5; // Փոփոխված արժեքներ՝ տատանումները նվազեցնելու համար
float setPoint = 0;
float error, previousError = 0, integral = 0, derivative;
float output;
float currentServoPos = 90;

// Սերվոյի արագության գործակից (0% - 100%)
int servoSpeedFactor = 20; 
int lastServoSpeed = 20;  

float angleX = 0, accX, gyroX;
float prevAngleX = 0;
unsigned long lastUpdateTime = 0;

// Տպման միջակայքի փոփոխականներ
unsigned long lastPrintTime = 0;
const unsigned long printInterval = 600; // Թարմացման միջակայք (500ms)

const float deadZoneThreshold = 0.5; // "Մեռած գոտի", որը կանխում է փոքր տատանումները

void setup() {
    Wire.begin();
    Wire.setClock(100000);  // I2C փոխանցման արագությունը՝ 400 kHz (արագ)
    
    Serial.begin(115200);
    mpu.initialize();
    mpu.setDLPFMode(0); // Հաճախականությունը՝ 256 Hz (արագ տվյալների թարմացում)

    myServo.attach(9);
    myServo.write(currentServoPos);

    Serial.println("Փոփոխելու համար PID արժեքները՝ օգտագործեք: p=(արժեք), i=(արժեք), կամ d=(արժեք)");
    Serial.println("Սերվոյի արագությունը փոխելու համար՝ օգտագործեք: s=(արժեք)% (օրինակ՝ s=80%)");
    Serial.println("Kalman գործակիցը փոխելու համար՝ օգտագործեք: k roll=(արժեք) (օրինակ՝ k roll=1)");
}

void loop() {
    unsigned long currentTime = millis();

    int16_t ax, ay, az, gx, gy, gz;
    mpu.getAcceleration(&ax, &ay, &az);
    mpu.getRotation(&gx, &gy, &gz);
    accX = ax / 16384.0;
    gyroX = gx / 131.0;

    float accelAngleX = atan2(ay, az) * 180 / PI;
    angleX = kalmanX.updateEstimate(accelAngleX + gyroX * 0.01);

    if (angleX >= -5 && angleX <= 5) { 
        servoSpeedFactor = 0; 
    } else { 
        servoSpeedFactor = lastServoSpeed; 
    }

    prevAngleX = angleX;
    lastUpdateTime = currentTime;

    error = setPoint - angleX;
    
    if (abs(error) > deadZoneThreshold) { // Աշխատում է միայն եթե սխալը մեծ է "մեռած գոտու" շեմից
        integral += error * 0.01;
        derivative = (error - previousError) / 0.01;
        output = Kp * error + Ki * integral + Kd * derivative;
        output = constrain(output, 0, 180);
    }

    if (servoSpeedFactor > 0) {  
        int stepSize = map(servoSpeedFactor, 0, 100, 1, 10);
        if (abs(currentServoPos - output) > 1) {
            currentServoPos += (currentServoPos < output) ? stepSize : -stepSize;
        }
        myServo.write(currentServoPos);
    }

    previousError = error;

    // Տպում տվյալները միայն եթե անցել է printInterval ժամանակը
    if (millis() - lastPrintTime >= printInterval) {
        lastPrintTime = millis(); // Թարմացնում ենք վերջին տպման պահը

        Serial.print("Roll: ");
        Serial.print(angleX, 2);
        Serial.print(" | Servo: ");
        Serial.print(currentServoPos);
        Serial.print(" | Speed: ");
        Serial.print(servoSpeedFactor);
        Serial.print("%");
        Serial.print(" | Kp: ");
        Serial.print(Kp);
        Serial.print(" | Ki: ");
        Serial.print(Ki);
        Serial.print(" | Kd: ");
        Serial.print(Kd);
        Serial.print(" | Kalman Gain: ");
        Serial.println(kalmanX.updateEstimate(accelAngleX));
    }

    checkSerialInput();
    delay(10);
}

void checkSerialInput() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');

        if (input.startsWith("p=")) {
            Kp = input.substring(2).toFloat();
            Serial.print("Kp updated: ");
            Serial.println(Kp);
        } else if (input.startsWith("i=")) {
            Ki = input.substring(2).toFloat();
            Serial.print("Ki updated: ");
            Serial.println(Ki);
        } else if (input.startsWith("d=")) {
            Kd = input.substring(2).toFloat();
            Serial.print("Kd updated: ");
            Serial.println(Kd);
        } else if (input.startsWith("s=")) {
            input.replace("%", ""); 
            lastServoSpeed = input.substring(2).toInt();
            servoSpeedFactor = constrain(lastServoSpeed, 0, 100);
            Serial.print("Servo speed updated: ");
            Serial.print(servoSpeedFactor);
            Serial.println("%");
        } else if (input.startsWith("k roll=")) {  
            float newKalmanGain = input.substring(8).toFloat();
            kalmanX = SimpleKalmanFilter(newKalmanGain, 1, 0.1);  
            Serial.print("Kalman gain updated: ");
            Serial.println(newKalmanGain);
        }
    }
}

New Bitmap image
New Bitmap image610×559 300 KB

2

A link to magnetometer calibration can be found in this topic...