8. MC_E02SM3(ATmega128보드)이용. IMU센서(EBIMU-9DOFV2)와 통신하기.(roll, pitch, yaw 분할버전)

.


내가 쓰고있 MC_E02SM3는 위와 같은 외부 핀 회로도를 갖는다. 여기서 TXD1을 센서의 RX에

RXD1을 센서의 TX에 교차하여 연결한다.





float로 선언된 숫자들은 uart통해서 확인 못함



1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
struct GYRO{
   float r;   //roll
   float p;   //pitch
   float y;   //yaw
   char r_a[10];   //roll값 하나하나 배열에 저장
   char p_a[10];
   char y_a[10];
} gyro;
 
unsigned char data1[100];
 
unsigned char temp_uart1;
 
SIGNAL(SIG_UART1_RECV)      //데이터가 오면 인터럽트가 일어남
{
   temp_uart1=UDR1;            //rx, 차례대로 각각의 자리에 글자를 받아옴
   gyro_sensor();
}   
 
 
void USART0_Transmit_Str(unsigned char data[])
{
   size_t i=0, len=strlen(data);
 
   
   for(i=0;i<len;i++)
   {
     /* Wait for empty transmit buffer */
      while ( !( UCSR0A & (1<<UDRE)) )   ;
 
      /* Put data into buffer, sends the data */
      UDR0 = data[i];  //tx, data 전송
   }
}
 
 
void gyro_sensor()
{
   if(temp_uart1=='*')   // * 나올때
   {
      i++;
      j=0;
   }
   else if(temp_uart1==',')   // , 나올때
   {
      i++;
      j=0;
   }
   else if(temp_uart1==10)   // 엔터 나올때 모두 출력
   {
      //모터 1,3은 pitch, 2,4는 roll      
    //각각의 값에 -48을 해야하는 이유?  -> ASCII 이기 때문
      if(gyro.r_a[0]=='-')   //-roll condition
      {
         if(gyro.r_a[2]=='.')      //-x.xx
            gyro.r = -1 * ( gyro.r_a[1]-48 + (gyro.r_a[3]-48)*0.1 + (gyro.r_a[4]-48)*0.01 );            
         else if(gyro.r_a[3]=='.')   //-xx.xx
            gyro.r = -1 * ( (gyro.r_a[1]-48)*10 + (gyro.r_a[2]-48) + (gyro.r_a[4]-48)*0.1 + (gyro.r_a[5]-48)*0.01 );
         else if(gyro.r_a[4]=='.')   //-xxx.xx
            gyro.r = -1 * ( (gyro.r_a[1]-48)*100 + (gyro.r_a[2]-48)*10 + (gyro.r_a[3]-48) + (gyro.r_a[5]-48)*0.1 + (gyro.r_a[6]-48)*0.01 );
      }
      else if(gyro.r_a[0]!='-')   //+roll condition
      {
         if(gyro.r_a[1]=='.')      //x.xx
            gyro.r = ( (gyro.r_a[0]-48) + (gyro.r_a[2]-48)*0.1 + (gyro.r_a[3]-48)*0.01 );            
         else if(gyro.r_a[2]=='.')   //xx.xx
            gyro.r = ( (gyro.r_a[0]-48)*10 + (gyro.r_a[1]-48) + (gyro.r_a[3]-48)*0.1 + (gyro.r_a[4]-48)*0.01 );
         else if(gyro.r_a[3]=='.')   //xxx.xx
            gyro.r = ( (gyro.r_a[0]-48)*100 + (gyro.r_a[1]-48)*10 + (gyro.r_a[2]-48) + (gyro.r_a[4]-48)*0.1 + (gyro.r_a[5]-48)*0.01 );
      }
 
      if(gyro.p_a[0]=='-')  //-pitch condition
      {
         if(gyro.p_a[2]=='.')
            gyro.p = -1 * ( gyro.p_a[1]-48 + (gyro.p_a[3]-48)*0.1 + (gyro.p_a[4]-48)*0.01 );            
         else if(gyro.p_a[3]=='.')
            gyro.p = -1 * ( (gyro.p_a[1]-48)*10 + (gyro.p_a[2]-48) + (gyro.p_a[4]-48)*0.1 + (gyro.p_a[5]-48)*0.01 );
         else if(gyro.p_a[4]=='.')
            gyro.p = -1 * ( (gyro.p_a[1]-48)*100 + (gyro.p_a[2]-48)*10 + (gyro.p_a[3]-48) + (gyro.p_a[5]-48)*0.1 + (gyro.p_a[6]-48)*0.01 );
      }
      else if(gyro.p_a[0]!='-'//+pitch condition
      {
         if(gyro.p_a[1]=='.')
            gyro.p = ( (gyro.p_a[0]-48) + (gyro.p_a[2]-48)*0.1 + (gyro.p_a[3]-48)*0.01 );            
         else if(gyro.p_a[2]=='.')
            gyro.p = ( (gyro.p_a[0]-48)*10 + (gyro.p_a[1]-48) + (gyro.p_a[3]-48)*0.1 + (gyro.p_a[4]-48)*0.01 );
         else if(gyro.p_a[3]=='.')
            gyro.p = ( (gyro.p_a[0]-48)*100 + (gyro.p_a[1]-48)*10 + (gyro.p_a[2]-48) + (gyro.p_a[4]-48)*0.1 + (gyro.p_a[5]-48)*0.01 );
      }
 
      if(gyro.y_a[0]=='-')  //-yaw condition
      {
         if(gyro.y_a[2]=='.')
            gyro.y = -1 * ( gyro.y_a[1]-48 + (gyro.y_a[3]-48)*0.1 + (gyro.y_a[4]-48)*0.01 );            
         else if(gyro.y_a[3]=='.')
            gyro.y = -1 * ( (gyro.y_a[1]-48)*10 + (gyro.y_a[2]-48) + (gyro.y_a[4]-48)*0.1 + (gyro.y_a[5]-48)*0.01 );
         else if(gyro.y_a[4]=='.')
            gyro.y = -1 * ( (gyro.y_a[1]-48)*100 + (gyro.y_a[2]-48)*10 + (gyro.y_a[3]-48) + (gyro.y_a[5]-48)*0.1 + (gyro.y_a[6]-48)*0.01 );
      }
      else if(gyro.y_a[0]!='-')  //+yaw condition
      {
         if(gyro.y_a[1]=='.')
            gyro.y = ( (gyro.y_a[0]-48) + (gyro.y_a[2]-48)*0.1 + (gyro.y_a[3]-48)*0.01 );            
         else if(gyro.y_a[2]=='.')
            gyro.y = ( (gyro.y_a[0]-48)*10 + (gyro.y_a[1]-48) + (gyro.y_a[3]-48)*0.1 + (gyro.y_a[4]-48)*0.01 );
         else if(gyro.y_a[3]=='.')
            gyro.y = ( (gyro.y_a[0]-48)*100 + (gyro.y_a[1]-48)*10 + (gyro.y_a[2]-48) + (gyro.y_a[4]-48)*0.1 + (gyro.y_a[5]-48)*0.01 );
      }
 
 
      sprintf(data1,"roll=%s, pitch=%s, yaw=%s\n\r",gyro.r_a, gyro.p_a, gyro.y_a);
      USART0_Transmit_Str(data1);
 
 
      
/*      if(gyro.p<0 & gyro.p>-0.5)
      {
      sprintf(data1," 0>pitch>-0.5 \n\r");
      USART0_Transmit_Str(data1);
      }
      else if(gyro.p>0)
      {
      sprintf(data1," 0<pitch \n\r");
      USART0_Transmit_Str(data1);
      }
*/
      i=0;
      j=0;
   }
   else if(i==1)
   {
      gyro.r_a[j]=temp_uart1;
      j++;
   }
   else if(i==2)
   {
      gyro.p_a[j]=temp_uart1;
      j++;
   }
   else if(i==3)
   {
      gyro.y_a[j]=temp_uart1;
      j++;
   }
}


'●프로젝트와 세미나 > IMU센서 마우스(Atmega128)' 카테고리의 다른 글

센서분석  (0) 2014.03.19

설정

트랙백

댓글