[原创] 晒设计方案+玩转陀螺仪

sjtitr 2014-2-23 07:51 楼主

STM32F429i Discovery还有一个陀螺仪，如果不能把它也搞起来，那有点太浪费STM的感情了。

好，那么本辑内容我们就来玩玩陀螺仪。

准备工程，就使用示例中的MEMS_Example

通过学习代码，了解到其中这个函数：

static void Demo_MEMS(void)
{
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
Buffer[0] = (int8_t)Buffer[0] - (int8_t)Gyro[0];
Buffer[1] = (int8_t)Buffer[1] - (int8_t)Gyro[1];
/* Update autoreload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
if ( Xval>Yval)
{
if ((int16_t)Buffer[0] > 40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_MAGENTA);
LCD_DrawFullRect(100, 40, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 310);
Delay(50);
}
if ((int16_t)Buffer[0] < -40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_RED);
LCD_DrawFullRect(100, 160, 40, 120);
LCD_FillTriangle(50, 190, 120, 160, 160, 10);
Delay(50);
}
}
else
{
if ((int16_t)Buffer[1] < -40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DrawFullRect(120, 140, 100, 40);
LCD_FillTriangle(120, 120, 5, 60, 260, 160);
Delay(50);
}
if ((int16_t)Buffer[1] > 40)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
LCD_SetTextColor(LCD_COLOR_BLUE);
LCD_DrawFullRect(20, 140, 100, 40);
LCD_FillTriangle(120, 120, 235, 60, 260, 160);
Delay(50);
}
}
}

以上这些就是使用陀螺仪的关键代码啦，关于陀螺仪初始化等等的内容，我们完全可以利用示例工程的资源。

那么接下来我们就来玩转上面这个函数吧。

当然，首先根据示例，我们把三个方向的角度都取出来：

Buffer[0] = Buffer[0] - Gyro[0];
Buffer[1] = Buffer[1] - Gyro[1];
Buffer[2] = Buffer[2] - Gyro[2];

……为了玩转嘛，楼主又准备了一个可以转的函数，这里免费提供给大家。

#include "math.h"
/**************************************************************************
* 3D Cube
***************************************************************************/
#define CUBE_HALF_A 70
typedef unsigned char u8_t;
typedef float pos_t; /* position */
typedef float rad_t; /* radian */
typedef struct{
pos_t x;
pos_t y;
}d2pos_t; /* 2D point type */
typedef struct{
pos_t x;
pos_t y;
pos_t z;
}d3pos_t; /* 3D point type */
typedef struct{
rad_t x;
rad_t y;
rad_t z;
}d3rad_t; /* 3D radian type */
typedef const u8_t * surface_t;
typedef struct{
u8_t vert_count;
u8_t face_count;
d3pos_t *vertexes;
surface_t *surfaces;
d3pos_t pos;
d3rad_t rad;
}poly_t;
d3pos_t cube_pos[8]={
{ CUBE_HALF_A, CUBE_HALF_A, CUBE_HALF_A},
{-CUBE_HALF_A, CUBE_HALF_A, CUBE_HALF_A},
{-CUBE_HALF_A, -CUBE_HALF_A, CUBE_HALF_A},
{ CUBE_HALF_A, -CUBE_HALF_A, CUBE_HALF_A},
{ CUBE_HALF_A, CUBE_HALF_A, -CUBE_HALF_A},
{-CUBE_HALF_A, CUBE_HALF_A, -CUBE_HALF_A},
{-CUBE_HALF_A, -CUBE_HALF_A, -CUBE_HALF_A},
{ CUBE_HALF_A, -CUBE_HALF_A, -CUBE_HALF_A}
};
const u8_t cube_face1[]={0, 1, 2, 3, 0};
const u8_t cube_face2[]={0, 3, 7, 4, 0};
const u8_t cube_face3[]={0, 4, 5, 1, 0};
const u8_t cube_face4[]={6, 5, 4, 7, 6};
const u8_t cube_face5[]={6, 2, 1, 5, 6};
const u8_t cube_face6[]={6, 7, 3, 2, 6};
surface_t cube_faces[6]={
cube_face1,cube_face2,cube_face3,cube_face4,cube_face5,cube_face6
};
poly_t cube_A = {
8,
6,
cube_pos,
cube_faces,
{0,0,0},
{0,0,0}
};
static void draw_line(int ax, int ay, int bx, int by);
void d3_draw(poly_t * poly_p, float eye_z, float screen_z)
{
d2pos_t d2p[8];
int i, j;
float Yn;
float x[8],y[8],z[8],xt,yt,zt,sinax,cosax,sinay,cosay,sinaz,cosaz;
int sx,sy,mx,my,ex,ey;
sinax = sin(poly_p->rad.x); // precalculate the sin and cos values
cosax = cos(poly_p->rad.x);
sinay = sin(poly_p->rad.y);
cosay = cos(poly_p->rad.y);
sinaz = sin(poly_p->rad.z);
cosaz = cos(poly_p->rad.z);
for (i = 0; i < poly_p->vert_count; i++) // translate 3d vertex position to 2d screen position
{
x[i]=poly_p->vertexes[i].x; // get current position x
y[i]=poly_p->vertexes[i].y; // get current position y
z[i]=poly_p->vertexes[i].z; // get current position z
yt = y[i] * cosax - z[i] * sinax; // rotate around X axis
zt = y[i] * sinax + z[i] * cosax;
y[i] = yt;
z[i] = zt;
xt = x[i] * cosay - z[i] * sinay; // rotate around Y axis
zt = x[i] * sinay + z[i] * cosay;
x[i] = xt;
z[i] = zt;
xt = x[i] * cosaz - y[i] * sinaz; // rotate around Z axis
yt = x[i] * sinaz + y[i] * cosaz;
x[i] = xt;
y[i] = yt;
x[i] += poly_p->pos.x; // calc the new position x
y[i] += poly_p->pos.y; // calc the new position y
z[i] += poly_p->pos.z; // calc the new position Z
z[i] = (eye_z - screen_z) / (eye_z - z[i]);
d2p[i].x = x[i] * z[i]; // translate 3d to 2d
d2p[i].y = y[i] * z[i]; // translate 3d to 2d
}
for (i = 0; i < poly_p->face_count; i++) // draw each surface of the object
{
sx = x[poly_p->surfaces[i][0]];
sy = y[poly_p->surfaces[i][0]];
mx = x[poly_p->surfaces[i][1]];
my = y[poly_p->surfaces[i][1]];
ex = x[poly_p->surfaces[i][2]];
ey = y[poly_p->surfaces[i][2]];
Yn = (ex-mx)*(sy-my)-(sx-mx)*(ey-my); // check direction of the surface
if(Yn > 0) // draw it if face to us
{
j = 0;
do{
j++;
sx=d2p[poly_p->surfaces[i][j-1]].x; // set line start x
sy=d2p[poly_p->surfaces[i][j-1]].y; // set line start y
ex=d2p[poly_p->surfaces[i][j]].x; // set line end x
ey=d2p[poly_p->surfaces[i][j]].y; // set line end y
draw_line(sx,sy,ex,ey);
}while(poly_p->surfaces[i][0] != poly_p->surfaces[i][j]);
}
}
}
void draw_line(int ax, int ay, int bx, int by)
{
ax+=110;
ay+=110;
bx+=110;
by+=110;
LCD_DrawUniLine(ax,ay,bx,by);
}

draw_line里面对坐标的修正是因为硬件的坐标原点在角落里，要移动到屏幕中间来，我比较懒，没有移到正中间，而是偏向一侧了。

使用的时候呢，只需要

cube_A.rad.x = rotatex;
cube_A.rad.y = rotatey;
cube_A.rad.z = rotatez;
d3_draw(&cube_A, 10000, 1000);

注意就是那三个参数，第一个表示描画对象；第二个是指视点的坐标，越大透视效果越差但是消隐越正确；第三个是投影面的坐标，应当小于视点坐标，并且越小画面越大。

最后就是玩转了，随个人喜好来玩吧
我就简单转了一小下吧，抛砖引玉啦

本辑没有工程分享啦，因为关键代码全部展示在上面了，自己贴一贴吧。

完。

回复评论

暂无评论，赶紧抢沙发吧