|
n ソースの目的
下記のソースは3.3V基準電圧で0～3.3Vの間をADして計算するジャイロセンサーで、角度を求めるソースです。5V基準電圧の時には、下記のソース説明を参考にして修正してご使用ください。
n ソース説明
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;
}
}
|
