[드론 프로젝트] myAHRS 자이로센서 사용하기
myAHRS 위드로봇 홈페이지에서 받아 프로그램을 사용하면 값을 비교적 쉽게 받을 수 있다.
1. myMotion CC 사용하기
2. ComportMaster 사용하기
3. avr4 studio 이용하기
1. 아래의 그림처럼 센서 vcc GND RX TX를 연결해 준다.
이 연결은 따로 소스를 작성하고 mcu를 이용할 필요가 없다.
설치를 하고 실행을 누르면 되는데 한 번 포맷을 하고 나서 되지 않는다. ㅜㅜ 굉장히 사용이 쉬운데...
아래의 저 그림을 클릭하면
위 창이 나오고 여기서 센서의 data를 받아오는 주기나 data를 그래프나 그래픽 형태로 나오게 할 수 있다. 왜 갑자기 저게 안 될까ㅠㅠㅠㅠ
2. ComportMaster 설치를 진행하고 myAHRS에 명령을 주면 센서에서 값을 보내온다.
<MC를 입력하면 센서에서 >M을 보내오고 $로 시작하는 프로트콜로 센서의 data를 보내온다.
3. 다음의 소스 코드로 각 값을 나타내 보았다.
2에서 나온 프로토콜로 소스를 작성한다.
#include <avr/io.h>
#include <util/delay.h>
#define F_CPU 16000000
#define sbi(PORTX, BitX) PORTX|=(1<<BitX)
#define cbi(PORTX, BitX) PORTX&=~(1<<BitX)
void Uart_Init(void);
unsigned char Receive(void);
void Send(unsigned char S_temp);
double myatof(const char *str); //ascill to float
unsigned char R_temp; //uart통신을 수신한 data를 임시로 저정시키는 변수
unsigned char S_temp; //uart통신을 송신할 data를 임시로 저정시키는 변수
//unsigned char data[] = {0,};
//unsigned char ID[2]={0,};
int i = 0;
int main(void)
{
int state = 0;
int flag = 0;
int buf = 0;
int roll_index = 0;
int pitch_index = 0;
int yaw_index = 0;
//Accelation
int A_x_index=0; int A_y_index=0; int A_z_index=0;
//Angular
int G_x_index=0; int G_y_index=0; int G_z_index=0;
//Geomagnetic
int M_x_index=0; int M_y_index=0; int M_z_index=0;
char roll_buffer[7];
char pitch_buffer[7];
char yaw_buffer[7];
//Accelation
char A_x[7]; char A_y[7]; char A_z[7];
//Angular
char G_x[7]; char G_y[7]; char G_z[7];
//Geomagnetic
char M_x[7]; char M_y[7]; char M_z[7];
Uart_Init();
//Send('<'); Send('D'); Send('\n'); //한 data 정보
//Send('<'); Send('I'); Send(' '); Send('O'); Send('_'); Send('R'); Send('A'); Send('T'); Send('E'); Send(' '); Send('0'); Send('\n');//outputdatarate 1Hz
//_delay_ms(5000);
//Send('<'); Send('S');
Send('<'); Send('M'); Send('C'); Send('\n'); //continous
while(1)
{
/*for(i=0;;i++)
{
data[i] = Receive();
if(R_temp == 10) //10 == '\n'
break;
Send(Receive());//Send(data[i]);
}*/
buf = Receive();
if(buf == '$')
{
state = 1;
flag =1;
}
if(flag ==1)
{
if(buf =='\n')
{
flag =0;
state =0;
//roll
//Send('!'); Send(myatof(roll_buffer)); Send('!'); // 아스크키 값이 실수로 바뀜확인
Send('r');Send('o');Send('l');Send('l');Send(' ');
for(i=0;i<roll_index;i++) //값 띄우기
{
Send(roll_buffer[i]);
}
Send(10);
for(i=0;i<7;i++) roll_buffer[i] = 0; //저장한 값 지우기
roll_index = 0;
//pitch
Send('p');Send('i');Send('t');Send('c');Send('h');Send(' ');
for(i=0;i<pitch_index;i++)
{
Send(pitch_buffer[i]);
}
Send(10);
for(i=0;i<7;i++) pitch_buffer[i] = 0;
pitch_index = 0;
//yaw
Send('y');Send('a');Send('w');Send(' ');
for(i=0;i<yaw_index;i++)
{
Send(yaw_buffer[i]);
}
Send(10); //Send(13);
for(i=0;i<7;i++) yaw_buffer[i] = 0;
yaw_index = 0;
//Accellation
Send('a');Send('c');Send('c');Send('e');Send('l');Send('l');Send('l');Send('a');Send('t');Send('i');Send('o');Send('n'); Send(' ');
for(i=0;i<A_x_index;i++)
{
Send(A_x[i]);
}
Send(',');
for(i=0;i<7;i++) A_x[i] = 0;
A_x_index = 0;
for(i=0;i<A_y_index;i++)
{
Send(A_y[i]);
}
Send(',');
for(i=0;i<7;i++) A_y[i] = 0;
A_y_index = 0;
for(i=0;i<A_z_index;i++)
{
Send(A_z[i]);
}
Send(',');
for(i=0;i<7;i++) A_z[i] = 0;
A_z_index = 0;
Send(10); //Send(13);
//Angular
Send('a');Send('n');Send('g');Send('u');Send('l');Send('a');Send('r');
for(i=0;i<G_x_index;i++)
{
Send(G_x[i]);
}
Send(',');
for(i=0;i<7;i++) G_x[i] = 0;
G_x_index = 0;
for(i=0;i<G_y_index;i++)
{
Send(G_y[i]);
}
Send(',');
for(i=0;i<7;i++) A_y[i] = 0;
G_y_index = 0;
for(i=0;i<G_z_index;i++)
{
Send(G_z[i]);
}
Send(',');
for(i=0;i<7;i++) G_z[i] = 0;
G_z_index = 0;
Send(10); //Send(13);
//Geomagnetic
Send('g');Send('e');Send('o');Send('m');Send('a');Send('n');Send('e');Send('t');Send('i');Send('c');Send(' ');
for(i=0;i<M_x_index;i++)
{
Send(M_x[i]);
}
Send(',');
for(i=0;i<7;i++) M_x[i] = 0;
M_x_index = 0;
for(i=0;i<M_y_index;i++)
{
Send(M_y[i]);
}
Send(',');
for(i=0;i<7;i++) M_y[i] = 0;
M_y_index = 0;
for(i=0;i<M_z_index;i++)
{
Send(M_z[i]);
}
Send(',');
for(i=0;i<7;i++) M_z[i] = 0;
M_z_index = 0;
Send(10); Send(13);
}
else if(buf == ',')
{
state++;
}
else
{
switch(state) // 값 저장시키기
{
case 3:
roll_buffer[roll_index] = buf;
roll_index++;
break;
case 4:
pitch_buffer[pitch_index] = buf;
pitch_index++;
break;
case 5:
yaw_buffer[yaw_index] = buf;
yaw_index++;
break;
case 6:
A_x[A_x_index] = buf;
A_x_index++;
break;
case 7:
A_y[A_y_index] = buf;
A_y_index++;
break;
case 8:
A_z[A_z_index] = buf;
A_z_index++;
break;
case 9:
G_x[G_x_index] = buf;
G_x_index++;
break;
case 10:
G_y[G_y_index] = buf;
G_y_index++;
break;
case 11:
G_z[G_z_index] = buf;
G_z_index++;
break;
case 12:
M_x[M_x_index] = buf;
M_x_index++;
break;
case 13:
M_y[M_y_index] = buf;
M_y_index++;
break;
case 14:
M_z[M_z_index] = buf;
M_z_index++;
break;
default:
break;
}
}
}
//_delay_ms(100);
//아스케에서 실수로 변환
//myatof(roll_buffer); myatof(pitch_buffer); myatof(yaw_buffer); myatof(roll_buffer);
//myatof(A_x); myatof(A_y); myatof(A_z);
//myatof(G_x); myatof(G_y); myatof(G_z);
//myatof(M_x); myatof(M_y); myatof(M_z);
}
return 0;
}
void Uart_Init(void)
{
cbi(SREG,7); //모든 인터럽트 비활성화
UBRR0H = 0; UBRR0L = 8; //115.2kbps(16MHz)
UCSR0A = (0<<RXC0) | (1<<UDRE0); //수신,송신 상태비트 초기화
UCSR0B = (1<<RXEN0) | (1<<TXEN0); //수신,송신 기능 활성화
UCSR0C = (3<<UCSZ00); //start 1비트, data8비트. stop1비트
cbi(DDRE,0); //RXD0 핀 입력으로 설정
sbi(DDRE,1); //TXD0 핀 출력으로 설정
sbi(SREG,7); //모든 인터럽트 활성화
}
unsigned char Receive(void) //시리얼로부터 1바이트 값을 (mcu가)받는 함수
{
while(!(UCSR0A&(1<<RXC0)) ); //데이터가 올 때까지 기다린다.
R_temp = UDR0; //수신받은 data를 임시로 저장시켜놓고(다시 새로운 data를 받아 넣을 수 있도록 UDR에서 받은 값 저장) 그 값을 반환
return R_temp; //데이터를 돌려준다.
}
void Send(unsigned char S_temp) //시리얼로부터 1바이트 값을 (mcu가)보내는 함수
{
while(!(UCSR0A&(1<<UDRE0)) ); //데이터가 빌 때까지 기다린다.
UDR0 = S_temp; //데이터를 전송한다.
}
double myatof(const char *str) //ascill to float
{
double num1 =0, num2=0, j=1; //num1은 정수자리 num2는 소수자리, i는 소수점
int flag = 0; //부호판단 위한 변수
if(*str =='-' || *str>='0' && *str<='9') //첫 글자가 =EHsms 0~9이면
{
if(*str == '-')
{
flag++; //음수 부호
str++; //str포인터 변수 증가
}
while(*str != '.' && (*str>='0' && *str<='9'))//str포인터가 -이고 0~9이면
{
num1 = (num1*10) + (*str-'0'); //num1 값 구함
str++;
}
if(*str == '.')
{
str++;
while(*str >='0'&& *str<= '9') //str포인터가 0~9이면
{
num2 = (num2*10)+(*str-'0'); //num2 값 구함
str++; //str포인터 증가
j*=0.1; //루프만큼 소수점 자리 증가
}
}
return !flag ? (num1 + (num2*j)) : -(num1+(num2*j) );//flag값에 따라 리턴값 음 양수 달라짐
}
else
return 0; //실패시 0리턴
}
이런 식으로 나올 것이다.