Upload files to ''

Measure_angle_Arduino_MPU6050.ino

@@ -0,0 +1,154 @@
+#include <Wire.h>
+#include <AFMotor.h>
+//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();
+}