1 x MPU-6050 gyro + accelerometer sensor (for rotation) 1 x Arduino Uno 1 x breadboard jumper wires
// uses Wire library: http://arduino.cc/en/reference/wire // on Arduino uno: // VCC to 5V; GND to GND // SDA to A4; SCL to A5 // MPU-6050 Accelerometer + Gyro // ----------------------------- // // Adapted from arduino.cc user "Krodal". // June 2012 // Open Source / Public Domain // // Using Arduino 1.0.1 // It will not work with an older version, // since Wire.endTransmission() uses a parameter // to hold or release the I2C bus. // // Documentation: // - The InvenSense documents: // - "MPU-6000 and MPU-6050 Product Specification", // PS-MPU-6000A.pdf // - "MPU-6000 and MPU-6050 Register Map and Descriptions", // RM-MPU-6000A.pdf or RS-MPU-6000A.pdf // - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide" // AN-MPU-6000EVB.pdf // // The accuracy is 16-bits. // // Temperature sensor from -40 to +85 degrees Celsius // 340 per degrees, -512 at 35 degrees. // // At power-up, all registers are zero, except these two: // Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero). // Register 0x75 (WHO_AM_I) = 0x68. // #include// The name of the sensor is "MPU-6050". // For program code, I omit the '-', // therefor I use the name "MPU6050....". // Register names according to the datasheet. // According to the InvenSense document // "MPU-6000 and MPU-6050 Register Map // and Descriptions Revision 3.2", there are no registers // at 0x02 ... 0x18, but according other information // the registers in that unknown area are for gain // and offsets. // #define MPU6050_AUX_VDDIO 0x01 // R/W #define MPU6050_SMPLRT_DIV 0x19 // R/W #define MPU6050_CONFIG 0x1A // R/W #define MPU6050_GYRO_CONFIG 0x1B // R/W #define MPU6050_ACCEL_CONFIG 0x1C // R/W #define MPU6050_FF_THR 0x1D // R/W #define MPU6050_FF_DUR 0x1E // R/W #define MPU6050_MOT_THR 0x1F // R/W #define MPU6050_MOT_DUR 0x20 // R/W #define MPU6050_ZRMOT_THR 0x21 // R/W #define MPU6050_ZRMOT_DUR 0x22 // R/W #define MPU6050_FIFO_EN 0x23 // R/W #define MPU6050_I2C_MST_CTRL 0x24 // R/W #define MPU6050_I2C_SLV0_ADDR 0x25 // R/W #define MPU6050_I2C_SLV0_REG 0x26 // R/W #define MPU6050_I2C_SLV0_CTRL 0x27 // R/W #define MPU6050_I2C_SLV1_ADDR 0x28 // R/W #define MPU6050_I2C_SLV1_REG 0x29 // R/W #define MPU6050_I2C_SLV1_CTRL 0x2A // R/W #define MPU6050_I2C_SLV2_ADDR 0x2B // R/W #define MPU6050_I2C_SLV2_REG 0x2C // R/W #define MPU6050_I2C_SLV2_CTRL 0x2D // R/W #define MPU6050_I2C_SLV3_ADDR 0x2E // R/W #define MPU6050_I2C_SLV3_REG 0x2F // R/W #define MPU6050_I2C_SLV3_CTRL 0x30 // R/W #define MPU6050_I2C_SLV4_ADDR 0x31 // R/W #define MPU6050_I2C_SLV4_REG 0x32 // R/W #define MPU6050_I2C_SLV4_DO 0x33 // R/W #define MPU6050_I2C_SLV4_CTRL 0x34 // R/W #define MPU6050_I2C_SLV4_DI 0x35 // R #define MPU6050_I2C_MST_STATUS 0x36 // R #define MPU6050_INT_PIN_CFG 0x37 // R/W #define MPU6050_INT_ENABLE 0x38 // R/W #define MPU6050_INT_STATUS 0x3A // R #define MPU6050_ACCEL_XOUT_H 0x3B // R #define MPU6050_ACCEL_XOUT_L 0x3C // R #define MPU6050_ACCEL_YOUT_H 0x3D // R #define MPU6050_ACCEL_YOUT_L 0x3E // R #define MPU6050_ACCEL_ZOUT_H 0x3F // R #define MPU6050_ACCEL_ZOUT_L 0x40 // R #define MPU6050_TEMP_OUT_H 0x41 // R #define MPU6050_TEMP_OUT_L 0x42 // R #define MPU6050_GYRO_XOUT_H 0x43 // R #define MPU6050_GYRO_XOUT_L 0x44 // R #define MPU6050_GYRO_YOUT_H 0x45 // R #define MPU6050_GYRO_YOUT_L 0x46 // R #define MPU6050_GYRO_ZOUT_H 0x47 // R #define MPU6050_GYRO_ZOUT_L 0x48 // R #define MPU6050_EXT_SENS_DATA_00 0x49 // R #define MPU6050_EXT_SENS_DATA_01 0x4A // R #define MPU6050_EXT_SENS_DATA_02 0x4B // R #define MPU6050_EXT_SENS_DATA_03 0x4C // R #define MPU6050_EXT_SENS_DATA_04 0x4D // R #define MPU6050_EXT_SENS_DATA_05 0x4E // R #define MPU6050_EXT_SENS_DATA_06 0x4F // R #define MPU6050_EXT_SENS_DATA_07 0x50 // R #define MPU6050_EXT_SENS_DATA_08 0x51 // R #define MPU6050_EXT_SENS_DATA_09 0x52 // R #define MPU6050_EXT_SENS_DATA_10 0x53 // R #define MPU6050_EXT_SENS_DATA_11 0x54 // R #define MPU6050_EXT_SENS_DATA_12 0x55 // R #define MPU6050_EXT_SENS_DATA_13 0x56 // R #define MPU6050_EXT_SENS_DATA_14 0x57 // R #define MPU6050_EXT_SENS_DATA_15 0x58 // R #define MPU6050_EXT_SENS_DATA_16 0x59 // R #define MPU6050_EXT_SENS_DATA_17 0x5A // R #define MPU6050_EXT_SENS_DATA_18 0x5B // R #define MPU6050_EXT_SENS_DATA_19 0x5C // R #define MPU6050_EXT_SENS_DATA_20 0x5D // R #define MPU6050_EXT_SENS_DATA_21 0x5E // R #define MPU6050_EXT_SENS_DATA_22 0x5F // R #define MPU6050_EXT_SENS_DATA_23 0x60 // R #define MPU6050_MOT_DETECT_STATUS 0x61 // R #define MPU6050_I2C_SLV0_DO 0x63 // R/W #define MPU6050_I2C_SLV1_DO 0x64 // R/W #define MPU6050_I2C_SLV2_DO 0x65 // R/W #define MPU6050_I2C_SLV3_DO 0x66 // R/W #define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W #define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W #define MPU6050_MOT_DETECT_CTRL 0x69 // R/W #define MPU6050_USER_CTRL 0x6A // R/W #define MPU6050_PWR_MGMT_1 0x6B // R/W #define MPU6050_PWR_MGMT_2 0x6C // R/W #define MPU6050_FIFO_COUNTH 0x72 // R/W #define MPU6050_FIFO_COUNTL 0x73 // R/W #define MPU6050_FIFO_R_W 0x74 // R/W #define MPU6050_WHO_AM_I 0x75 // R // Defines for the bits, to be able to change // between bit number and binary definition. // By using the bit number, programming the sensor // is like programming the AVR microcontroller. // But instead of using "(1< 40 ) digitalWrite( xLED_PIN, HIGH ); else digitalWrite( xLED_PIN, LOW ); if ( yRotation > 40 ) digitalWrite( yLED_PIN, HIGH ); else digitalWrite( yLED_PIN, LOW ); // Delay so we don't swamp the serial port delay(1000); } void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro) { last_read_time = time; last_x_angle = x; last_y_angle = y; last_z_angle = z; last_gyro_x_angle = x_gyro; last_gyro_y_angle = y_gyro; last_gyro_z_angle = z_gyro; } inline unsigned long get_last_time() {return last_read_time;} inline float get_last_x_angle() {return last_x_angle;} inline float get_last_y_angle() {return last_y_angle;} inline float get_last_z_angle() {return last_z_angle;} inline float get_last_gyro_x_angle() {return last_gyro_x_angle;} inline float get_last_gyro_y_angle() {return last_gyro_y_angle;} inline float get_last_gyro_z_angle() {return last_gyro_z_angle;} // Use the following global variables and access functions // to calibrate the acceleration sensor float base_x_accel; float base_y_accel; float base_z_accel; float base_x_gyro; float base_y_gyro; float base_z_gyro; int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) { // Read the raw values. // Read 14 bytes at once, // containing acceleration, temperature and gyro. // With the default settings of the MPU-6050, // there is no filter enabled, and the values // are not very stable. Returns the error value accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr; int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro)); // Swap all high and low bytes. // After this, the registers values are swapped, // so the structure name like x_accel_l does no // longer contain the lower byte. uint8_t swap; #define SWAP(x,y) swap = x; x = y; y = swap SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l); SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l); SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l); SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l); SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l); SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l); SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l); return error; } // The sensor should be motionless on a horizontal surface // while calibration is happening void calibrate_sensors() { int num_readings = 10; float x_accel = 0; float y_accel = 0; float z_accel = 0; float x_gyro = 0; float y_gyro = 0; float z_gyro = 0; accel_t_gyro_union accel_t_gyro; //Serial.println("Starting Calibration"); // Discard the first set of values read from the IMU read_gyro_accel_vals((uint8_t *) &accel_t_gyro); // Read and average the raw values from the IMU for (int i = 0; i < num_readings; i++) { read_gyro_accel_vals((uint8_t *) &accel_t_gyro); x_accel += accel_t_gyro.value.x_accel; y_accel += accel_t_gyro.value.y_accel; z_accel += accel_t_gyro.value.z_accel; x_gyro += accel_t_gyro.value.x_gyro; y_gyro += accel_t_gyro.value.y_gyro; z_gyro += accel_t_gyro.value.z_gyro; delay(100); } x_accel /= num_readings; y_accel /= num_readings; z_accel /= num_readings; x_gyro /= num_readings; y_gyro /= num_readings; z_gyro /= num_readings; // Store the raw calibration values globally base_x_accel = x_accel; base_y_accel = y_accel; base_z_accel = z_accel; base_x_gyro = x_gyro; base_y_gyro = y_gyro; base_z_gyro = z_gyro; Serial.println("Finishing Calibration"); } void initTiltSensor() { int error; uint8_t c; // Initialize the 'Wire' class for the I2C-bus. Wire.begin(); pinMode( xLED_PIN, OUTPUT ); pinMode( yLED_PIN, OUTPUT ); // default at power-up: // Gyro at 250 degrees second // Acceleration at 2g // Clock source at internal 8MHz // The device is in sleep mode. // error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); // According to the datasheet, the 'sleep' bit // should read a '1'. But I read a '0'. // That bit has to be cleared, since the sensor // is in sleep mode at power-up. Even if the // bit reads '0'. error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); // Clear the 'sleep' bit to start the sensor. MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); //Initialize the angles calibrate_sensors(); set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0); } void readTiltData() { int error; double dT; accel_t_gyro_union accel_t_gyro; // Read the raw values. error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); // Get the time of reading for rotation computations unsigned long t_now = millis(); // Convert gyro values to degrees/sec float FS_SEL = 131; float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; // Get raw acceleration values //float G_CONVERT = 16384; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; // Get angle values from accelerometer float RADIANS_TO_DEGREES = 180/3.14159; // float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_z = 0; // Compute the (filtered) gyro angles float dt =(t_now - get_last_time())/1000.0; float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); // Apply the complementary filter to figure out the change in angle - choice of alpha is // estimated now. Alpha depends on the sampling rate... float alpha = 0.96; float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle // Update the saved data with the latest values set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); // save in global variables xRotation = angle_x; yRotation = angle_y; } // -------------------------------------------------------- // MPU6050_read // // This is a common function to read multiple bytes // from an I2C device. // // It uses the boolean parameter for Wire.endTransMission() // to be able to hold or release the I2C-bus. // This is implemented in Arduino 1.0.1. // // Only this function is used to read. // There is no function for a single byte. // int MPU6050_read(int start, uint8_t *buffer, int size) { int i, n, error; Wire.beginTransmission(MPU6050_I2C_ADDRESS); n = Wire.write(start); if (n != 1) return (-10); n = Wire.endTransmission(false); // hold the I2C-bus if (n != 0) return (n); // Third parameter is true: relase I2C-bus after data is read. Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); i = 0; while(Wire.available() && i< size) { buffer[i++]=Wire.read(); } if ( i != size) return (-11); return (0); // return : no error } // -------------------------------------------------------- // MPU6050_write // // This is a common function to write multiple bytes to an I2C device. // // If only a single register is written, // use the function MPU_6050_write_reg(). // // Parameters: // start : Start address, use a define for the register // pData : A pointer to the data to write. // size : The number of bytes to write. // // If only a single register is written, a pointer // to the data has to be used, and the size is // a single byte: // int data = 0; // the data to write // MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1); // int MPU6050_write(int start, const uint8_t *pData, int size) { int n, error; Wire.beginTransmission(MPU6050_I2C_ADDRESS); n = Wire.write(start); // write the start address if (n != 1) return (-20); n = Wire.write(pData, size); // write data bytes if (n != size) return (-21); error = Wire.endTransmission(true); // release the I2C-bus if (error != 0) return (error); return (0); // return : no error } // -------------------------------------------------------- // MPU6050_write_reg // // An extra function to write a single register. // It is just a wrapper around the MPU_6050_write() // function, and it is only a convenient function // to make it easier to write a single register. // int MPU6050_write_reg(int reg, uint8_t data) { int error; error = MPU6050_write(reg, &data, 1); return (error); }