API tools faq
paste
Guest User

Untitled

a guest
May 23rd, 2018
106
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.07 KB | None | 0 0
raw download clone embed print report
  1. #include "I2Cdev.h"
  2.  
  3. #include "MPU6050_6Axis_MotionApps20.h"
  4. //#include "MPU6050.h" // not necessary if using MotionApps include file
  5.  
  6.  
  7. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  8. #include "Wire.h"
  9. #endif
  10.  
  11. MPU6050 mpu;
  12. //MPU6050 mpu(0x69); // <-- use for AD0 high
  13.  
  14. const int trigPin = 9;
  15. const int echoPin = 10;
  16.  
  17. #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
  18. #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
  19. bool blinkState = false;
  20.  
  21. // MPU control/status vars
  22. bool dmpReady = false; // set true if DMP init was successful
  23. uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
  24. uint8_t devStatus;
  25. uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
  26. uint16_t fifoCount; // count of all bytes currently in FIFO
  27. uint8_t fifoBuffer[64]; // FIFO storage buffer
  28.  
  29. // orientation/motion vars
  30. Quaternion q; // [w, x, y, z] quaternion container
  31. VectorInt16 aa; // [x, y, z] accel sensor measurements
  32. VectorInt16 aaReal; // [x, y, z]gravity-free accel sensor measurements
  33. VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
  34. VectorFloat gravity; // [x, y, z] gravity vector
  35. float euler[3]; // [psi, theta, phi] Euler angle container
  36. float ypr[3], yprd[3] = {0, 0, 0};
  37.  
  38. // packet structure for InvenSense teapot demo
  39. uint8_t teapotPacket[14] = {'$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, 'r', 'n' };
  40.  
  41. // ================================================================
  42. // === INTERRUPT DETECTION ROUTINE ===
  43. // ================================================================
  44. volatile bool mpuInterrupt = false;
  45. void dmpDataReady() {
  46. mpuInterrupt = true;
  47. }
  48.  
  49. // ================================================================
  50. // === INITIAL SETUP ===
  51. // ================================================================
  52. int16_t yo = 0, po = 0, ro = 0;
  53. int lv = 1, flag = 0;
  54. const int MPU_addr = 0x68; // I2C address of the MPU-6050
  55. int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
  56. int32_t AcXo = 0, AcYo = 0, AcZo = 0, prev, curr;
  57. int32_t AcXd = 0, AcYd = 0, AcZd = 0;
  58. int i = 1, flaggl = 0;
  59. int glc = 0, lv2 = 1;
  60. long duration;
  61. int distance, distanced;
  62. void setup() {
  63.  
  64. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  65. Wire.begin();
  66. Wire.setClock(400000);
  67. #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  68. Fastwire::setup(400, true);
  69. #endif
  70. Serial.begin(115200);
  71. // initialize device
  72. Serial.println(F("Initializing I2C devices..."));
  73. mpu.initialize();
  74. pinMode(INTERRUPT_PIN, INPUT);
  75. // verify connection
  76. Serial.println(F("Testing device connections..."));
  77. Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  78. // wait for ready
  79. Serial.println(F("nSend any character to begin DMP programming and demo:
  80. "));
  81. while (Serial.available() && Serial.read()); // empty buffer
  82. while (!Serial.available()); // wait for data
  83. while (Serial.available() && Serial.read()); // empty buffer again
  84. // load and configure the DMP
  85. Serial.println(F("Initializing DMP..."));
  86. devStatus = mpu.dmpInitialize();
  87. // supply your own gyro offsets here, scaled for min sensitivity
  88. mpu.setXGyroOffset(220);
  89. mpu.setYGyroOffset(76);
  90. mpu.setZGyroOffset(-85);
  91. mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  92. // make sure it worked (returns 0 if so)
  93. if (devStatus == 0) {
  94. // turn on the DMP, now that it's ready
  95. Serial.println(F("Enabling DMP..."));
  96. mpu.setDMPEnabled(true);
  97.  
  98. // enable Arduino interrupt detection
  99. Serial.println(F("Enabling interrupt detection (Arduino external
  100. interrupt 0)..."));
  101. attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
  102. mpuIntStatus = mpu.getIntStatus();
  103.  
  104. // set our DMP Ready flag so the main loop() function knows it's okay to use it
  105. Serial.println(F("DMP ready! Waiting for first interrupt..."));
  106. dmpReady = true;
  107.  
  108. // get expected DMP packet size for later comparison
  109. packetSize = mpu.dmpGetFIFOPacketSize();
  110. } else {
  111. // ERROR!
  112. // 1 = initial memory load failed
  113. // 2 = DMP configuration updates failed
  114. // (if it's going to break, usually the code will be 1)
  115. Serial.print(F("DMP Initialization failed (code "));
  116. Serial.print(devStatus);
  117. Serial.println(F(")"));
  118. }
  119.  
  120. // configure LED for output
  121. pinMode(LED_PIN, OUTPUT);
  122. glc = 0;
  123. distance = distanced = 0;
  124. }
  125.  
  126. // ================================================================
  127. // === MAIN PROGRAM LOOP ===
  128. // ================================================================
  129. void loop() {
  130. // if programming failed, don't try to do anything
  131. if (!dmpReady) return;
  132.  
  133. // wait for MPU interrupt or extra packet(s) available
  134. while (!mpuInterrupt && fifoCount < packetSize) {
  135. }
  136. mpuInterrupt = false;
  137. mpuIntStatus = mpu.getIntStatus();
  138. // get current FIFO count
  139. fifoCount = mpu.getFIFOCount();
  140. // check for overflow (this should never happen unless our code is too inefficient)
  141. if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  142. // reset so we can continue cleanly
  143. mpu.resetFIFO();
  144. Serial.println(F("FIFO overflow!"));
  145. // otherwise, check for DMP data ready interrupt (this should happen frequently)
  146. } else if (mpuIntStatus & 0x02) {
  147. // wait for correct available data length, should be a VERY short wait
  148. while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  149. // read a packet from FIFO
  150. mpu.getFIFOBytes(fifoBuffer, packetSize);
  151. // track FIFO count here in case there is > 1 packet available
  152. // (this lets us immediately read more without waiting for an interrupt)
  153. fifoCount -= packetSize;
  154. //#ifdef OUTPUT_READABLE_YAWPITCHROLL
  155. // display Euler angles in degrees
  156. mpu.dmpGetQuaternion(&q, fifoBuffer);
  157. mpu.dmpGetGravity(&gravity, &q);
  158. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  159. if (millis() > 30000 && flag == 0) {
  160. if ( flag == 0 && lv < 21) {
  161. yo += ypr[0] * 180 / M_PI;
  162. po += ypr[1] * 180 / M_PI;
  163. ro += ypr[2] * 180 / M_PI;
  164. ++lv;
  165. } else if (flag == 0) {
  166. flag = 1;
  167. yo /= 20;
  168. po /= 20;
  169. ro /= 20;
  170. }
  171. }
  172. //#endif
  173. // blink LED to indicate activity
  174. blinkState = !blinkState;
  175. digitalWrite(LED_PIN, blinkState);
  176. Wire.beginTransmission(MPU_addr);
  177. Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  178. Wire.endTransmission(false);
  179. Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  180. AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  181. AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  182. AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  183. Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  184. GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  185. GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  186. GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  187. mpu.resetFIFO();
  188. if (i < 21 && !flaggl) {
  189. AcXo += AcX;
  190. AcYo += AcY;
  191. AcZo += AcZ;
  192. ++i;
  193. Serial.println();
  194. Serial.println("***************************************************************************");
  195. } else if (!flaggl) {
  196. flaggl = 1;
  197. AcXo = AcXo / 20;
  198. AcYo = AcYo / 20;
  199. AcZo = AcZo / 20;
  200. }
  201. curr = (AcZ - AcZo / 100);
  202. if (prev == curr )
  203. ++glc;
  204. else {
  205. glc = 0;
  206. prev = curr;
  207. }
  208. if (glc == 10) {
  209. Serial.println("lock broken!!!");
  210. glc = 0;
  211. loop();
  212. }
  213. if (flag == 1 && flaggl == 1) {
  214. if (lv2 < 21) {
  215. delayMicroseconds(2);
  216. digitalWrite(trigPin, HIGH);
  217. delayMicroseconds(10);
  218. digitalWrite(trigPin, LOW);
  219. duration = pulseIn(echoPin, HIGH);
  220. distance = duration * 0.034 / 2;
  221. AcXd += (AcX - AcXo) / 100;
  222. AcYd += (AcY - AcYo) / 100;
  223. AcZd += (AcZ - AcZo) / 100;
  224. yprd[0] += ypr[0] * 180 / M_PI - yo;
  225. yprd[1] += ypr[1] * 180 / M_PI - po;
  226. yprd[2] += ypr[2] * 180 / M_PI - ro;
  227. distanced += distance;
  228. ++lv2;
  229. } else {
  230. AcXd /= 20;
  231. AcYd /= 20;
  232. AcZd /= 20;
  233. yprd[0] /= 20;
  234. yprd[1] /= 20;
  235. yprd[2] /= 20;
  236. distanced /= 20;
  237. lv2 = 1;
  238. Serial.print("AcX = ");
  239. Serial.print(AcXd);
  240. Serial.print(" | AcY = ");
  241. Serial.print(AcYd);
  242. Serial.print(" | AcZ = ");
  243. Serial.print(AcZd);
  244. Serial.print(" | yaw = ");
  245. Serial.print(yprd[0]);
  246. Serial.print(" | pitch = ");
  247. Serial.print(yprd[1]);
  248. Serial.print(" | roll = ");
  249. Serial.print(yprd[2]);
  250. Serial.print(" | distance = ");
  251. Serial.println(distanced);
  252. AcXd = 0;
  253. AcYd = 0;
  254. AcZd = 0;
  255. yprd[0] = 0;
  256. yprd[1] = 0;
  257. yprd[2] = 0;
  258. distanced = 0;
  259. }
  260. }
  261. }
  262. }
Add Comment
Please, Sign In to add comment
Public Pastes
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!