2016年2月26日 星期五

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

沒有留言:

張貼留言