SHARE
TWEET

Untitled

a guest Mar 7th, 2017 315 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2. #include <Wire.h>
  3. // Gy 521 Start
  4. #include "I2Cdev.h"
  5. #include "MPU6050_6Axis_MotionApps20.h"
  6. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  7. #include "Wire.h"
  8. #endif
  9. MPU6050 mpu;
  10. #define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
  11. // MPU control/status vars
  12. bool dmpReady = false;  // set true if DMP init was successful
  13. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  14. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  15. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  16. uint16_t fifoCount;     // count of all bytes currently in FIFO
  17. uint8_t fifoBuffer[64]; // FIFO storage buffer
  18. // orientation/motion vars
  19. Quaternion q;           // [w, x, y, z]         quaternion container
  20. VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  21. VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  22. VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  23. VectorFloat gravity;    // [x, y, z]            gravity vector
  24. float euler[3];         // [psi, theta, phi]    Euler angle container
  25. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  26. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
  27. void dmpDataReady() {
  28.   mpuInterrupt = true;
  29. }
  30.  
  31. //#define yprAnglesAndAcc
  32. // GY521 end
  33.  
  34. //servo
  35. Servo motor1;
  36. Servo motor2;
  37. Servo motor3;
  38. Servo motor4;
  39.  
  40. //DEFINE VARIABLES
  41. int m1; //motor 1-2 = ROLL
  42. int m2;
  43. int m3; //motor 3-4 = PITCH
  44. int m4;
  45. int counter = 0;
  46.  
  47. bool first = true; //is this the first time looping?
  48. //Variables for ROLL
  49. int X_eV;             // Angle error (for roll)
  50. float X_gV;           // For storing accelerometer data
  51. float X_gVArray [5];  // Put accelerometer data in array
  52. float X_oVH;          // Desiered angular velocity
  53. float X_gVH;          // For storing Gyro data
  54. float X_gVHArray [5]; // Put gyro data in array
  55. float X_eVH;          // Angualar velocity error
  56. float X_dKraft;       // For storing thrust difference between propellers
  57. float X_gVHmed;       // Average angular velcoity
  58. float X_gVmed;        // Average angle
  59. //Variables for PITCH
  60. int Y_eV;             // Angle error (for pitch)
  61. float Y_gV;
  62. float Y_gVArray [5];
  63. float Y_oVH;
  64. float Y_gVH;
  65. float Y_gVHArray [5];
  66. float Y_eVH;
  67. float Y_dKraft;
  68. float Y_gVHmed;
  69. float Y_gVmed;
  70.  
  71. //Settings
  72. float kp1 = 0.6;    //Set P-term to limit desired angular velocity
  73. float kp2 = 1.5;    //Set P-term to limit difference between m1 and m2
  74. int thrust = 1400;  //Set thrust
  75. int maxVal = 1650;
  76. int minVal = 1250;
  77. int X_oV = 0;       //Set desired angle for ROLL
  78. int Y_oV = 0;       //Set desired angle for PITCH
  79.  
  80.  
  81. void setup()
  82. {
  83.   Serial.begin(250000);
  84.   // GY521 Start
  85. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  86.   Wire.begin();
  87.   Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  88. #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  89.   Fastwire::setup(400, true);
  90. #endif
  91.  
  92.   mpu.initialize();
  93.   pinMode(INTERRUPT_PIN, INPUT);
  94. /*
  95.   // wait for ready
  96.   Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  97.   while (Serial.available() && Serial.read()); // empty buffer
  98.   while (!Serial.available());                 // wait for data
  99.   while (Serial.available() && Serial.read()); // empty buffer again*/
  100.  
  101.   // load and configure the DMP
  102.   Serial.println(F("Initializing DMP..."));
  103.   devStatus = mpu.dmpInitialize();
  104.  
  105.   // supply your own gyro offsets here, scaled for min sensitivity
  106.   mpu.setXGyroOffset(220);
  107.   mpu.setYGyroOffset(76);
  108.   mpu.setZGyroOffset(-85);
  109.   mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  110.  
  111.   // make sure it worked (returns 0 if so)
  112.   if (devStatus == 0) {
  113.     // turn on the DMP, now that it's ready
  114.     Serial.println(F("Enabling DMP..."));
  115.     mpu.setDMPEnabled(true);
  116.  
  117.     // enable Arduino interrupt detection
  118.     Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
  119.     attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
  120.     mpuIntStatus = mpu.getIntStatus();
  121.  
  122.     // set our DMP Ready flag so the main loop() function knows it's okay to use it
  123.     Serial.println(F("DMP ready! Waiting for first interrupt..."));
  124.     dmpReady = true;
  125.  
  126.     // get expected DMP packet size for later comparison
  127.     packetSize = mpu.dmpGetFIFOPacketSize();
  128.   } else {
  129.     // ERROR!
  130.     // 1 = initial memory load failed
  131.     // 2 = DMP configuration updates failed
  132.     // (if it's going to break, usually the code will be 1)
  133.     Serial.print(F("DMP Initialization failed (code "));
  134.     Serial.print(devStatus);
  135.     Serial.println(F(")"));
  136.   }
  137.   // GY521 end
  138.  
  139.  
  140.  
  141.  
  142.   motor1.attach(3); // ESC pin
  143.   motor2.attach(5); // ESC pin
  144.   motor3.attach(6); // ESC pin !!!!WILL PROBABLY NEED TO CHANGE!!!
  145.   motor4.attach(9); // ESC pin      <-- !!!!HERE TO!!!!
  146. }
  147.  
  148.  
  149. void loop() {
  150.   //add starting frequence
  151.   //init (current start freq)
  152.   if (first) {
  153.     for (int i = 1000; i <  1700; i += 1) {
  154.       motor1.writeMicroseconds(i);
  155.       motor2.writeMicroseconds(i);
  156.       motor3.writeMicroseconds(i);
  157.       motor4.writeMicroseconds(i);
  158.       Serial.println(i);
  159.       delay(25);
  160.     }
  161.     for (int j = 1700; j > 1390 ; j -= 1) {
  162.       motor1.writeMicroseconds(j);
  163.       motor2.writeMicroseconds(j);
  164.       motor3.writeMicroseconds(j);
  165.       motor4.writeMicroseconds(j);
  166.       Serial.println(j);
  167.       delay(25);
  168.     }
  169.     first = false;
  170.   }
  171.   while (true) {
  172.  
  173.     if (!dmpReady) return;
  174.  
  175.     // wait for MPU interrupt or extra packet(s) available
  176.     while (!mpuInterrupt && fifoCount < packetSize) {
  177.       // other program behavior stuff here
  178.       // .
  179.       // .
  180.       // .
  181.       // if you are really paranoid you can frequently test in between other
  182.       // stuff to see if mpuInterrupt is true, and if so, "break;" from the
  183.       // while() loop to immediately process the MPU data
  184.       // .
  185.       // .
  186.       // .
  187.     }
  188.  
  189.     // reset interrupt flag and get INT_STATUS byte
  190.     mpuInterrupt = false;
  191.     mpuIntStatus = mpu.getIntStatus();
  192.     fifoCount = mpu.getFIFOCount();
  193.     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  194.       mpu.resetFIFO();
  195.       Serial.println(F("FIFO overflow!"));
  196.     } else if (mpuIntStatus & 0x02) {
  197.       while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  198.       mpu.getFIFOBytes(fifoBuffer, packetSize);
  199.       fifoCount -= packetSize;
  200.  
  201.  
  202.  
  203.  
  204.       mpu.dmpGetQuaternion(&q, fifoBuffer);
  205.       mpu.dmpGetGravity(&gravity, &q);
  206.       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  207.       mpu.dmpGetAccel(&aa, fifoBuffer);
  208.       mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  209.       mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
  210.       //Read data from accelerometer and gyro
  211.       X_gV = ypr[1] * 180 / M_PI; // roll
  212.       Y_gV = ypr[2] * 180 / M_PI;  // Pitch
  213.       //Save all accelerometer data in a array
  214.       X_gVArray[counter % 5] = X_gV;
  215.       Y_gVArray[counter % 5] = Y_gV;
  216.  
  217.       X_gVH = aaWorld.x / 10;   // Acc x
  218.       Y_gVH = aaWorld.y / 10;   // acc y
  219.       //Save all gyro data in a different array
  220.       X_gVHArray[counter % 5] = X_gVH;
  221.       Y_gVHArray[counter % 5] = Y_gVH;
  222.      
  223.       counter++;
  224.  
  225.       if (counter % 5 == 0) {
  226.         //Compute for every five readings
  227.         //Find average angle
  228.         X_gVmed = (X_gVArray[0] + X_gVArray[1] + X_gVArray[2] + X_gVArray[3] + X_gVArray[4]) / 5;
  229.         Y_gVmed = (Y_gVArray[0] + Y_gVArray[1] + Y_gVArray[2] + Y_gVArray[3] + Y_gVArray[4]) / 5;
  230.         //Find avarage angular velocity
  231.         X_gVHmed = (X_gVHArray[0] + X_gVHArray[1] + X_gVHArray[2] + X_gVHArray[3] + X_gVHArray[4]) / 5;
  232.         Y_gVHmed = (Y_gVHArray[0] + Y_gVHArray[1] + Y_gVHArray[2] + Y_gVHArray[3] + Y_gVHArray[4]) / 5;
  233.         //Error angle
  234.         X_eV = X_oV - X_gVmed;
  235.         Y_eV = Y_oV - Y_gVmed;
  236.         //Set desired angular velocity
  237.         X_oVH = X_eV * kp1;
  238.         Y_oVH = Y_eV * kp1;
  239.  
  240.         //To avoid sign error:
  241.         if (X_oVH >= 0) {
  242.           X_eVH = X_oVH - X_gVHmed;
  243.         }
  244.         else {
  245.           X_eVH = X_oVH + X_gVHmed;
  246.         }
  247.         if (Y_oVH >= 0) {
  248.           Y_eVH = Y_oVH - Y_gVHmed;
  249.         }
  250.         else {
  251.           Y_eVH = Y_oVH + Y_gVHmed;
  252.         }
  253.  
  254.         //Thrust differential
  255.         X_dKraft = X_eVH * kp2;
  256.         Y_dKraft = Y_eVH * kp2;
  257.         //Final thrust
  258.         m1 = thrust + X_dKraft;
  259.         m2 = thrust - X_dKraft;
  260.         m3 = thrust + Y_dKraft;
  261.         m4 = thrust - Y_dKraft;
  262.  
  263.         //Check if m1 exceeds the limit
  264.         if (m1 > maxVal) {
  265.           m1 = maxVal;
  266.         }
  267.         else if (m1 < minVal) {
  268.           m1 = minVal;
  269.         }
  270.         //Check if m2 exceeds the limit
  271.         if (m2 > maxVal) {
  272.           m2 = maxVal;
  273.         }
  274.         else if (m2 < minVal) {
  275.           m2 = minVal;
  276.         }
  277.  
  278.         //Send PWM signals
  279.         motor1.writeMicroseconds(m1);
  280.         motor2.writeMicroseconds(m2);
  281.         motor3.writeMicroseconds(m3);
  282.         motor4.writeMicroseconds(m4);
  283.  
  284.       }
  285.       if (counter % 10 == 0) {
  286.         //Print pwm signals for motors, acc and gyro data for every 20 loop
  287.         Serial.print("X : ");
  288.         Serial.print(m1);
  289.         Serial.print(" - ");
  290.         Serial.print(m2);
  291.         Serial.print(" - ");
  292.         Serial.print(X_gV);
  293.         Serial.print(" - ");
  294.         Serial.print(X_gVH);
  295.         Serial.print(" ");
  296.         Serial.print("Y : ");
  297.         Serial.print(m3);
  298.         Serial.print(" - ");
  299.         Serial.print(m4);
  300.         Serial.print(" - ");
  301.         Serial.print(Y_gV);
  302.         Serial.print(" - ");
  303.         Serial.print(Y_gVH);
  304.         Serial.println(" ");
  305.       }
  306.     }
  307.   }
  308. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top