晒设计方案+玩转陀螺仪

sjtitr

晒设计方案+玩转陀螺仪 [复制链接]

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

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

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

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

1. static void Demo_MEMS(void)
   
2. {   
   
3.   
   
4.   /* Read Gyro Angular data */
   
5.   Demo_GyroReadAngRate(Buffer);
   
6. 
   
7.   Buffer[0] = (int8_t)Buffer[0] - (int8_t)Gyro[0];
   
8.   Buffer[1] = (int8_t)Buffer[1] - (int8_t)Gyro[1];
   
9.   
   
10.   /* Update autoreload and capture compare registers value*/
    
11.   Xval = ABS((int8_t)(Buffer[0]));
    
12.   Yval = ABS((int8_t)(Buffer[1]));
    
13.   
    
14.   if ( Xval>Yval)
    
15.   {
    
16.     if ((int16_t)Buffer[0] > 40)
    
17.     {
    
18.       /* Clear the LCD */
    
19.       LCD_Clear(LCD_COLOR_WHITE);
    
20.       LCD_SetTextColor(LCD_COLOR_MAGENTA);
    
21.       LCD_DrawFullRect(100, 40, 40, 120);
    
22.       LCD_FillTriangle(50, 190, 120, 160, 160, 310);
    
23.       Delay(50);
    
24.     }
    
25.     if ((int16_t)Buffer[0] < -40)
    
26.     {
    
27.       /* Clear the LCD */
    
28.       LCD_Clear(LCD_COLOR_WHITE);
    
29.       LCD_SetTextColor(LCD_COLOR_RED);
    
30.       LCD_DrawFullRect(100, 160, 40, 120);
    
31.       LCD_FillTriangle(50, 190, 120, 160, 160, 10);
    
32.       Delay(50);
    
33.     }
    
34.   }
    
35.   else
    
36.   {
    
37.     if ((int16_t)Buffer[1] < -40)
    
38.     {
    
39.       /* Clear the LCD */
    
40.       LCD_Clear(LCD_COLOR_WHITE);
    
41.       LCD_SetTextColor(LCD_COLOR_GREEN);
    
42.       LCD_DrawFullRect(120, 140, 100, 40);
    
43.       LCD_FillTriangle(120, 120, 5, 60, 260, 160);      
    
44.       Delay(50);
    
45.     }
    
46.     if ((int16_t)Buffer[1] > 40)
    
47.     {      
    
48.       /* Clear the LCD */
    
49.       LCD_Clear(LCD_COLOR_WHITE);
    
50.       LCD_SetTextColor(LCD_COLOR_BLUE);
    
51.       LCD_DrawFullRect(20, 140, 100, 40);
    
52.       LCD_FillTriangle(120, 120, 235, 60, 260, 160);
    
53.       Delay(50);
    
54.     }
    
55.   }
    
56. }
    

复制代码

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

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

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

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

复制代码

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

1. 
   
2. #include "math.h"
   
3. /**************************************************************************
   
4. *                 3D Cube
   
5. ***************************************************************************/  
   
6. #define CUBE_HALF_A        70
   
7. typedef unsigned char u8_t;
   
8. 
   
9. typedef float pos_t;                /* position */
   
10. typedef float        rad_t;          /* radian */
    
11. 
    
12. typedef struct{
    
13.         pos_t        x;
    
14.         pos_t        y;
    
15. }d2pos_t;                                /* 2D point type */
    
16. 
    
17. typedef struct{
    
18.         pos_t        x;
    
19.         pos_t        y;
    
20.         pos_t        z;
    
21. }d3pos_t;                                /* 3D point type */
    
22. 
    
23. typedef struct{
    
24.         rad_t        x;
    
25.         rad_t        y;
    
26.         rad_t        z;
    
27. }d3rad_t;                        /* 3D radian type */
    
28. 
    
29. typedef const u8_t * surface_t;
    
30. 
    
31. typedef struct{
    
32.         u8_t                vert_count;
    
33.         u8_t                face_count;
    
34.         d3pos_t                *vertexes;
    
35.         surface_t        *surfaces;
    
36.         d3pos_t                pos;
    
37.         d3rad_t                rad;
    
38. }poly_t;
    
39. 
    
40. d3pos_t cube_pos[8]={
    
41.         { CUBE_HALF_A,  CUBE_HALF_A,  CUBE_HALF_A},
    
42.         {-CUBE_HALF_A,  CUBE_HALF_A,  CUBE_HALF_A},
    
43.         {-CUBE_HALF_A, -CUBE_HALF_A,  CUBE_HALF_A},
    
44.         { CUBE_HALF_A, -CUBE_HALF_A,  CUBE_HALF_A},
    
45.         { CUBE_HALF_A,  CUBE_HALF_A, -CUBE_HALF_A},
    
46.         {-CUBE_HALF_A,  CUBE_HALF_A, -CUBE_HALF_A},
    
47.         {-CUBE_HALF_A, -CUBE_HALF_A, -CUBE_HALF_A},
    
48.         { CUBE_HALF_A, -CUBE_HALF_A, -CUBE_HALF_A}
    
49. };
    
50. const u8_t cube_face1[]={0, 1, 2, 3, 0};
    
51. const u8_t cube_face2[]={0, 3, 7, 4, 0};
    
52. const u8_t cube_face3[]={0, 4, 5, 1, 0};
    
53. const u8_t cube_face4[]={6, 5, 4, 7, 6};
    
54. const u8_t cube_face5[]={6, 2, 1, 5, 6};
    
55. const u8_t cube_face6[]={6, 7, 3, 2, 6};
    
56. surface_t        cube_faces[6]={
    
57.         cube_face1,cube_face2,cube_face3,cube_face4,cube_face5,cube_face6
    
58. };
    
59. 
    
60. poly_t cube_A = {
    
61.         8,
    
62.         6,
    
63.         cube_pos,
    
64.         cube_faces,
    
65.         {0,0,0},
    
66.         {0,0,0}
    
67. };
    
68. 
    
69. static void draw_line(int ax, int ay, int bx, int by);
    
70. 
    
71. void d3_draw(poly_t * poly_p, float eye_z, float screen_z)     
    
72. {  
    
73.   d2pos_t d2p[8];
    
74.   int i, j;
    
75.   float Yn;
    
76.   float x[8],y[8],z[8],xt,yt,zt,sinax,cosax,sinay,cosay,sinaz,cosaz;
    
77.   int sx,sy,mx,my,ex,ey;
    
78.    
    
79.   sinax = sin(poly_p->rad.x);               // precalculate the sin and cos values  
    
80.   cosax = cos(poly_p->rad.x);
    
81.    
    
82.   sinay = sin(poly_p->rad.y);
    
83.   cosay = cos(poly_p->rad.y);
    
84.    
    
85.   sinaz = sin(poly_p->rad.z);
    
86.   cosaz = cos(poly_p->rad.z);
    
87.    
    
88.   for (i = 0; i < poly_p->vert_count; i++)  // translate 3d vertex position to 2d screen position  
    
89.   {  
    
90.     x[i]=poly_p->vertexes[i].x;             // get current position x
    
91.     y[i]=poly_p->vertexes[i].y;             // get current position y
    
92.     z[i]=poly_p->vertexes[i].z;             // get current position z
    
93.       
    
94. 
    
95.     yt = y[i] * cosax - z[i] * sinax;       // rotate around X axis  
    
96.     zt = y[i] * sinax + z[i] * cosax;
    
97.     y[i] = yt;  
    
98.     z[i] = zt;  
    
99.       
    
100.     xt = x[i] * cosay - z[i] * sinay;       // rotate around Y axis  
     
101.     zt = x[i] * sinay + z[i] * cosay;
     
102.     x[i] = xt;  
     
103.     z[i] = zt;  
     
104.       
     
105.     xt = x[i] * cosaz - y[i] * sinaz;       // rotate around Z axis  
     
106.     yt = x[i] * sinaz + y[i] * cosaz;
     
107.     x[i] = xt;  
     
108.     y[i] = yt;  
     
109.       
     
110.     x[i] += poly_p->pos.x;                                    // calc the new position x
     
111.     y[i] += poly_p->pos.y;                  // calc the new position y  
     
112.     z[i] += poly_p->pos.z;                  // calc the new position Z  
     
113. 
     
114.     z[i] = (eye_z - screen_z) / (eye_z - z[i]);
     
115.       
     
116.     d2p[i].x = x[i] * z[i];                 // translate 3d to 2d
     
117.     d2p[i].y = y[i] * z[i];                 // translate 3d to 2d
     
118.   }  
     
119.    
     
120.   for (i = 0; i < poly_p->face_count; i++)  // draw each surface of the object  
     
121.   {
     
122.     sx = x[poly_p->surfaces[i][0]];
     
123.     sy = y[poly_p->surfaces[i][0]];
     
124.     mx = x[poly_p->surfaces[i][1]];
     
125.     my = y[poly_p->surfaces[i][1]];
     
126.     ex = x[poly_p->surfaces[i][2]];
     
127.     ey = y[poly_p->surfaces[i][2]];
     
128. 
     
129.     Yn = (ex-mx)*(sy-my)-(sx-mx)*(ey-my);   // check direction of the surface
     
130. 
     
131.     if(Yn > 0)                              // draw it if face to us
     
132.     {
     
133.       j = 0;
     
134.       
     
135.       do{
     
136.         j++;
     
137.         sx=d2p[poly_p->surfaces[i][j-1]].x;         // set line start x
     
138.         sy=d2p[poly_p->surfaces[i][j-1]].y;         // set line start y
     
139.         ex=d2p[poly_p->surfaces[i][j]].x;           // set line end x
     
140.         ey=d2p[poly_p->surfaces[i][j]].y;           // set line end y
     
141.         draw_line(sx,sy,ex,ey);
     
142.       }while(poly_p->surfaces[i][0] != poly_p->surfaces[i][j]);
     
143.     }
     
144.   }  
     
145. }  
     
146. 
     
147. void draw_line(int ax, int ay, int bx, int by)
     
148. {
     
149.         ax+=110;
     
150.         ay+=110;
     
151.         bx+=110;
     
152.         by+=110;
     
153.   LCD_DrawUniLine(ax,ay,bx,by);
     
154. }
     

复制代码

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

使用的时候呢，只需要

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

复制代码

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

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



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

完。