MPU6050 reaction if greater than or less than parameters

Hi, i’m working on a project for a golf putting practice tool.

I have adapted a code that converts the data from the MPU6050 into an angle reading, I now need to make a vibration feedback happen if the angle (x axis) is less than -2 degrees or more than +2 degrees.

Thankyou


#include<Wire.h>
const int MPU_addr=0x68;
double AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; //These will be the raw data from the MPU6050.
uint32_t timer; //it’s a timer, saved as a big-ass unsigned int. We use it to save times from the “micros()” command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double compAngleX, compAngleY; //These are the angles in the complementary filter
#define degconvert 57.2957786 //there are like 57 degrees in a radian.

void setup() {
// Set up MPU 6050:
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(115200);
delay(100);

//setup starting angle
//1) collect the data
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

//2) calculate pitch and roll
double roll = atan2(AcY, AcZ)*degconvert;
double pitch = atan2(-AcX, AcZ)*degconvert;

//3) set the starting angle to this pitch and roll
double gyroXangle = roll;
double gyroYangle = pitch;
double compAngleX = roll;
double compAngleY = pitch;

//start a timer
timer = micros();
}

void loop() {
//Now begins the main loop.
//Collect raw data from the sensor.
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer’s output to seconds from microseconds, 3)casts the value as a double saved to “dt”.
timer = micros(); //start the timer again so that we can calculate the next dt.

//the next two lines calculate the orientation of the accelerometer relative to the earth and convert the output of atan2 from radians to degrees
//We will use this data to correct any cumulative errors in the orientation that the gyroscope develops.
double roll = atan2(AcY, AcZ)*degconvert;
double pitch = atan2(-AcX, AcZ)*degconvert;

//The gyroscope outputs angular velocities. To convert these velocities from the raw data to deg/second, divide by 131.
//Notice, we’re dividing by a double “131.0” instead of the int 131.
double gyroXrate = GyX/131.0;
double gyroYrate = GyY/131.0;

//THE COMPLEMENTARY FILTER
//This filter calculates the angle based MOSTLY on integrating the angular velocity to an angular displacement.
//dt, recall, is the time between gathering data from the MPU6050. We’ll pretend that the
//angular velocity has remained constant over the time dt, and multiply angular velocity by
//time to get displacement.
//The filter then adds a small correcting factor from the accelerometer (“roll” or “pitch”), so the gyroscope knows which way is down.
compAngleX = 0.99 * (compAngleX + gyroXrate * dt) + 0.01 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.99 * (compAngleY + gyroYrate * dt) + 0.01 * pitch;

Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY);Serial.print("\n");

Did you read this?

Why not?