LOGIN
Kunal Jain
4
Collaborator
student (Project Leader)
 · HisarIndia
Share
Report
Get Link
SHOW & TELL
#include <Servo.h>




#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
   #include "Wire.h"
#endif

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
//#define OUTPUT_READABLE_REALACCEL
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
   mpuInterrupt = true;
}





//Create the 4 esc objects
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

//Esc pins
int escPin1 = 5;
int escPin2 = 9;
int escPin3 = 10;
int escPin4 = 11;

//Min and max pulse
int minPulseRate        = 1000;
int maxPulseRate        = 2000;
int throttleChangeDelay = 50;


//SETUP
void setup() {
 
 Serial.begin(115200);
 Serial.setTimeout(500);
 
 
 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
       Wire.begin();
       Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
   #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
       Fastwire::setup(400, true);
   #endif
 
   while (!Serial); // wait for Leonardo enumeration, others continue immediately
   Serial.println(F("Initializing I2C devices..."));
   mpu.initialize();
   pinMode(INTERRUPT_PIN, INPUT);
   Serial.println(F("Testing device connections..."));
   Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
   Serial.println(F("Initializing DMP..."));
   devStatus = mpu.dmpInitialize();
   mpu.setXGyroOffset(63);
   mpu.setYGyroOffset(-45);
   mpu.setZGyroOffset(-22);
   mpu.setZAccelOffset(5255); // 5255 factory default for my test chip
   if (devStatus == 0) {
       Serial.println(F("Enabling DMP..."));
       mpu.setDMPEnabled(true);
       Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
       attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
       mpuIntStatus = mpu.getIntStatus();
       Serial.println(F("DMP ready! Waiting for first interrupt..."));
       dmpReady = true;
       packetSize = mpu.dmpGetFIFOPacketSize();
   } else {
       Serial.print(F("DMP Initialization failed (code "));
       Serial.print(devStatus);
       Serial.println(F(")"));
   }
   pinMode(LED_PIN, OUTPUT);

 
 
 
 
 
 
 //Init escs
 initEscs();
}

//LOOP
void loop() {
 
       // if programming failed, don't try to do anything
   if (!dmpReady) return;
   while (!mpuInterrupt && fifoCount < packetSize) {
   }
   mpuInterrupt = false;
   mpuIntStatus = mpu.getIntStatus();
   fifoCount = mpu.getFIFOCount();
   if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
       mpu.resetFIFO();
       Serial.println(F("FIFO overflow!"));
 } else if (mpuIntStatus & 0x02) {
       while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
       mpu.getFIFOBytes(fifoBuffer, packetSize);
       fifoCount -= packetSize;

       #ifdef OUTPUT_READABLE_YAWPITCHROLL
           // display Euler angles in degrees
           mpu.dmpGetQuaternion(&q, fifoBuffer);
           mpu.dmpGetGravity(&gravity, &q);
           mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
         
           float x,y,z;
           x=(ypr[0] * 180/M_PI);
           y=(ypr[1] * 180/M_PI);
           z=(ypr[2] * 180/M_PI);
           
           Serial.print("\t\t\t\t\t ypr\tx=");
           Serial.print(x);
           Serial.print("\t\ty=");
           Serial.print(y);
           Serial.print("\t\tz=");
           Serial.println(z);
         
       #endif

       blinkState = !blinkState;
       digitalWrite(LED_PIN, blinkState);
       
   }
 
  
 
 // Wait for some input
 if (Serial.available() > 0) {
   
  // Read the new throttle value
   int throttle = normalizeThrottle(Serial.parseInt());
   
   // Print it out
   Serial.print("    Setting throttle to: ");
   Serial.println(throttle);
   
   // Change throttle to the new value
   changeThrottle(throttle);
 }

}

//Change throttle value
void changeThrottle(int throttle) {
 int currentThrottle = readThrottle();
 
 int step = 1;
 if(throttle < currentThrottle) {
   step = -1;
 }
 
 // Slowly move to the new throttle value 
 while(currentThrottle != throttle) {
   writeTo4Escs(currentThrottle + step);
   
   currentThrottle = readThrottle();
   delay(throttleChangeDelay);
 }
}

//Read the throttle value
int readThrottle() {
 int throttle = esc1.read();
 
 Serial.print("Ct th is: ");
 Serial.print(throttle);
 
 
 return throttle;
}

//Change velocity of the 4 escs at the same time
void writeTo4Escs(int throttle) {
 

 int A,B,C,D;
 C=throttle;
 A=C;
 B=throttle;
 D=B;
 esc1.write(A); //never change value of esc 1
 esc2.write(B);
 esc3.write(C);
 esc4.write(D);
 Serial.print("        ");
 Serial.print(A);
 Serial.print("     ");
 Serial.print(B);
 Serial.print("     ");
 Serial.print(C);
 Serial.print("     ");
 Serial.print(D);
 Serial.println();

}

//Init escs
void initEscs() {
 
 
 
 
 


 

 esc1.attach(escPin1, minPulseRate, maxPulseRate);
 esc2.attach(escPin2, minPulseRate, maxPulseRate);
 esc3.attach(escPin3, minPulseRate, maxPulseRate);
 esc4.attach(escPin4, minPulseRate, maxPulseRate);
 
 //Init motors with 0 value
 writeTo4Escs(0);
}

//Start the motors
void startUpMotors() {
 writeTo4Escs(50);
}

// Ensure the throttle value is between 0 - 180
int normalizeThrottle(int value) {
 if(value < 0) {
   return 0;
   
 } else if(value > 80) {
   return 80;
 }
 
 return value;
}
Contributions
Abhi Verma

Your intellectual presence is very much an intuition , and very much desired in the Walker But Project . If this Project will not
be operational The cause will be Balancing , And you know how it is done

Abhi Verma

PID for drone
Needs that vrey much thank u sir

Abhi Verma

Provided missing PID controller code example to help with test program, well done Abhi

Kunal Jain

Good start on test program, great effort keep it up!

9 likes 
Like
Award Contribution
Load 6 previous comments
Kunal Jain
4
Collaborator
student (Project Leader)
 · HisarIndia
Share
Report
Get Link

I’ll try it.
But it will take time.
Thank u very much sir for your help.

Like
1 like 
Award Contribution
Engineer
 · ThiruvananthapuramIndia
Share
Report
Get Link

I think you have to use the PID code for roll and pitch using mpu6050 and yaw from the hmc518 magnetometer.The yaw values of mpu6050 are not stable so better is to use a magnetometer.
Use a PID controller for each axis ie roll pitch and yaw and then tune them either by trial and error.

Like
1 like 
Award Contribution
North eugene
8
Top Contributor
Designer special solution && automation , Software engneering student , Robotics developer
Share
Report
Get Link

Abhi the code is elegant , Re Usable on platforms other than Quad . Nice work Bro

Like
3 likes 
Award Contribution
North eugene
8
Top Contributor
Designer special solution && automation , Software engneering student , Robotics developer
Share
Report
Get Link

using GY 80 10 DOF makes it more versatile as it has all the sensors

  1. altimeter , gyro , accl , mag, temp , humidity. and is easy to configure
Like
2 likes 
Award Contribution
Abhi Verma
35
Community Expert
ROBOToTHON 2017 Mentor, Electronics Engineer
 · DelhiIndia
Share
Report
Get Link
Contribution

Your intellectual presence is very much an intuition , and very much desired in the Walker But Project . If this Project will not
be operational The cause will be Balancing , And you know how it is done

Like
1 like 
Award Contribution
North eugene
8
Top Contributor
Designer special solution && automation , Software engneering student , Robotics developer
Share
Report
Get Link

Please Post the codes with PID ones that you have have tested
the application of these codes is universal and be adapted to any application of Robotics
Will apreciate your generocity

Right now just have codes for X + and X- (Gyro)only not accl / or mag

Like
1 like 
Award Contribution
Leave a reply...
DISCOVER
CHAT
ALERTS
DISCUSSIONS
FEED
New Post
Start Project
Online Users
Share Link
Help