|
글쓴이 :
NEWTC
작성일 : 10-07-29 12:08
조회 : 11,561
|
안녕하세요?
( 소스의 목적)
아래소스는 3.3V 기준 전압으로 0~3.3V 사이를 AD하여 계산하는 자이로센서로 각도 구하는 소스입니다. 5V 기준 전압시는 아래 소스 설명을 참고하여, 수정하여 사용하세요.
( 소스 설명 )
1. AVR adc 전압 스케일이 3.3 볼트 일때 vref(계산을 위해 mV 로변경)기준으로 계산합니다.
- 1024는 adc의 1024단계의 분해능을 의미
- 3300mV adc의 vref인 3.3V를 의미함
2. 1024 분해능에서 1 변동할때의 전압값(Vref = 3.3V) : 3300mV / 1024 = 3.22265625mV
* 참고적으로 1024 분해능에서 1 변동할때의 전압값(Vref = 5V)
( 0~5V 사이를 AD 하고, 실제로는 0~3.3V를 사용하므로, 분해능의 단계수를 다시 계산해야합니다. 즉, 3.3/(5/1024) = 675.84 단계 약 676 값이 3.3 볼트입니다. 0~3.3V의 값을 보려면, 0~676의 값으로 계산하면 됩니다.
1024 분해능에서 1 변동할때의 전압값(Vref = 5V) : 3300mV / 676 = 4.8816568mV
)
3. 감도(Sensitivity) 값 관련
AM-GYRO V01 ( 즉, IDG300)의 감도(Sensitivity) 2mV/degree/sec
AM-GYRO V02 (즉, IDG650)의 감도(Sensitivity) 0.5mV/degree/sec
4. 최종적인 각도 계산을 위한 Scale Vector 값 관련
AM-GYRO V01 ( 즉, IDG300) 이용시,
- 1024 분해능에서 1 변동할때의 각속도 값(Vref = 3.3V) :
3.22265625 mV / ( 2mV/degree/sec) = 1.611328125 degree/sec
- 1024 분해능에서 1 변동할때의 각속도 값(Vref = 5V) :
4.8816568mV / ( 2mV/degree/sec) = 2.4408284 degree/sec
AM-GYRO V02 ( 즉, IDG650) 이용시,
- 1024 분해능에서 1 변동할때의 각속도 값(Vref = 3.3V) :
3.22265625 mV / ( 0.5mV/degree/sec) = 6.4453125 degree/sec
- 1024 분해능에서 1 변동할때의 각속도 값(Vref = 5V) :
4.8816568mV / ( 0.5mV/degree/sec) = 9.7533136 degree/sec
5. 즉, 정리하여 보면, 각속도 구하는 기본식은
( 각속도 초기 adc값 - 센서 회전된 adc 값 ) * scale vector
여기서 adc 샘플링 시간을 누적시키면 각도가 됩니다.(적분)
adc 샘플링 주기가 예로 0.01초이면
degree += (( 각속도 초기 adc값 - 센서 회전된 adc 값 ) * scale vector ) * 0.01sec
수고하세요~
- 아래 -
#include <iom128v.h>
#include <macros.h>
#include <stdio.h>
#define SCALE_FACTOR 1.611328125
#define TIME 0.01
#define OFF_SET 458
//590.5611
int value = 0;
float value_sum = 0;
float offset;
float angular_acceleration[50];
float angular_average = 0;
float before_angle = 0;
float after_angle = 0;
float angle_factor = 0;
char adc_flag = 0;
char time_flag = 0;
void port_init(void)
{
PORTA = 0x00;
DDRA = 0x00;
PORTB = 0x00;
DDRB = 0x00;
PORTC = 0x00; //m103 output only
DDRC = 0x00;
PORTD = 0x00;
DDRD = 0x00;
PORTE = 0x00;
DDRE = 0x00;
PORTF = 0x00;
DDRF = 0x00;
PORTG = 0x00;
DDRG = 0x03;
}
//TIMER0 initialize - prescale:64
// WGM: Normal
// desired value: 1KHz
// actual value: 1.000KHz (0.0%)
void timer0_init(void)
{
TCCR0 = 0x00; //stop
ASSR = 0x00; //set async mode
TCNT0 = 0x06; //set count
OCR0 = 0xFA;
TCCR0 = 0x04; //start timer
}
#pragma interrupt_handler timer0_ovf_isr:iv_TIM0_OVF
void timer0_ovf_isr(void)
{
TCNT0 = 0x06; //reload counter value
PORTG ^= 1;
ADCSRA |= 0x40;
}
//TIMER1 initialize - prescale:8
// WGM: 0) Normal, TOP=0xFFFF
// desired value: 100Hz
// actual value: 100.000Hz (0.0%)
void timer1_init(void)
{
TCCR1B = 0x00; //stop
TCNT1H = 0xB1; //setup
TCNT1L = 0xE0;
OCR1AH = 0x4E;
OCR1AL = 0x20;
OCR1BH = 0x4E;
OCR1BL = 0x20;
OCR1CH = 0x4E;
OCR1CL = 0x20;
ICR1H = 0x4E;
ICR1L = 0x20;
TCCR1A = 0x00;
//TCCR1B = 0x02; //start Timer
}
#pragma interrupt_handler timer1_ovf_isr:iv_TIM1_OVF
void timer1_ovf_isr(void)
{
//TIMER1 has overflowed
TCNT1H = 0xB1; //reload counter high value
TCNT1L = 0xE0; //reload counter low value
//PORTG ^= 2;
time_flag = 1;
}
//UART0 initialize
// desired baud rate: 115200
// actual: baud rate:111111 (3.7%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
UCSR0B = 0x00; //disable while setting baud rate
UCSR0A = 0x00;
UCSR0C = 0x06;
UBRR0L = 0x08; //set baud rate lo
UBRR0H = 0x00; //set baud rate hi
UCSR0B = 0x18;
}
// printf ?
int putchar(char c)
{
while (((UCSR0A>>UDRE0)&0x01) == 0) ; // UDRE, data register empty
UDR0 = c;
return c;
}
// scanf
int getchar(void)
{
while ((UCSR0A & 0x80) == 0);
return UDR0;
}
//ADC initialize
// Conversion time: 104uS
void adc_init(void)
{
ADCSRA = 0x00; //disable adc
ADMUX = 0x00; //select adc input 0
ACSR = 0x80;
ADCSRA = 0x8F;
}
#pragma interrupt_handler adc_isr:iv_ADC
void adc_isr( void )
{
//conversion complete, read value (int) using...
value = ADCL; //Read 8 low bits first (important)
value |= ( int )ADCH << 8; //read 2 high bits and shift into top byte
adc_flag++;
value_sum += value;
if(adc_flag >=1 )
TCCR0 = 0x00; //stop
//printf( "\\r\\n ADC = %d", value );
}
//call this routine to initialize all peripherals
void init_devices(void)
{
//stop errant interrupts until set up
CLI(); //disable all interrupts
XDIV = 0x00; //xtal divider
XMCRA = 0x00; //external memory
port_init();
uart0_init();
adc_init();
timer0_init();
timer1_init();
MCUCR = 0x00;
EICRA = 0x00; //extended ext ints
EICRB = 0x00; //extended ext ints
EIMSK = 0x00;
TIMSK = 0x01; //timer interrupt sources
TIMSK = 0x04; //timer interrupt sources
TIMSK = 0x05; //timer interrupt sources
ETIMSK = 0x00; //extended timer interrupt sources
SEI(); //re-enable interrupts
//all peripherals are now initialized
}
void delay ( unsigned int cnt )
{
unsigned int i,j;
for( i=0 ; i< cnt ; i++ )
for( j=0 ; j< 1000 ; j++ );
}
void main (void)
{
int i;
int temp;
init_devices();
printf( "\\r\\n GYRO..QQ");
printf( "\\r\\n ADCSRA = 0x%x", ADCSRA );
printf( "\\r\\n ADMUX = 0x%x", ADMUX );
printf( "\\r\\n ACSR = 0x%x", ACSR );
delay( 1000 );
for( i=0 ; i<100 ; i ++ )
angular_acceleration[i] = 0;
ADMUX = 0x00;
ADCSRA |= 0x40;
offset = 0;
for( i=0 ; i<50 ; )
{
if ( adc_flag )
{
temp = value;
offset += value;
adc_flag = 0;
ADCSRA |= 0x40;
//printf( "\\r\\n $$ = %d", value );
i ++ ;
}
}
offset = (int)(offset / 50);
printf( "\\r\\n offset = %f", offset );
//for( i=0 ; i<50 ; i ++ )
// angular_acceleration[i] = offset;
delay( 2000 );
delay( 2000 );
TCCR1B = 0x02; //start Timer
TCCR0 = 0x05; //start timer
value_sum = 0;
adc_flag = 0;
while(1)
{
//delay( 1000 );
if( time_flag )
{
PORTG |= 0x02;
value_sum = value_sum / adc_flag;
angular_acceleration[0] = (( float )( (int)(value_sum) - offset )) * SCALE_FACTOR;
after_angle = before_angle + ( angular_acceleration[9] + angular_acceleration[8] + angular_acceleration[7] + angular_acceleration[6] + angular_acceleration[5] + angular_acceleration[4] + angular_acceleration[3] + angular_acceleration[2] + angular_acceleration[1] + angular_acceleration[0] ) / 10;
angle_factor = after_angle * TIME;
before_angle = after_angle;
for( i=0 ; i<9 ; i ++ )
{
angular_acceleration[i+1] = angular_acceleration[i];
//printf ( " nangle = %.2f", angular_acceleration[i]);
}
printf ( "\\r\\n X_angle = %.2f", angle_factor);
angular_average = 0;
adc_flag = 0;
value_sum = 0;
time_flag = 0;
TCCR0 = 0x05;
}
PORTG &= 0xfd;
}
}
|
|
