#include <Wire.h>
#include <MPU6050.h>
int dianji1=3;
int dianji2=5;
int dianji3=6;
int dianji4=9;
int BEEP=13;
MPU6050 mpu;
void setup()
{
Serial.begin(115200);
Serial.println("Initialize MPU6050");
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
checkSettings();
pinMode(dianji1,OUTPUT);
pinMode(dianji2,OUTPUT);
pinMode(dianji3,OUTPUT);
pinMode(dianji4,OUTPUT);
pinMode(BEEP,OUTPUT);
digitalWrite(dianji1,HIGH);
delay(500);
}
void checkSettings()
{
Serial.println();
Serial.print(" * Sleep Mode: ");
Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
Serial.print(" * Clock Source: ");
switch(mpu.getClockSource())
{
case MPU6050_CLOCK_KEEP_RESET:
Serial.println("Stops the clock and keeps the timing generator in reset");
break;
case MPU6050_CLOCK_EXTERNAL_19MHZ:
Serial.println("PLL with external 19.2MHz reference");
break;
case MPU6050_CLOCK_EXTERNAL_32KHZ:
Serial.println("PLL with external 32.768kHz reference");
break;
case MPU6050_CLOCK_PLL_ZGYRO:
Serial.println("PLL with Z axis gyroscope reference");
break;
case MPU6050_CLOCK_PLL_YGYRO:
Serial.println("PLL with Y axis gyroscope reference");
break;
case MPU6050_CLOCK_PLL_XGYRO:
Serial.println("PLL with X axis gyroscope reference");
break;
case MPU6050_CLOCK_INTERNAL_8MHZ:
Serial.println("Internal 8MHz oscillator");
break;
}
Serial.print(" * Accelerometer: ");
switch(mpu.getRange())
{
case MPU6050_RANGE_16G:
Serial.println("+/- 16 g");
break;
case MPU6050_RANGE_8G:
Serial.println("+/- 8 g");
break;
case MPU6050_RANGE_4G:
Serial.println("+/- 4 g");
break;
case MPU6050_RANGE_2G:
Serial.println("+/- 2 g");
break;
}
Serial.print(" * Accelerometer offsets: ");
Serial.print(mpu.getAccelOffsetX());
Serial.print(" / ");
Serial.print(mpu.getAccelOffsetY());
Serial.print(" / ");
Serial.println(mpu.getAccelOffsetZ());
Serial.println();
}
void loop()
{
float jiaodu=0;
Vector normAccel = mpu.readNormalizeAccel();
jiaodu=acos(normAccel.ZAxis/8.8);
delay(50);
LABEL1:
if((acos(normAccel.ZAxis/8.8)-jiaodu)>0)
{
if(-5<jiaodu)
{
LABEL:
digitalWrite(dianji3,HIGH);
digitalWrite(dianji1,LOW);
if(jiaodu>5)
{
digitalWrite(dianji3,LOW);
digitalWrite(dianji1,LOW);
if(jiaodu>20)
{
digitalWrite(BEEP,HIGH);
digitalWrite(dianji3,LOW);
digitalWrite(dianji1,HIGH);
goto LABEL1;
}
else
{
digitalWrite(dianji3,LOW);
digitalWrite(dianji1,LOW);
}
}
else
goto LABEL;
}
else
{
digitalWrite(dianji3,LOW);
digitalWrite(dianji1,LOW);
}
}
else
{
if(jiaodu<5)
{
LABEL2:
digitalWrite(dianji1,HIGH);
digitalWrite(dianji3,LOW);
if(jiaodu<-5)
{
digitalWrite(dianji3,LOW);
digitalWrite(dianji1,LOW);
if(jiaodu<-20)
{
digitalWrite(BEEP,HIGH);
digitalWrite(dianji3,HIGH);
digitalWrite(dianji1,LOW);
goto LABEL1;
}
else
{
digitalWrite(dianji1,LOW);
digitalWrite(dianji3,LOW);
}
}
else
goto LABEL2;
}
}
}
求大神指导,编译通过,无法正常运行,求科普,貌似陷入死循环了 |