Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "I2Cdev.h"
- #include "MPU6050_6Axis_MotionApps20.h"
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- #include "Wire.h"
- #endif
- const int motor1 = 10; //definindo constantes para as portas dos motores e do buzzer
- const int motor2 = 9;
- int buzzer = 6;
- MPU6050 mpu;
- bool blinkState = false;
- // MPU control/status vars
- 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
- // orientation/motion vars
- 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
- // packet structure for InvenSense teapot demo
- 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;
- }
- void setup() {
- pinMode(motor1, OUTPUT); //Configurando pinos dos motores e buzzer
- pinMode(motor2, OUTPUT);
- pinMode(buzzer,OUTPUT);
- Serial.begin(115200);
- configuracaoMPU6050();
- }
- void loop() {
- mpu6050();
- }
- //Declarando funcoes para controle dos motores
- void stopMotors(){ //Funcao para parar os motores
- digitalWrite(motor1, LOW);
- digitalWrite(motor2, LOW);
- }
- void turnMotor1(){// Funcao para acionar apenas o motor 1
- digitalWrite(motor1, LOW);
- analogWrite(motor2, 255);
- }
- void turnMotor2(){ // Funcao para acionar apenas o motor 2
- analogWrite(motor1, 255);
- digitalWrite(motor2, LOW);
- }
- void turnMotors(){ //Funções para acionar os dois motores
- analogWrite(motor1, 255);
- analogWrite(motor2, 255);
- }
- void alarme(int tempo){ //Funcao para tocar o alarme no tempo escolhido em ms
- tone(buzzer,261);
- delay(tempo);
- }
- void desligaAlarme(){ //Funcao para desligar o Alarme
- noTone(buzzer);
- }
- void mpu6050(){
- // if programming failed, don't try to do anything
- if (!dmpReady) return;
- // wait for MPU interrupt or extra packet(s) available
- while (!mpuInterrupt && fifoCount < packetSize) {
- // other program behavior stuff here
- // .
- // .
- // .
- // if you are really paranoid you can frequently test in between other
- // stuff to see if mpuInterrupt is true, and if so, "break;" from the
- // while() loop to immediately process the MPU data
- // .
- // .
- // .
- }
- // reset interrupt flag and get INT_STATUS byte
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
- // get current FIFO count
- fifoCount = mpu.getFIFOCount();
- // check for overflow (this should never happen unless our code is too inefficient)
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
- // reset so we can continue cleanly
- mpu.resetFIFO();
- Serial.println(F("FIFO overflow!"));
- // otherwise, check for DMP data ready interrupt (this should happen frequently)
- } else if (mpuIntStatus & 0x02) {
- // wait for correct available data length, should be a VERY short wait
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
- // read a packet from FIFO
- mpu.getFIFOBytes(fifoBuffer, packetSize);
- // track FIFO count here in case there is > 1 packet available
- // (this lets us immediately read more without waiting for an interrupt)
- fifoCount -= packetSize;
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
- Serial.print("gyro: x = ");
- Serial.print(ypr[0] * 180/M_PI);
- Serial.print(" y = ");
- Serial.print(ypr[1] * 180/M_PI);
- Serial.print(" z = ");
- Serial.print(ypr[2] * 180/M_PI);
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
- mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
- Serial.print(" |||| Accel: X = ");
- Serial.print(aaWorld.x);
- Serial.print(" Y = ");
- Serial.print(aaWorld.y);
- Serial.print(" Z = ");
- Serial.println(aaWorld.z);
- }
- }
- void configuracaoMPU6050(){
- // join I2C bus (I2Cdev library doesn't do this automatically)
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- Wire.begin();
- TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
- #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
- Fastwire::setup(400, true);
- #endif
- // initialize serial communication
- // (115200 chosen because it is required for Teapot Demo output, but it's
- // really up to you depending on your project)
- mpu.initialize();
- devStatus = mpu.dmpInitialize();
- // supply your own gyro offsets here, scaled for min sensitivity
- mpu.setXGyroOffset(220);
- mpu.setYGyroOffset(76);
- mpu.setZGyroOffset(-85);
- mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
- // make sure it worked (returns 0 if so)
- if (devStatus == 0) {
- mpu.setDMPEnabled(true);
- // enable Arduino interrupt detection
- attachInterrupt(0, dmpDataReady, RISING);
- mpuIntStatus = mpu.getIntStatus();
- // set our DMP Ready flag so the main loop() function knows it's okay to use it
- dmpReady = true;
- // get expected DMP packet size for later comparison
- packetSize = mpu.dmpGetFIFOPacketSize();
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement