- //DataBuffer.h
- #ifndef DataBuffer_H
- #define DataBuffer_H
- #if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
- #else
- #include "WProgram.h"
- #endif
- class DataBuffer{
- public:
- int BUFFER_SIZE;//buffer大小
- int Index;//buffer已存数据位
- byte aAccelBuffer[125];//存储单元
- DataBuffer();//构造函数
- void InitBuffer();//清空buffer
- };
- #endif
- //DataBuffer.cpp
- #include"DataBuffer.h"
- DataBuffer::DataBuffer(){
- BUFFER_SIZE=125;
- Index=2;
- }
- void DataBuffer::InitBuffer(){
- Index = 2;
- for(int i=Index; i<BUFFER_SIZE; i++) {
- aAccelBuffer[i] = 0x00;
- }
- aAccelBuffer[0] = 0xfe;
- aAccelBuffer[1] = 0xfd;
- aAccelBuffer[122] = 0xfd;
- aAccelBuffer[123] = 0xfe;
- aAccelBuffer[124] = 0x00;
- }
- //MPU6050.h
- #ifndef MPU6050_H
- #define MPU6050_H
- #if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
- #else
- #include "WProgram.h"
- #endif
- #include"DataBuffer.h"
- #define MPU6050_ACCEL_XOUT_H 0x3B
- #define MPU6050_PWR_MGMT_1 0x6B
- #define MPU6050_PWR_MGMT_2 0x6C
- #define MPU6050_WHO_AM_I 0x75
- #define MPU6050_I2C_ADDRESS 0x68
- class MPU6050{
- public:
- MPU6050();
- void readFromSensor(DataBuffer * db);
- int MPU6050_read(int start, uint8_t *buffer, int size);
- int MPU6050_write_reg(int reg, uint8_t *data);
- typedef union accel_t_gyro_union {
- struct {
- uint8_t x_accel_h;
- uint8_t x_accel_l;
- uint8_t y_accel_h;
- uint8_t y_accel_l;
- uint8_t z_accel_h;
- uint8_t z_accel_l;
- uint8_t t_h;
- uint8_t t_l;
- uint8_t x_gyro_h;
- uint8_t x_gyro_l;
- uint8_t y_gyro_h;
- uint8_t y_gyro_l;
- uint8_t z_gyro_h;
- uint8_t z_gyro_l;
- } reg;
- struct {
- int x_accel;
- int y_accel;
- int z_accel;
- int temperature;
- int x_gyro;
- int y_gyro;
- int z_gyro;
- } value;
- };
- accel_t_gyro_union accel_t_gyro;
- };
- #endif
- //MPU6050.cpp
- #include"MPU6050.h"
- #include"DataBuffer.h"
- #include <math.h>
- #include <Wire.h>
- #include <SoftwareSerial.h>
- MPU6050::MPU6050(){
-
- };
- int MPU6050::MPU6050_read(int start, uint8_t *buffer, int size)
- {
- int i, n;
- Wire.begin();
- Wire.beginTransmission(MPU6050_I2C_ADDRESS);
- n = Wire.write(start);
- if (n != 1)
- return (-10);
-
- n = Wire.endTransmission(false);
- 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);
- }
- void MPU6050::readFromSensor(DataBuffer * db) {
-
- int error;
- error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
- if(error != 0) {
- Serial.print(F("Read accel, temp and gyro, error = "));
- Serial.println(error,DEC);
- }
- 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);
-
- Serial.print(F("accel x,y,z: "));
- Serial.print(accel_t_gyro.value.x_accel, DEC);
- Serial.print(F(", "));
- Serial.print(accel_t_gyro.value.y_accel, DEC);
- Serial.print(F(", "));
- Serial.print(accel_t_gyro.value.z_accel, DEC);
- Serial.print(F(", at "));
- Serial.print(db->Index);
- Serial.println(F(""));
-
- if((db->Index) < (db->BUFFER_SIZE) && (db->Index) > 1) {
- int tempX = accel_t_gyro.value.x_accel;
- int tempY = accel_t_gyro.value.y_accel;
- int tempZ = accel_t_gyro.value.z_accel;
- char temp = (char)(tempX >> 8);
- if(temp == 0x00)
- temp = 0x7f;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
- temp = (char)(tempX);
- if(temp == 0x00)
- temp = 0x01;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
-
- temp = (char)(tempY >> 8);
- if(temp == 0x00)
- temp = 0x7f;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
- temp = (char)(tempY);
- if(temp == 0x00)
- temp = 0x01;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
-
- temp = (char)(tempZ >> 8);
- if(temp == 0x00)
- temp = 0x7f;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
- temp = (char)(tempZ);
- if(temp == 0x00)
- temp = 0x01;
- db->aAccelBuffer[db->Index] = temp;
- db->Index++;
- }
- }
- int MPU6050::MPU6050_write_reg(int reg, uint8_t *data)
- {
- int error,n;
- Wire.beginTransmission(MPU6050_I2C_ADDRESS);
-
- n = Wire.write(reg);
- if (n != 1)
- {error=-20;return (error);}
-
- n = Wire.write(data, 1);
- if (n != 1)
- {error=-21;return (error);}
-
- error = Wire.endTransmission(true);
- if (error != 0)
- return (error);
- return (error);
- }
- //main file
- #include <math.h>
- #include <Wire.h>
- #include <SoftwareSerial.h>
- #include"DataBuffer.h"
- #include"MPU6050.h"
- #define SENSOR_READ_INTERVAL 50 //��ȡMPU�ļ��ʱ��
- unsigned long prevSensoredTime = 0;//�ϴζ�ȡʱ��
- unsigned long curSensoredTime = 0;//��ǰʱ��
- SoftwareSerial BTSerial(2, 3);
- MPU6050 mpu6050=MPU6050();
- DataBuffer databuffer=DataBuffer();
- void setup() {
- Serial.begin(9600);
- BTSerial.begin(9600);
- Wire.begin();
- int error;
- uint8_t c;
- error = mpu6050.MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
- Serial.print(F("WHO_AM_I : "));
- Serial.print(c,HEX);
- Serial.print(F(", error = "));
- Serial.println(error,DEC);
- error = mpu6050.MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
- Serial.print(F("PWR_MGMT_2 : "));
- Serial.print(c,HEX);
- Serial.print(F(", error = "));
- Serial.println(error,DEC);
- mpu6050.MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
- databuffer.InitBuffer();
- }
- void loop() {
- curSensoredTime = millis();
- while(Wire.available())
- {Wire.read( );}
- if(curSensoredTime - prevSensoredTime > SENSOR_READ_INTERVAL) {
- mpu6050.readFromSensor(&databuffer);
- prevSensoredTime = curSensoredTime;
- if(databuffer.Index >= databuffer.BUFFER_SIZE - 3) {
- sendToRemote(&databuffer);
- databuffer.InitBuffer();
- Serial.println("------------- Send 20 accel data to remote");
- }
- }
- }
- void sendToRemote(DataBuffer * db) {
- BTSerial.write( "accel" );
- BTSerial.write( (char*)(db->aAccelBuffer) );
- }
复制代码 |