L3G4200とPID制御を使ったクアドコプターのバランス調整方法

Question

私は、大学のマイクロプロセッサープロジェクトのクアドコプターを作っています。私はすべてのハードウェアをセットアップしましたが、今はBalancing Algorithmでスタッキングしています。 このプロジェクトで働いていた人がいれば、このパートのコードを教えてください。ところで、私はセンサーL3G4200を使用します。 ありがとうございます。

Answer 1

私はあなたと同じセンサーを持っていませんが、このコードは私が取り組んできたスニペットです。あなたはそれを見ることができます...多分それは助けることができます。もしそうなら、良い：

/* 

    Project goal: 

    Arduino built quadcopter. 



    Motor diagram: 
    Legend: 

    num : motor number 
    -> : clockwise 
    <- : counter-clockwise 

    2 ->  <- 1 
    \  /
     \ -/
     | | 
    /- \ 
    /  \ 
    3 <-  -> 4 

    By : Dat HA 

    Date : 2016/11/09 
     yyyy/mm/dd 

*/ 

//////////////////////////////////////////////////////////////////////////////////////////////////////// 

#include <Wire.h>      //include I2C library 
#include <Servo.h> 
#include "I2Cdev.h"      //include another I2C library 
#include "Wire.h"      //include another I2C library (yes, again) 
#include "MPU6050_6Axis_MotionApps20.h" //include the MPU6050 library 

//////////////////////////////////////////////////////////////////////////////////////////////////////// 

bool dmpReady = false;    //state of the mpu6050 
volatile bool mpuInterrupt = false; //some random bool 

uint8_t mpuIntStatus;    //holds actual interrupt status byte from MPU 
uint8_t devStatus;     //return status after each device operation 
uint8_t motorPins[4] = {3, 9, 10, 11}; //yaw, pitch, roll 
uint8_t fifoBuffer[64];    //FIFO storage buffer 

uint16_t packetSize;    //expected DMP packet size (default is 42 bytes) 
uint16_t fifoCount;     //count of all bytes currently in FIFO 

MPU6050 mpu;      //change mpu's adress 
Quaternion q;      //quaternion container 
VectorFloat gravity;    //gravity vector 

//////////////////////////////////////////////////////////////////////////////////////////////////////// 

Servo motor1; 
Servo motor2; 
Servo motor3; 
Servo motor4; 

//////////////////////////////////////////////////////////////////////////////////////////////////////// 

void setup() { 
    Serial.begin(115200); //start serial communication 
    Wire.begin();   //start I2C communication 

    TWBR = 24; 
    mpu.initialize(); 
    devStatus = mpu.dmpInitialize(); 
    mpu.setXGyroOffset(0); 
    mpu.setYGyroOffset(0); 
    mpu.setZGyroOffset(0); 
    mpu.setZAccelOffset(0); 

    if (devStatus == 0) 
    { 
    mpu.setDMPEnabled(true); 
    mpuInterrupt = true; 
    mpuIntStatus = mpu.getIntStatus(); 
    dmpReady = true; 
    packetSize = mpu.dmpGetFIFOPacketSize(); 
    } 

    motor1.attach(motorPins[0]); 
    motor2.attach(motorPins[1]); 
    motor3.attach(motorPins[2]); 
    motor4.attach(motorPins[3]); 
} 

//////////////////////////////////////////////////////////////////////////////////////////////////////// 

void loop() { 
    //controller 
    int p = analogRead(A0); 
    int r = analogRead(A1); 

    p = map(p, 0, 1023, -50, 50); 
    r = map(r, 0, 1023, -50, 50); 

    //gyro 
    int motorSpeed = 50; 
    int ypr[3]; 
    float yprFlo[3]; //ypr as a float 
    mpu.resetFIFO(); 
    mpuInterrupt = false; 
    mpuIntStatus = mpu.getIntStatus(); 
    fifoCount = mpu.getFIFOCount(); 

    if (mpuIntStatus & 0x02) 
    { 
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 
    mpu.getFIFOBytes(fifoBuffer, packetSize); 
    fifoCount -= packetSize; 
    mpu.dmpGetQuaternion(&q, fifoBuffer); 
    mpu.dmpGetGravity(&gravity, &q); 
    mpu.dmpGetYawPitchRoll(yprFlo, &q, &gravity); 

    for (int i = 0; i < 3; i++) 
    { 
     ypr[i] = map((yprFlo[i] * 180/M_PI), -90, 90, -50, 50); 
    } 
    } 

    //print 
    Serial.print(ypr[0]); 
    Serial.print(" "); 
    Serial.print(ypr[1]); 
    Serial.print(" "); 
    Serial.print(ypr[2]); 
    Serial.print("   "); 

    //motor 
    int motorSpeed1 = map(motorSpeed - ypr[1] + ypr[2], -100, 200, 90, 0); 
    int motorP1 = map(motorSpeed1,90,0,0,100); 
    Serial.print(motorP1); 
    Serial.print(" "); 
    motor1.write(motorSpeed1); 

    int motorSpeed2 = map(motorSpeed - ypr[1] - ypr[2], -100, 200, 90, 180); 
    int motorP2 = map(motorSpeed2,90,180,0,100); 
    Serial.print(motorP2); 
    Serial.print(" "); 
    motor2.write(motorSpeed2); 

    int motorSpeed3 = map(motorSpeed + ypr[1] - ypr[2], -100, 200, 90, 0); 
    int motorP3 = map(motorSpeed2,90,0,0,100); 
    Serial.print(motorP3); 
    Serial.print(" "); 
    motor3.write(motorSpeed3); 

    int motorSpeed4 = map(motorSpeed + ypr[1] + ypr[2], -100, 200, 90, 180); 
    int motorP4 = map(motorSpeed2,90,180,0,100); 
    Serial.print(motorP4); 
    Serial.print(" "); 
    motor4.write(motorSpeed4); 

    Serial.println(); 

}