[Solved] How to Balance Quadcopter using L3G4200 and PID controll [closed]

by


I don’t have the exact sensor as you, but this code is a snippet that I’ve been working on. You can look at it… Maybe it could help. If it did, then good 🙂

/*

  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();

}

1

solved How to Balance Quadcopter using L3G4200 and PID controll [closed]