diff --git a/Measure_angle_Arduino_MPU6050.ino b/Measure_angle_Arduino_MPU6050.ino new file mode 100644 index 0000000..59ae6b4 --- /dev/null +++ b/Measure_angle_Arduino_MPU6050.ino @@ -0,0 +1,154 @@ +#include +#include +//Declaring some global variables +int gyro_x, gyro_y, gyro_z = 0; +long gyro_x_cal, gyro_y_cal, gyro_z_cal = 0.0; +boolean set_gyro_angles; + +long acc_x, acc_y, acc_z, acc_total_vector = 0.0; +float angle_roll_acc, angle_pitch_acc = 0.0; + +float angle_pitch, angle_roll = 0.0; +int angle_pitch_buffer, angle_roll_buffer = 0; +float angle_pitch_output, angle_roll_output = 0.0; + +long loop_timer = 0.0; +int temp = 0; +AF_DCMotor motor1(1); +AF_DCMotor motor2(2); +AF_DCMotor motor3(3); +AF_DCMotor motor4(4); + + + +void setup() { + Wire.begin(); //Start I2C as master + setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 + for (int cal_int = 0; cal_int < 1000 ; cal_int ++) { //Read the raw acc and gyro data from the MPU-6050 for 1000 times + read_mpu_6050_data(); + gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable + gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable + gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable + delay(3); //Delay 3us to have 250Hz for-loop + + motor1.setSpeed(200); + motor2.setSpeed(200); + motor3.setSpeed(200); + motor4.setSpeed(200); + } + + // divide by 1000 to get avarage offset + gyro_x_cal /= 1000; + gyro_y_cal /= 1000; + gyro_z_cal /= 1000; + Serial.begin(9600); + loop_timer = micros(); //Reset the loop timer +} + +void loop() { + + read_mpu_6050_data(); + //Subtract the offset values from the raw gyro values + gyro_x -= gyro_x_cal; + gyro_y -= gyro_y_cal; + gyro_z -= gyro_z_cal; + + //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5) + angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable + angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable + //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians + angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel + angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel + + //Accelerometer angle calculations + acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector + //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians + angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296; //Calculate the pitch angle + angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296; //Calculate the roll angle + + angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch + angle_roll_acc -= 0.0; //Accelerometer calibration value for roll + + if (set_gyro_angles) { //If the IMU is already started + angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle + angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle + } + else { //At first start + angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle + angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle + set_gyro_angles = true; //Set the IMU started flag + } + + //To dampen the pitch and roll angles a complementary filter is used + angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch_acc * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value + angle_roll_output = angle_roll_output * 0.9 + angle_roll_acc * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value + Serial.print(" | Angle = "); Serial.println(angle_pitch_output); + + + + while (micros() - loop_timer < 10); + {//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop + loop_timer = micros();//Reset the loop timer + } + if (angle_pitch_output > 3) + { + motor1.run(FORWARD); + motor2.run(FORWARD); + motor3.run(FORWARD); + motor4.run(FORWARD); + + } + else if (angle_pitch_output < -3) + { + motor1.run(BACKWARD); + motor2.run(BACKWARD); + motor3.run(BACKWARD); + motor4.run(BACKWARD); + + } + else if (angle_pitch_output >= -3 || angle_pitch_output <= 3) + { + motor1.run(RELEASE); + motor2.run(RELEASE); + motor3.run(RELEASE); + motor4.run(RELEASE); + + } +} + + + + +void setup_mpu_6050_registers() { + //Activate the MPU-6050 + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x6B); //Send the requested starting register + Wire.write(0x00); //Set the requested starting register + Wire.endTransmission(); + //Configure the accelerometer (+/-8g) + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x1C); //Send the requested starting register + Wire.write(0x10); //Set the requested starting register + Wire.endTransmission(); + //Configure the gyro (500dps full scale) + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x1B); //Send the requested starting register + Wire.write(0x08); //Set the requested starting register + Wire.endTransmission(); +} + + +void read_mpu_6050_data() { //Subroutine for reading the raw gyro and accelerometer data + Wire.beginTransmission(0x68); //Start communicating with the MPU-6050 + Wire.write(0x3B); //Send the requested starting register + Wire.endTransmission(); //End the transmission + Wire.requestFrom(0x68, 14); //Request 14 bytes from the MPU-6050 + while (Wire.available() < 14); //Wait until all the bytes are received + acc_x = Wire.read() << 8 | Wire.read(); + acc_y = Wire.read() << 8 | Wire.read(); + acc_z = Wire.read() << 8 | Wire.read(); + temp = Wire.read() << 8 | Wire.read(); + gyro_x = Wire.read() << 8 | Wire.read(); + gyro_y = Wire.read() << 8 | Wire.read(); + gyro_z = Wire.read() << 8 | Wire.read(); +}