SHARE
TWEET

Projeto_lilypad

a guest Dec 2nd, 2019 100 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "I2Cdev.h"
  2.  
  3. #include "MPU6050_6Axis_MotionApps20.h"
  4.  
  5. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  6.     #include "Wire.h"
  7. #endif
  8.  
  9. const int motor1  = 10; //definindo constantes para as portas dos motores e do buzzer
  10. const int motor2  = 9;
  11.  
  12. int buzzer = 6;
  13.  
  14. MPU6050 mpu;
  15.  
  16. bool blinkState = false;
  17.  
  18. // MPU control/status vars
  19. bool dmpReady = false;  // set true if DMP init was successful
  20. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  21. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  22. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  23. uint16_t fifoCount;     // count of all bytes currently in FIFO
  24. uint8_t fifoBuffer[64]; // FIFO storage buffer
  25.  
  26. // orientation/motion vars
  27. Quaternion q;           // [w, x, y, z]         quaternion container
  28. VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  29. VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  30. VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  31. VectorFloat gravity;    // [x, y, z]            gravity vector
  32. float euler[3];         // [psi, theta, phi]    Euler angle container
  33. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  34.  
  35. // packet structure for InvenSense teapot demo
  36. uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
  37.  
  38. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
  39. void dmpDataReady() {
  40.     mpuInterrupt = true;
  41. }
  42. void setup() {
  43.  
  44.     pinMode(motor1, OUTPUT); //Configurando pinos dos motores e buzzer
  45.     pinMode(motor2, OUTPUT);      
  46.     pinMode(buzzer,OUTPUT);  
  47.     Serial.begin(115200);
  48.     configuracaoMPU6050();
  49.      
  50. }
  51.  
  52.  
  53. void loop() {
  54.  
  55.   mpu6050();
  56.  
  57. }
  58. //Declarando funcoes para controle dos motores
  59. void stopMotors(){ //Funcao para parar os motores
  60.     digitalWrite(motor1, LOW);
  61.     digitalWrite(motor2, LOW);
  62. }
  63. void turnMotor1(){// Funcao para acionar apenas o motor 1
  64.     digitalWrite(motor1, LOW);
  65.     analogWrite(motor2, 255);
  66.  
  67. }
  68. void turnMotor2(){ // Funcao para acionar apenas o motor 2
  69.     analogWrite(motor1, 255);
  70.     digitalWrite(motor2, LOW);
  71.  
  72.  
  73. }
  74. void turnMotors(){ //Funções para acionar os dois motores
  75.     analogWrite(motor1, 255);
  76.     analogWrite(motor2, 255);
  77. }
  78.  
  79. void alarme(int tempo){ //Funcao para tocar o alarme no tempo escolhido em ms
  80.   tone(buzzer,261);
  81.   delay(tempo);  
  82.    
  83. }
  84.  
  85. void desligaAlarme(){ //Funcao para desligar o Alarme
  86.   noTone(buzzer);
  87. }
  88.  
  89.  
  90.  
  91. void mpu6050(){
  92.     // if programming failed, don't try to do anything
  93.     if (!dmpReady) return;
  94.  
  95.     // wait for MPU interrupt or extra packet(s) available
  96.     while (!mpuInterrupt && fifoCount < packetSize) {
  97.         // other program behavior stuff here
  98.         // .
  99.         // .
  100.         // .
  101.         // if you are really paranoid you can frequently test in between other
  102.         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
  103.         // while() loop to immediately process the MPU data
  104.         // .
  105.         // .
  106.         // .
  107.     }
  108.  
  109.     // reset interrupt flag and get INT_STATUS byte
  110.     mpuInterrupt = false;
  111.     mpuIntStatus = mpu.getIntStatus();
  112.  
  113.     // get current FIFO count
  114.     fifoCount = mpu.getFIFOCount();
  115.  
  116.     // check for overflow (this should never happen unless our code is too inefficient)
  117.     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  118.         // reset so we can continue cleanly
  119.         mpu.resetFIFO();
  120.         Serial.println(F("FIFO overflow!"));
  121.  
  122.     // otherwise, check for DMP data ready interrupt (this should happen frequently)
  123.     } else if (mpuIntStatus & 0x02) {
  124.         // wait for correct available data length, should be a VERY short wait
  125.         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  126.  
  127.         // read a packet from FIFO
  128.         mpu.getFIFOBytes(fifoBuffer, packetSize);
  129.        
  130.         // track FIFO count here in case there is > 1 packet available
  131.         // (this lets us immediately read more without waiting for an interrupt)
  132.         fifoCount -= packetSize;
  133.  
  134.         mpu.dmpGetQuaternion(&q, fifoBuffer);
  135.         mpu.dmpGetGravity(&gravity, &q);
  136.         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  137.         Serial.print("gyro: x = ");
  138.         Serial.print(ypr[0] * 180/M_PI);
  139.         Serial.print("  y = ");
  140.         Serial.print(ypr[1] * 180/M_PI);
  141.         Serial.print("  z = ");
  142.         Serial.print(ypr[2] * 180/M_PI);
  143.  
  144.  
  145.         mpu.dmpGetQuaternion(&q, fifoBuffer);
  146.         mpu.dmpGetAccel(&aa, fifoBuffer);
  147.         mpu.dmpGetGravity(&gravity, &q);
  148.         mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  149.         mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
  150.         Serial.print(" |||| Accel: X = ");
  151.         Serial.print(aaWorld.x);
  152.         Serial.print("  Y = ");
  153.         Serial.print(aaWorld.y);
  154.         Serial.print("  Z = ");
  155.         Serial.println(aaWorld.z);
  156.     }
  157. }
  158.  
  159.  
  160. void configuracaoMPU6050(){
  161.    // join I2C bus (I2Cdev library doesn't do this automatically)
  162.     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  163.         Wire.begin();
  164.         TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  165.     #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  166.         Fastwire::setup(400, true);
  167.     #endif
  168.  
  169.     // initialize serial communication
  170.     // (115200 chosen because it is required for Teapot Demo output, but it's
  171.     // really up to you depending on your project)
  172.    
  173.     mpu.initialize();
  174.    
  175.     devStatus = mpu.dmpInitialize();
  176.  
  177.     // supply your own gyro offsets here, scaled for min sensitivity
  178.     mpu.setXGyroOffset(220);
  179.     mpu.setYGyroOffset(76);
  180.     mpu.setZGyroOffset(-85);
  181.     mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  182.  
  183.     // make sure it worked (returns 0 if so)
  184.     if (devStatus == 0) {
  185.      
  186.         mpu.setDMPEnabled(true);
  187.  
  188.         // enable Arduino interrupt detection
  189.         attachInterrupt(0, dmpDataReady, RISING);
  190.         mpuIntStatus = mpu.getIntStatus();
  191.  
  192.         // set our DMP Ready flag so the main loop() function knows it's okay to use it
  193.         dmpReady = true;
  194.  
  195.         // get expected DMP packet size for later comparison
  196.         packetSize = mpu.dmpGetFIFOPacketSize();
  197.     }
  198. }
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