Hello, customer?
(Purpose of source)
Followings are sources that find angle using gyro sensor calculating with AD between 0~3.3V based on reference voltage 3.3V. In case of reference voltage 5V, modify with reference of below description of source.
(Description of source)
1. If voltage scale of AVR adc is 3.3V, calculate based on vref(change to mV for calculation).
- 1024 means resolution of 1024 steps in adc
- means 3.3V which is vref in 3300mV adc.
2. Value of voltage when 1 fluctuates in 1024 resolution (Vref = 3.3V):
3300mV / 1024 = 3.22265625mV
* For reference, value of voltage when 1 fluctuates in 1024 resolution (Vref = 5V) (you have to calculate the number of steps in resolution because AD is applied between 0~5V and 0~3.3V is used actually. That is to say, 3.3/(5/1024) = 675.84 steps, approx. value of 676 is 3.3V. If you want to find the value of 0~3.3V, calculate as the value of 0~676. Value of voltage when 1 fluctuates in 1024 resolution (Vref = 5V) : 3300mV / 676 = 4.8816568mV)
3. Related with the value of Sensitivity
Sensitivity of AM-GYRO V01 (i.e, IDG300) 2mV/degree/sec
Sensitivity of AM-GYRO V02 (i.e, IDG650) 0.5mV/degree/sec
4. Related with value of Scale vector for final angle calculation
When you use AM-GYRO V01 ( i.e, IDG300),
- Value of angular speed when 1 fluctuates in 1024 resolution (Vref = 3.3V) :
3.22265625 mV / ( 2mV/degree/sec) = 1.611328125 degree/sec
-Value of angular speed when 1 fluctuates in 1024 resolution (Vref = 5V) :
4.8816568mV / ( 2mV/degree/sec) = 2.4408284 degree/sec
When you use AM-GYRO V02 (i.e, IDG650) :
- Value of angular speed when 1 fluctuates in 1024 resolution(Vref =3.3V):
3.22265625 mV / ( 0.5mV/degree/sec) = 6.4453125 degree/sec.
- Value of angular speed when 1 fluctuates in 1024 resolution (Vref = 5V) :
4.8816568mV / ( 0.5mV/degree/sec) = 9.7533136 degree/sec
5. So, to put above into order, basic formula finding angular speed:
(Early adc value of angular speed ? value of adc which sensor is rotated) * scale vector, if adc sampling time is accumulated here, it will be angle. (integral). If adc sampling cycle, for instance, is 0.01 sec, degree += (( early adc value of angular speed ? sensor-rotated adc value)
* scale vector ) * 0.01sec
- Below ?
#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;
}
}
