재미있어보여서 저장 자이로센서
출처 : http://kocoafab.cc/tutorial/view/239
개요
출처 : www.dx.com
GY-521 MPU 6050모듈은 자이로 센서와 가속도계가 합쳐진 모듈입니다.
자이로 센서는 x,y,z축을 기준으로 변화하는 방향에 따른 정보를 제공하는 센서입니다.
측정 된 데이터 값의 정확성을 높이기 위해서 자이로센서는 주로 가속도계나 자력계와 결합합니다.
자이로 센서 모듈은 굉장히 많은 곳에 이용할 수 있습니다.
특히 어떤 움직임을 감지하여 사용 할 때 많이 이용할 수 있습니다.
위 사진들 처럼 핸들, 손가락 등을 사용해서 게임을 하거나 물체를 움직일 때 많이 사용됩니다.
본문에서는 GY-521 MPU 6050 모듈을 연결한 후 값을 간단히 시리얼 모니터로 출력해보고
다음은 프로세싱으로 움직임에 따른 변화를 확인해 보겠습니다.
미리보기 동영상
시작 전 개념이해하기
부품목록
NO | 부품명 | 수량 |
1 | 오렌지 보드 | 1 |
2 | 브레드 보드 | 1 |
3 | GY-521 MPU 6050 모듈 | 1 |
4 | 점퍼 케이블 | 5 |
부품명 | 오렌지 보드 | 브레드 보드 | GY-521 MPU 6050 | 점퍼 케이블 |
사진 | X1 | X1 | X1 | X5 |
하드웨어 Making
회로도
브레드보드 레이아웃
소프트웨어 Coding
아래의 스케치를 아두이노에 업로드 하세요.
먼저 연결후 값을 간단하게 출력하는 스케치 입니다.
/* 본 스케치는 arduino.cc의 JoghChi님이 작성하신 스케치입니다.*/ #include<Wire.h> const int MPU=0x68; //MPU 6050 의 I2C 기본 주소 int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; void setup(){ Wire.begin(); //Wire 라이브러리 초기화 Wire.beginTransmission(MPU); //MPU로 데이터 전송 시작 Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); //MPU-6050 시작 모드로 Wire.endTransmission(true); Serial.begin(9600); } void loop(){ Wire.beginTransmission(MPU); //데이터 전송시작 Wire.write(0x3B); // register 0x3B (ACCEL_XOUT_H), 큐에 데이터 기록 Wire.endTransmission(false); //연결유지 Wire.requestFrom(MPU,14,true); //MPU에 데이터 요청 //데이터 한 바이트 씩 읽어서 반환 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) //시리얼 모니터에 출력 Serial.print("AcX = "); Serial.print(AcX); Serial.print(" | AcY = "); Serial.print(AcY); Serial.print(" | AcZ = "); Serial.print(AcZ); Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); Serial.print(" | GyX = "); Serial.print(GyX); Serial.print(" | GyY = "); Serial.print(GyY); Serial.print(" | GyZ = "); Serial.println(GyZ); delay(333); }
소프트웨어 및 하드웨어 설명
1.하드웨어 설명
아두이노와 다른 장치들 사이에서 I2C 통신을 하기 위해서는 SCL 부분과 A5를 연결하고SDA부분을 A4와 연결해야 합니다.
2. 스케치 설명
위 스케치는 간단하게 센서로 값을 읽어오고 시리얼 모니터에 출력하는 예제입니다.
이 센서는 I2C통신을 사용하여 데이터를 송수신 할 수 있습니다.
Wire 라이브러리는 이런 I2C 장치들 사이의 통신 기능을 구현해 놓은 기본 라이브러리 입니다.
자세한 설명은 다음 링크를 참고하세요. I2C 통신
void loop(){
Wire.beginTransmission(MPU); //데이터 전송시작 Wire.write(0x3B); // register 0x3B (ACCEL_XOUT_H), 큐에 데이터 기록 Wire.endTransmission(false); //연결유지 Wire.requestFrom(MPU,14,true); //MPU에 데이터 요청 //데이터 한 바이트 씩 읽어서 반환 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)
이 스케치에서 알아야 하는 것은 GY-521모듈을 사용하기 위해서는 Wire라이브러리를 사용해야 한다는 사실입니다.
Wire 라이브러리를 사용하기 위한 초기 세팅은 setup함수 부분을 그대로 사용하면 됩니다.
loop함수를 보시면 Wire라이브러리를 이용한 통신이 어떻게 일어나는 지 알 수있습니다.
먼저 beginTransmission함수를 통해 데이터 전송이 시작 됩니다.
데이터 전송은 아직 실제로 일어나는 것이 아니라 write 함수로 전송 버퍼에 데이터를 기록한 후
endTransmission함수가 호출 된 후에 실제 전송이 읽어 납니다.
read함수는 requestForm함수 호출에 의해 전송딘 데이터를 한 바이트 씩 읽어서 반환하는 함수입니다.
이런 설명이 이해가지 않더라도 이 스케치를 그대로 복사하여 사용하시고 AcX 등 변수에
실제 데이터 값이 저장되어 있다는 것만을 아시면 됩니다.
그 후 이 변수들을 사용하여 시리얼 모니터, LCD등으로 출력하시면 됩니다.
첫 번째 스케치 시리얼 모니터 결과 화면입니다.
다음은 두 번째 스케치를 보겠습니다.
/*GY-521 모듈 사용하기 본 스케치는 arduino.cc의 Krodal님의 스케치를 바탕으로 작성되었습니다. */ #include<Wire.h> //아두이노 기본 라이브러리 Wire를 포함해야 합니다. // MPU-6050 레지스터 map #define MPU6050_AUX_VDDIO 0x01 #define MPU6050_SMPLRT_DIV 0x19 #define MPU6050_CONFIG 0x1A #define MPU6050_GYRO_CONFIG 0x1B #define MPU6050_ACCEL_CONFIG 0x1C #define MPU6050_FF_THR 0x1D #define MPU6050_FF_DUR 0x1E #define MPU6050_MOT_THR 0x1F #define MPU6050_MOT_DUR 0x20 #define MPU6050_ZRMOT_THR 0x21 #define MPU6050_ZRMOT_DUR 0x22 #define MPU6050_FIFO_EN 0x23 #define MPU6050_I2C_MST_CTRL 0x24 #define MPU6050_I2C_SLV0_ADDR 0x25 #define MPU6050_I2C_SLV0_REG 0x26 #define MPU6050_I2C_SLV0_CTRL 0x27 #define MPU6050_I2C_SLV1_ADDR 0x28 #define MPU6050_I2C_SLV1_REG 0x29 #define MPU6050_I2C_SLV1_CTRL 0x2A #define MPU6050_I2C_SLV2_ADDR 0x2B #define MPU6050_I2C_SLV2_REG 0x2C #define MPU6050_I2C_SLV2_CTRL 0x2D #define MPU6050_I2C_SLV3_ADDR 0x2E #define MPU6050_I2C_SLV3_REG 0x2F #define MPU6050_I2C_SLV3_CTRL 0x30 #define MPU6050_I2C_SLV4_ADDR 0x31 #define MPU6050_I2C_SLV4_REG 0x32 #define MPU6050_I2C_SLV4_DO 0x33 #define MPU6050_I2C_SLV4_CTRL 0x34 #define MPU6050_I2C_SLV4_DI 0x35 #define MPU6050_I2C_MST_STATUS 0x36 #define MPU6050_INT_PIN_CFG 0x37 #define MPU6050_INT_ENABLE 0x38 #define MPU6050_INT_STATUS 0x3A #define MPU6050_ACCEL_XOUT_H 0x3B #define MPU6050_ACCEL_XOUT_L 0x3C #define MPU6050_ACCEL_YOUT_H 0x3D #define MPU6050_ACCEL_YOUT_L 0x3E #define MPU6050_ACCEL_ZOUT_H 0x3F #define MPU6050_ACCEL_ZOUT_L 0x40 #define MPU6050_TEMP_OUT_H 0x41 #define MPU6050_TEMP_OUT_L 0x42 #define MPU6050_GYRO_XOUT_H 0x43 #define MPU6050_GYRO_XOUT_L 0x44 #define MPU6050_GYRO_YOUT_H 0x45 #define MPU6050_GYRO_YOUT_L 0x46 #define MPU6050_GYRO_ZOUT_H 0x47 #define MPU6050_GYRO_ZOUT_L 0x48 #define MPU6050_EXT_SENS_DATA_00 0x49 #define MPU6050_EXT_SENS_DATA_01 0x4A #define MPU6050_EXT_SENS_DATA_02 0x4B #define MPU6050_EXT_SENS_DATA_03 0x4C #define MPU6050_EXT_SENS_DATA_04 0x4D #define MPU6050_EXT_SENS_DATA_05 0x4E #define MPU6050_EXT_SENS_DATA_06 0x4F #define MPU6050_EXT_SENS_DATA_07 0x50 #define MPU6050_EXT_SENS_DATA_08 0x51 #define MPU6050_EXT_SENS_DATA_09 0x52 #define MPU6050_EXT_SENS_DATA_10 0x53 #define MPU6050_EXT_SENS_DATA_11 0x54 #define MPU6050_EXT_SENS_DATA_12 0x55 #define MPU6050_EXT_SENS_DATA_13 0x56 #define MPU6050_EXT_SENS_DATA_14 0x57 #define MPU6050_EXT_SENS_DATA_15 0x58 #define MPU6050_EXT_SENS_DATA_16 0x59 #define MPU6050_EXT_SENS_DATA_17 0x5A #define MPU6050_EXT_SENS_DATA_18 0x5B #define MPU6050_EXT_SENS_DATA_19 0x5C #define MPU6050_EXT_SENS_DATA_20 0x5D #define MPU6050_EXT_SENS_DATA_21 0x5E #define MPU6050_EXT_SENS_DATA_22 0x5F #define MPU6050_EXT_SENS_DATA_23 0x60 #define MPU6050_MOT_DETECT_STATUS 0x61 #define MPU6050_I2C_SLV0_DO 0x63 #define MPU6050_I2C_SLV1_DO 0x64 #define MPU6050_I2C_SLV2_DO 0x65 #define MPU6050_I2C_SLV3_DO 0x66 #define MPU6050_I2C_MST_DELAY_CTRL 0x67 #define MPU6050_SIGNAL_PATH_RESET 0x68 #define MPU6050_MOT_DETECT_CTRL 0x69 #define MPU6050_USER_CTRL 0x6A #define MPU6050_PWR_MGMT_1 0x6B #define MPU6050_PWR_MGMT_2 0x6C #define MPU6050_FIFO_COUNTH 0x72 #define MPU6050_FIFO_COUNTL 0x73 #define MPU6050_FIFO_R_W 0x74 #define MPU6050_WHO_AM_I 0x75 // 센서 사용시 사용할 변수 #define MPU6050_D0 0 #define MPU6050_D1 1 #define MPU6050_D2 2 #define MPU6050_D3 3 #define MPU6050_D4 4 #define MPU6050_D5 5 #define MPU6050_D6 6 #define MPU6050_D7 7 // AUX_VDDIO Register #define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC // CONFIG Register // DLPF은 Digital Low Pass Filter #define MPU6050_DLPF_CFG0 MPU6050_D0 #define MPU6050_DLPF_CFG1 MPU6050_D1 #define MPU6050_DLPF_CFG2 MPU6050_D2 #define MPU6050_EXT_SYNC_SET0 MPU6050_D3 #define MPU6050_EXT_SYNC_SET1 MPU6050_D4 #define MPU6050_EXT_SYNC_SET2 MPU6050_D5 //EXT_SYNC_SET 변수 #define MPU6050_EXT_SYNC_SET_0 (0) #define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1)) #define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2)) #define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)) #define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_DISABLED MPU6050_EXT_SYNC_SET_0 #define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1 #define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2 #define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3 #define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4 #define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5 #define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6 #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7 //DLPF_CFG 변수 #define MPU6050_DLPF_CFG_0 (0) #define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0)) #define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1)) #define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) #define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2)) #define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0)) #define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)) #define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) #define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0 #define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1 #define MPU6050_DLPF_94HZ MPU6050_DLPF_CFG_2 #define MPU6050_DLPF_44HZ MPU6050_DLPF_CFG_3 #define MPU6050_DLPF_21HZ MPU6050_DLPF_CFG_4 #define MPU6050_DLPF_10HZ MPU6050_DLPF_CFG_5 #define MPU6050_DLPF_5HZ MPU6050_DLPF_CFG_6 #define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7 // GYRO_CONFIG Register #define MPU6050_FS_SEL0 MPU6050_D3 #define MPU6050_FS_SEL1 MPU6050_D4 #define MPU6050_ZG_ST MPU6050_D5 #define MPU6050_YG_ST MPU6050_D6 #define MPU6050_XG_ST MPU6050_D7 //the FS_SEL 변수 #define MPU6050_FS_SEL_0 (0) #define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0)) #define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1)) #define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0)) #define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0 #define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1 #define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2 #define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3 // ACCEL_CONFIG Register #define MPU6050_ACCEL_HPF0 MPU6050_D0 #define MPU6050_ACCEL_HPF1 MPU6050_D1 #define MPU6050_ACCEL_HPF2 MPU6050_D2 #define MPU6050_AFS_SEL0 MPU6050_D3 #define MPU6050_AFS_SEL1 MPU6050_D4 #define MPU6050_ZA_ST MPU6050_D5 #define MPU6050_YA_ST MPU6050_D6 #define MPU6050_XA_ST MPU6050_D7 //ACCEL_HPF 변수 #define MPU6050_ACCEL_HPF_0 (0) #define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0)) #define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1)) #define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) #define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2)) #define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) #define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0 #define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1 #define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2 #define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3 #define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4 #define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7 //AFS_SEL 변수 #define MPU6050_AFS_SEL_0 (0) #define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0)) #define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1)) #define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0)) #define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0 #define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1 #define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2 #define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3 // FIFO_EN Register #define MPU6050_SLV0_FIFO_EN MPU6050_D0 #define MPU6050_SLV1_FIFO_EN MPU6050_D1 #define MPU6050_SLV2_FIFO_EN MPU6050_D2 #define MPU6050_ACCEL_FIFO_EN MPU6050_D3 #define MPU6050_ZG_FIFO_EN MPU6050_D4 #define MPU6050_YG_FIFO_EN MPU6050_D5 #define MPU6050_XG_FIFO_EN MPU6050_D6 #define MPU6050_TEMP_FIFO_EN MPU6050_D7 // I2C_MST_CTRL Register #define MPU6050_I2C_MST_CLK0 MPU6050_D0 #define MPU6050_I2C_MST_CLK1 MPU6050_D1 #define MPU6050_I2C_MST_CLK2 MPU6050_D2 #define MPU6050_I2C_MST_CLK3 MPU6050_D3 #define MPU6050_I2C_MST_P_NSR MPU6050_D4 #define MPU6050_SLV_3_FIFO_EN MPU6050_D5 #define MPU6050_WAIT_FOR_ES MPU6050_D6 #define MPU6050_MULT_MST_EN MPU6050_D7 //I2C_MST_CLK #define MPU6050_I2C_MST_CLK_0 (0) #define MPU6050_I2C_MST_CLK_1 (bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_2 (bit(MPU6050_I2C_MST_CLK1)) #define MPU6050_I2C_MST_CLK_3 (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_4 (bit(MPU6050_I2C_MST_CLK2)) #define MPU6050_I2C_MST_CLK_5 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_6 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) #define MPU6050_I2C_MST_CLK_7 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_8 (bit(MPU6050_I2C_MST_CLK3)) #define MPU6050_I2C_MST_CLK_9 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)) #define MPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)) #define MPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) #define MPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0 #define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1 #define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2 #define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3 #define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4 #define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5 #define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6 #define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7 #define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8 #define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9 #define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10 #define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11 #define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12 #define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13 #define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14 #define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15 // I2C_SLV0_ADDR Register #define MPU6050_I2C_SLV0_RW MPU6050_D7 // I2C_SLV0_CTRL Register #define MPU6050_I2C_SLV0_LEN0 MPU6050_D0 #define MPU6050_I2C_SLV0_LEN1 MPU6050_D1 #define MPU6050_I2C_SLV0_LEN2 MPU6050_D2 #define MPU6050_I2C_SLV0_LEN3 MPU6050_D3 #define MPU6050_I2C_SLV0_GRP MPU6050_D4 #define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6 #define MPU6050_I2C_SLV0_EN MPU6050_D7 // A mask for the length #define MPU6050_I2C_SLV0_LEN_MASK 0x0F // I2C_SLV1_ADDR Register #define MPU6050_I2C_SLV1_RW MPU6050_D7 // I2C_SLV1_CTRL Register #define MPU6050_I2C_SLV1_LEN0 MPU6050_D0 #define MPU6050_I2C_SLV1_LEN1 MPU6050_D1 #define MPU6050_I2C_SLV1_LEN2 MPU6050_D2 #define MPU6050_I2C_SLV1_LEN3 MPU6050_D3 #define MPU6050_I2C_SLV1_GRP MPU6050_D4 #define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6 #define MPU6050_I2C_SLV1_EN MPU6050_D7 // A mask for the length #define MPU6050_I2C_SLV1_LEN_MASK 0x0F // I2C_SLV2_ADDR Register #define MPU6050_I2C_SLV2_RW MPU6050_D7 // I2C_SLV2_CTRL Register #define MPU6050_I2C_SLV2_LEN0 MPU6050_D0 #define MPU6050_I2C_SLV2_LEN1 MPU6050_D1 #define MPU6050_I2C_SLV2_LEN2 MPU6050_D2 #define MPU6050_I2C_SLV2_LEN3 MPU6050_D3 #define MPU6050_I2C_SLV2_GRP MPU6050_D4 #define MPU6050_I2C_SLV2_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV2_BYTE_SW MPU6050_D6 #define MPU6050_I2C_SLV2_EN MPU6050_D7 // A mask for the length #define MPU6050_I2C_SLV2_LEN_MASK 0x0F // I2C_SLV3_ADDR Register #define MPU6050_I2C_SLV3_RW MPU6050_D7 // I2C_SLV3_CTRL Register #define MPU6050_I2C_SLV3_LEN0 MPU6050_D0 #define MPU6050_I2C_SLV3_LEN1 MPU6050_D1 #define MPU6050_I2C_SLV3_LEN2 MPU6050_D2 #define MPU6050_I2C_SLV3_LEN3 MPU6050_D3 #define MPU6050_I2C_SLV3_GRP MPU6050_D4 #define MPU6050_I2C_SLV3_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV3_BYTE_SW MPU6050_D6 #define MPU6050_I2C_SLV3_EN MPU6050_D7 // A mask for the length #define MPU6050_I2C_SLV3_LEN_MASK 0x0F // I2C_SLV4_ADDR Register #define MPU6050_I2C_SLV4_RW MPU6050_D7 // I2C_SLV4_CTRL Register #define MPU6050_I2C_MST_DLY0 MPU6050_D0 #define MPU6050_I2C_MST_DLY1 MPU6050_D1 #define MPU6050_I2C_MST_DLY2 MPU6050_D2 #define MPU6050_I2C_MST_DLY3 MPU6050_D3 #define MPU6050_I2C_MST_DLY4 MPU6050_D4 #define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV4_INT_EN MPU6050_D6 #define MPU6050_I2C_SLV4_EN MPU6050_D7 // A mask for the delay #define MPU6050_I2C_MST_DLY_MASK 0x1F // I2C_MST_STATUS Register #define MPU6050_I2C_SLV0_NACK MPU6050_D0 #define MPU6050_I2C_SLV1_NACK MPU6050_D1 #define MPU6050_I2C_SLV2_NACK MPU6050_D2 #define MPU6050_I2C_SLV3_NACK MPU6050_D3 #define MPU6050_I2C_SLV4_NACK MPU6050_D4 #define MPU6050_I2C_LOST_ARB MPU6050_D5 #define MPU6050_I2C_SLV4_DONE MPU6050_D6 #define MPU6050_PASS_THROUGH MPU6050_D7 // I2C_PIN_CFG Register #define MPU6050_CLKOUT_EN MPU6050_D0 #define MPU6050_I2C_BYPASS_EN MPU6050_D1 #define MPU6050_FSYNC_INT_EN MPU6050_D2 #define MPU6050_FSYNC_INT_LEVEL MPU6050_D3 #define MPU6050_INT_RD_CLEAR MPU6050_D4 #define MPU6050_LATCH_INT_EN MPU6050_D5 #define MPU6050_INT_OPEN MPU6050_D6 #define MPU6050_INT_LEVEL MPU6050_D7 // INT_ENABLE Register #define MPU6050_DATA_RDY_EN MPU6050_D0 #define MPU6050_I2C_MST_INT_EN MPU6050_D3 #define MPU6050_FIFO_OFLOW_EN MPU6050_D4 #define MPU6050_ZMOT_EN MPU6050_D5 #define MPU6050_MOT_EN MPU6050_D6 #define MPU6050_FF_EN MPU6050_D7 // INT_STATUS Register #define MPU6050_DATA_RDY_INT MPU6050_D0 #define MPU6050_I2C_MST_INT MPU6050_D3 #define MPU6050_FIFO_OFLOW_INT MPU6050_D4 #define MPU6050_ZMOT_INT MPU6050_D5 #define MPU6050_MOT_INT MPU6050_D6 #define MPU6050_FF_INT MPU6050_D7 // MOT_DETECT_STATUS Register #define MPU6050_MOT_ZRMOT MPU6050_D0 #define MPU6050_MOT_ZPOS MPU6050_D2 #define MPU6050_MOT_ZNEG MPU6050_D3 #define MPU6050_MOT_YPOS MPU6050_D4 #define MPU6050_MOT_YNEG MPU6050_D5 #define MPU6050_MOT_XPOS MPU6050_D6 #define MPU6050_MOT_XNEG MPU6050_D7 // IC2_MST_DELAY_CTRL Register #define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0 #define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1 #define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2 #define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3 #define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4 #define MPU6050_DELAY_ES_SHADOW MPU6050_D7 // SIGNAL_PATH_RESET Register #define MPU6050_TEMP_RESET MPU6050_D0 #define MPU6050_ACCEL_RESET MPU6050_D1 #define MPU6050_GYRO_RESET MPU6050_D2 // MOT_DETECT_CTRL Register #define MPU6050_MOT_COUNT0 MPU6050_D0 #define MPU6050_MOT_COUNT1 MPU6050_D1 #define MPU6050_FF_COUNT0 MPU6050_D2 #define MPU6050_FF_COUNT1 MPU6050_D3 #define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4 #define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5 //MOT_COUNT #define MPU6050_MOT_COUNT_0 (0) #define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) #define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0 //FF_COUNT #define MPU6050_FF_COUNT_0 (0) #define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) #define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 //ACCEL_ON_DELAY #define MPU6050_ACCEL_ON_DELAY_0 (0) #define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) #define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) #define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0)) #define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 #define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 #define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 #define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 // USER_CTRL Register #define MPU6050_SIG_COND_RESET MPU6050_D0 #define MPU6050_I2C_MST_RESET MPU6050_D1 #define MPU6050_FIFO_RESET MPU6050_D2 #define MPU6050_I2C_IF_DIS MPU6050_D4 #define MPU6050_I2C_MST_EN MPU6050_D5 #define MPU6050_FIFO_EN MPU6050_D6 // PWR_MGMT_1 Register #define MPU6050_CLKSEL0 MPU6050_D0 #define MPU6050_CLKSEL1 MPU6050_D1 #define MPU6050_CLKSEL2 MPU6050_D2 #define MPU6050_TEMP_DIS MPU6050_D3 #define MPU6050_CYCLE MPU6050_D5 #define MPU6050_SLEEP MPU6050_D6 #define MPU6050_DEVICE_RESET MPU6050_D7 //CLKSEL #define MPU6050_CLKSEL_0 (0) #define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) #define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 #define MPU6050_CLKSEL_X MPU6050_CLKSEL_1 #define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2 #define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3 #define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 #define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 #define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 #define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7 // PWR_MGMT_2 Register #define MPU6050_STBY_ZG MPU6050_D0 #define MPU6050_STBY_YG MPU6050_D1 #define MPU6050_STBY_XG MPU6050_D2 #define MPU6050_STBY_ZA MPU6050_D3 #define MPU6050_STBY_YA MPU6050_D4 #define MPU6050_STBY_XA MPU6050_D5 #define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 #define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 //LP_WAKE_CTRL #define MPU6050_LP_WAKE_CTRL_0 (0) #define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) #define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 #define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 #define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2 #define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 //MPU-6050의 기본 I2C 주소는 0x68. #define MPU6050_I2C_ADDRESS 0x68 //저항, axis 변수들을 선언 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; }; //변경된 각도와 최종 각도를 저장할 변수 unsigned long last_read_time; float last_x_angle; float last_y_angle; float last_z_angle; float last_gyro_x_angle; float last_gyro_y_angle; float last_gyro_z_angle; //각도를 바꾸는 함수 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;} //센서에서 바로 읽어온 값을 저장할 변수 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) { 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)); //high byte와 low 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; } //움직임이 없을 때 일차적으로 값을 읽어와 전역변수에 저장 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; //첫번째 부분 읽어오기 read_gyro_accel_vals((uint8_t *) &accel_t_gyro); //원시 데이터들의 평균 읽기 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; //전역 변수에 저장 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; } void setup() { int error; uint8_t c; Serial.begin(19200); Wire.begin(); error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); //센서 시작 모드 MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); //각도 초기화 calibrate_sensors(); set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0); } void loop() { int error; double dT; accel_t_gyro_union accel_t_gyro; error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); //회전을 했을 떄 시간 알기 unsigned long t_now = millis(); //원시 데이터를 각도로 변환 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; //acceleration 원시 데이터 저장 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; //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; //gyro angles 계산1 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(); //gyro angles 계산2 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(); //알파를 이용해서 최종 각도 계산3 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는 z-angle 없음 //최종 각도 저장 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); //프로세싱으로 데이터 보내기 Serial.print(F("DEL:")); //Delta T Serial.print(dt, DEC); Serial.print(F("#ACC:")); //Accelerometer angle Serial.print(accel_angle_x, 2); Serial.print(F(",")); Serial.print(accel_angle_y, 2); Serial.print(F(",")); Serial.print(accel_angle_z, 2); Serial.print(F("#GYR:")); Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_y, 2); Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_z, 2); Serial.print(F("#FIL:")); //Filtered angle Serial.print(angle_x, 2); Serial.print(F(",")); Serial.print(angle_y, 2); Serial.print(F(",")); Serial.print(angle_z, 2); Serial.println(F("")); delay(5); } //MPU6050에서 데이터 읽기 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); if (n != 0) return (n); Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); i = 0; while(Wire.available() && i//전송 버퍼에 데이터 기록하는 함수 int MPU6050_write(int start, const uint8_t *pData, int size) { int n, error; Wire.beginTransmission(MPU6050_I2C_ADDRESS); n = Wire.write(start); if (n != 1) return (-20); n = Wire.write(pData, size); if (n != size) return (-21); error = Wire.endTransmission(true); // release the I2C-bus if (error != 0) return (error); return (0); // return : no error } //싱글 레지스터에 기록하기 위한 함수 int MPU6050_write_reg(int reg, uint8_t data) { int error; error = MPU6050_write(reg, &data, 1); return (error); }
다음은 프로세싱 스케치입니다.
/*
Show GY521 Data.
본 스케치는 http://www.geekmomprojects.com/에서 참고 하였습니다.
*/
import processing.serial.*;
Serial myPort;
int lf = 10;
String inString;
int calibrating;
float dt;
float x_gyr; //Gyroscope data
float y_gyr;
float z_gyr;
float x_acc; //Accelerometer data
float y_acc;
float z_acc;
float x_fil; //Filtered data
float y_fil;
float z_fil;
void setup() {
//P3D 사용 3D표현
size(1400, 800, P3D);
stroke(0,0,0);
colorMode(RGB, 256);
String portName = Serial.list()[1]; //포트 번호 지정
println(Serial.list()); //포트 리스트 출력
myPort = new Serial(this, portName, 19200);
myPort.clear();
myPort.bufferUntil(lf);
}
//도형 색깔 및 위치 지정
void draw_rect(int r, int g, int b) {
scale(90);
beginShape(QUADS);
fill(r, g, b);
vertex(-1, 1.5, 0.25);
vertex( 1, 1.5, 0.25);
vertex( 1, -1.5, 0.25);
vertex(-1, -1.5, 0.25);
endShape();
}
void draw() {
background(0);
int distance = 50;
int x_rotation = 90;
//gyro 값과 사각형
pushMatrix();
translate(width/6, height/2, -50);
rotateX(radians(-x_gyr - x_rotation));
rotateY(radians(-y_gyr));
draw_rect(249, 250, 50);
popMatrix();
//accel 값과 사각형
pushMatrix();
translate(width/2, height/2, -50);
rotateX(radians(-x_acc - x_rotation));
rotateY(radians(-y_acc));
draw_rect(56, 140, 206);
popMatrix();
//둘을 합친 값과 사각형
pushMatrix();
translate(5*width/6, height/2, -50);
rotateX(radians(-x_fil - x_rotation));
rotateY(radians(-y_fil));
draw_rect(93, 175, 83);
popMatrix();
//값 스트링 형태로 저장
textSize(24);
String accStr = "(" + (int) x_acc + ", " + (int) y_acc + ")";
String gyrStr = "(" + (int) x_gyr + ", " + (int) y_gyr + ")";
String filStr = "(" + (int) x_fil + ", " + (int) y_fil + ")";
//값 표시
fill(249, 250, 50);
text("Gyroscope", (int) width/6.0 - 60, 25);
text(gyrStr, (int) (width/6.0) - 40, 50);
fill(56, 140, 206);
text("Accelerometer", (int) width/2.0 - 50, 25);
text(accStr, (int) (width/2.0) - 30, 50);
fill(83, 175, 93);
text("Combination", (int) (5.0*width/6.0) - 40, 25);
text(filStr, (int) (5.0*width/6.0) - 20, 50);
}
//데이터 받아오기
void serialEvent(Serial p) {
inString = (myPort.readString());
try {
String[] dataStrings = split(inString, '#');
for (int i = 0; i < dataStrings.length; i++) {
String type = dataStrings[i].substring(0, 4);
String dataval = dataStrings[i].substring(4);
if (type.equals("DEL:")) {
dt = float(dataval);
} else if (type.equals("ACC:")) { //Accelerometer값 가져오기
String data[] = split(dataval, ',');
x_acc = float(data[0]);
y_acc = float(data[1]);
z_acc = float(data[2]);
} else if (type.equals("GYR:")) { //자이로 값 가져오기
String data[] = split(dataval, ',');
x_gyr = float(data[0]);
y_gyr = float(data[1]);
z_gyr = float(data[2]);
} else if (type.equals("FIL:")) { //최종 값 가져오기
String data[] = split(dataval, ',');
x_fil = float(data[0]);
y_fil = float(data[1]);
z_fil = float(data[2]);
}
}
} catch (Exception e) {
println("Caught Exception");
}
}
소프트웨어 및 하드웨어 설명
1. 스케치 설명
본 스케치의 양이 엄청나죠? 그리고 굉장히 어렵습니다.
Wire 라이브러리를 사용하기 때문에 read, write 부분이 있습니다.
사실 스케치에 대해 정확히 이해할 필요는 없습니다.
스케치에서 위에 여러 변수 정의하는 부분들은 잘 몰라도 좋습니다.
원본 데이터가 어떻게 계산이 되는 지도 잘 몰라도 좋습니다.
//프로세싱으로 데이터 보내기 Serial.print(F("DEL:")); //Delta T Serial.print(dt, DEC); Serial.print(F("#ACC:")); //Accelerometer angle Serial.print(accel_angle_x, 2); Serial.print(F(",")); Serial.print(accel_angle_y, 2); Serial.print(F(",")); Serial.print(accel_angle_z, 2); Serial.print(F("#GYR:")); Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_y, 2); Serial.print(F(",")); Serial.print(unfiltered_gyro_angle_z, 2); Serial.print(F("#FIL:")); //Filtered angle Serial.print(angle_x, 2); Serial.print(F(",")); Serial.print(angle_y, 2); Serial.print(F(",")); Serial.print(angle_z, 2); Serial.println(F(""));
저희가 사용할 것은 최종 계산된 값이기 때문입니다.
나중에 이 센서를 사용하실 경우 위 스케치를 그대로 복사하시고 윗 부분에서
accel_angle_x(accelerometer 각도 값),
unfiltered_gyro_angle_x(gyroscope 원본 데이터 값),
anlgle_x(최종 각도 값) 각각 무슨 값인지 알고 사용하실 수 있기만 하면 됩니다.
자이로 센서로부터 바로 읽어온 값은 시간이 지날수록 정확성이 떨어집니다.
그래서 accelerometer의 값을 같이 이용해서 값을 측정해야 정확합니다.
마지막 anlgle_x는 최종적으로 두 값을 이용하여 측정된 값입니다.
이것은 프로세싱을 실행시키면 더 자세히 아실 수 있을 것입니다.
프로세싱을 실행하시면 세개의 사각형이 나옵니다.
각각 세 가지 값에 맞게 움직입니다.
'게임개발 > 3D 기술' 카테고리의 다른 글
ProtoStar (0) | 2016.03.09 |
---|---|
Z-depth With Alpha MaxScript (0) | 2015.10.12 |
현실 + 가상현실로 더욱 리얼해진 게임 The void (0) | 2015.10.04 |
Physically Based Rendering for Artists (0) | 2015.10.04 |
ObjectPooling 서비스 (0) | 2015.10.04 |