紀大帥的奇妙旅程: 02/26/16

OpenGL + adxl345 -> 藉由adxl345來轉動你的茶壺

用adxl345 sensor 可以讀出來 x, y, z 的加速度....

因為地球有重力加速度(垂直往下)....

如果你把adxl345 平放在桌上....你可以發現...x 和 y 軸的加速度為 0....然後z 軸大概為 1G 左右

由於會有些noise.....這個需要做一下校正....

但沒有校正也可以啦....

從參考網址2來看

我們可以得到 roll 和 pitch 的計算公式

roll = (atan2(-fYg, fZg)*180.0)/M_PI;
pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

那甚麼是roll 和 pitch 呢?

請看reference 3 的網站...他有動畫...一看就知道.....

原理是當你轉動你的adxl345

由於地球有重心引力.....sensor 的 x, y ,z 的加速度會因為你的轉動而可得到不同的分量

所以我們可以從adxl345所量測到的x,y,z 的加速度...反推回去目前sensor 和地球座標的夾角...

那有了 roll 和 pitch....也只是一堆數字.....看起來好像不夠酷....

沒關係....我記得我有在arduino 看到一個例子....adxl345 結合 teaspot....

那raspberry 也可以畫 teaspot壓....

找了一下資料...

發覺在下面的目錄有opengl 的例子....

/opt/vc/src/hello_pi

剛好有 hello_teapot.....真是太lucky.....

打開 triangle.c 來看.... 找一下我要改的地方....

static void update_model(CUBE_STATE_T *state,double roll, double pitch)

{

// update position

state->rot_angle_x = inc_and_wrap_angle(state->rot_angle_x, state->rot_angle_x_inc);

state->rot_angle_y = inc_and_wrap_angle(state->rot_angle_y, state->rot_angle_y_inc);

state->rot_angle_z = inc_and_wrap_angle(state->rot_angle_z, state->rot_angle_z_inc);

state->distance = inc_and_clip_distance(state->distance, state->distance_inc);

glLoadIdentity();

// move camera back to see the cube

glTranslatef(0.f, 0.f, -state->distance);

// Rotate model to new position

glRotatef(state->rot_angle_x, 1.f, 0.f, 0.f);

glRotatef(state->rot_angle_y, 0.f, 1.f, 0.f);

glRotatef(state->rot_angle_z, 0.f, 0.f, 1.f);

}

可以看出來... 他會一直去抓 x 和 y 和 z 的angle...... 然後 inc 他....

所以只要在這邊動點手腳.....把最後送到 glRotatef 的值換掉....應該可以了....

修改的code 大概如下 :

static void update_model(CUBE_STATE_T *state,double roll, double pitch)

{

// update position

state->rot_angle_z = inc_and_wrap_angle(state->rot_angle_z, state->rot_angle_z_inc);

state->distance = inc_and_clip_distance(state->distance, state->distance_inc);

glLoadIdentity();

// move camera back to see the cube

glTranslatef(0.f, 0.f, -state->distance);

// Rotate model to new position

glRotatef(roll, 1.f, 0.f, 0.f);

glRotatef(pitch, 0.f, 1.f, 0.f);

glRotatef(state->rot_angle_z, 0.f, 0.f, 1.f);

}

int main(){

.......

struct acc_dat gdata;

double alpha = 0.5f,roll,pitch,fXg = 0.0,fYg = 0.0,fZg = 0.0;

while (!terminate)

{

gdata = getAxes(fd_adxl345, 1);

// low pass filter , to remove noise

fXg = gdata.x * alpha + (fXg * (1.0f - alpha));

fYg = gdata.y * alpha + (fYg * (1.0f - alpha));

fZg = gdata.z * alpha + (fZg * (1.0f - alpha));

//Roll and Pitch Equations

roll = (atan2(-fYg, fZg)*180.0)/M_PI;

pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

update_model(state,roll,pitch);

redraw_scene(state);

}

新增一個 c 的 file -> adxl345.c

#include "./adxl345.h"

void enableMeasurement(char FD_ADDR)

{

wiringPiI2CWriteReg8(FD_ADDR,POWER_CTL,MEASURE);

}

void setBandwidthRate(char FD_ADDR, int rate_flag)

{

wiringPiI2CWriteReg8(FD_ADDR,BW_RATE,rate_flag);

}

void setRange(char FD_ADDR, int range_flag)

{

int value;

value = wiringPiI2CReadReg8(FD_ADDR, DATA_FORMAT);

value &= ~0x0F;

value |= range_flag;

value |= 0x08;

// printf("set range value : %d",value);

wiringPiI2CWriteReg8(FD_ADDR, DATA_FORMAT, value);

}

struct acc_dat getAxes(char FD_ADDR, int gforce)

{

double dx,dy,dz;

struct acc_dat dat;

int x,y,z;

char bytes[6];

bytes[0] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA);

bytes[1] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+1);

bytes[2] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+2);

bytes[3] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+3);

bytes[4] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+4);

bytes[5] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+5);

//printf("%d\n",bytes[0]);

//printf("%d\n",bytes[1]);

x = bytes[0] | (bytes[1] << 8);

if(x & (1<<15))

x = x - (1<<16);

//printf("%d\n",x);

y = bytes[2] | (bytes[3] << 8);

if(y & (1<<15))

y = y - (1<<16);

z = bytes[4] | (bytes[5] << 8);

if(z & (1<<15))

z = z - (1<<16);

dx = (double)x * SCALE_MULTIPLIER;

dy = (double)y * SCALE_MULTIPLIER;

dz = (double)z * SCALE_MULTIPLIER;

if (gforce == 0)

{

dx = dx * EARTH_GRAVITY_MS2;

dy = dy * EARTH_GRAVITY_MS2;

dz = dz * EARTH_GRAVITY_MS2;

}

dat.x = (dx);

dat.y = (dy);

dat.z = (dz);

return dat;

}

然後 adxl345.h 裡面大概就是一些定義...這裡我就不貼了....

然後修改Makefile

OBJS=triangle.o video.o models.o adxl345.o
BIN=hello_teapot.bin
LDFLAGS+=-lilclient
LDFLAGS+=-lwiringPi

include ../Makefile.include

然後就是編譯......

sudo ./make

執行

sudo ./hello_teapot.bin

你就可以在你的螢幕看到一個 teaspot....然後轉動你的 adxl345....你就可以看到茶壺也會跟著轉

demo 的影片在下面的連結

demo video: https://www.youtube.com/watch?v=O3V-0z6UWF0

note 1. teaspot 所需要用的gpu-mem 比較大....請把gpu mem 設到128M.... sudo raspi-config -> Advanced option -> ....

note 2. 我有使用 wiringPi 的library....請先安裝 wiringPi .... http://wiringpi.com/

Reference 2 : http://blog.oscarliang.net/use-gy80-arduino-adxl345-accelerometer/

Reference 3 : http://howthingsfly.si.edu/flight-dynamics/roll-pitch-and-yaw

OpenGL on Raspberry

Reference : https://jan.newmarch.name/LinuxSound/Diversions/RaspberryPiOpenGL/

Raspberry Pi SPI and I2C Tutorial -> using wiringPi

Reference : https://learn.sparkfun.com/tutorials/raspberry-pi-spi-and-i2c-tutorial

using wiringpi to access adxl345 on raspberry

為了速度....所以把 python code 轉成 c code....

但可能bottle-neck 還是在i2c 的protocol 上面

安裝wiringPi 請參考這篇安裝wiringPi

wiringPi 使用I2C 請參考這篇wiringPi use i2c

compile 的指令如下:

gcc -Wall -o adxl345 main.cpp adxl345.cpp -lwiringPi
sudo ./adxl345

sample code 如下:
main.cpp
=======================
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#include <wiringPiI2C.h>
#include <string.h>
#include "./adxl345.h"

int main (void)
{

int fd_adxl345 ;

struct acc_dat gdata ;

fd_adxl345 = wiringPiI2CSetup(0x53);
//printf("%d",fd_adxl345);

ADXL345 adxl345(fd_adxl345);
gdata = adxl345.getAxes(true);

printf("x : %f g\n",gdata.x);
printf("y : %f g\n",gdata.y);
printf("z : %f g\n",gdata.z);

}

=======================

adxl345.h
==============================
#ifndef __ADXL345
#define __ADXL345

#include <wiringPiI2C.h>
#include <math.h>
#include <stdio.h>

#define EARTH_GRAVITY_MS2 9.80665
#define SCALE_MULTIPLIER 0.004

#define DATA_FORMAT 0x31
#define BW_RATE 0x2C
#define POWER_CTL 0x2D

#define BW_RATE_1600HZ 0x0F
#define BW_RATE_800HZ 0x0E
#define BW_RATE_400HZ 0x0D
#define BW_RATE_200HZ 0x0C
#define BW_RATE_100HZ 0x0B
#define BW_RATE_50HZ 0x0A
#define BW_RATE_25HZ 0x09

#define RANGE_2G 0x00
#define RANGE_4G 0x01
#define RANGE_8G 0x02
#define RANGE_16G 0x03

#define MEASURE 0x08
#define AXES_DATA 0x32

struct acc_dat{
double x;
double y;
double z;
};

class ADXL345{

public:
int FD_ADDR;
ADXL345(int addr);
void enableMeasurement();
void setBandwidthRate(int rate_flag);
void setRange(int range_flag);
//struct acc_dat getAxes(bool gforce = false);
struct acc_dat getAxes(bool gforce);
};

#endif

================================================

adxl345.cpp

#include "./adxl345.h"

ADXL345::ADXL345(int addr)

{

FD_ADDR = addr;

setBandwidthRate(BW_RATE_100HZ);

setRange(RANGE_2G);

enableMeasurement();

}

void ADXL345::enableMeasurement()

{

wiringPiI2CWriteReg8(FD_ADDR,POWER_CTL,MEASURE);

}

void ADXL345::setBandwidthRate(int rate_flag)

{

//printf("%d",rate_flag);

wiringPiI2CWriteReg8(FD_ADDR,BW_RATE,rate_flag);

}

void ADXL345::setRange(int range_flag)

{

int value;

value = wiringPiI2CReadReg8(FD_ADDR, DATA_FORMAT);

value &= ~0x0F;

value |= range_flag;

value |= 0x08;

// printf("set range value : %d",value);

wiringPiI2CWriteReg8(FD_ADDR, DATA_FORMAT, value);

}

struct acc_dat ADXL345::getAxes(bool gforce = false)

{

double dx,dy,dz;

struct acc_dat dat;

int x,y,z;

char bytes[6];

bytes[0] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA);

bytes[1] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+1);

bytes[2] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+2);

bytes[3] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+3);

bytes[4] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+4);

bytes[5] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+5);

//printf("%d\n",bytes[0]);

//printf("%d\n",bytes[1]);

x = bytes[0] | (bytes[1] << 8);

if(x & (1<<15))

x = x - (1<<16);

//printf("%d\n",x);

y = bytes[2] | (bytes[3] << 8);

if(y & (1<<15))

y = y - (1<<16);

z = bytes[4] | (bytes[5] << 8);

if(z & (1<<15))

z = z - (1<<16);

dx = (double)x * SCALE_MULTIPLIER;

dy = (double)y * SCALE_MULTIPLIER;

dz = (double)z * SCALE_MULTIPLIER;

if (gforce == false)
{
dx = dx * EARTH_GRAVITY_MS2;
dy = dy * EARTH_GRAVITY_MS2;
dz = dz * EARTH_GRAVITY_MS2;
}
dat.x = (dx);
dat.y = (dy);
dat.z = (dz);

return dat;
}

=============================================

2016年2月26日 星期五

OpenGL + adxl345 -> 藉由adxl345來轉動你的茶壺

OpenGL on Raspberry

Raspberry Pi SPI and I2C Tutorial -> using wiringPi

using wiringpi to access adxl345 on raspberry

2016年2月26日星期五