|
Building self balancing scooter- troubles with Integrating Gyro
Hi there, i am building a self balancing scooter and i am trying to integrate the gyro (ADIS16100) in order to use it as a tilt sensor but the value of the integration keeps drifting away weather the Gyro is moving or not. Currently every 10 seconds the value increases by a small amount. Does anyone know of a better way to integrate or a way to determine the offset than just assuming its constant which clearly its not?
#include<16f877A.h>
#DEVICE ADC=8
#fuses hs,nowdt,noprotect,noput,nobrownout
#use delay(clock=20000000)
#include<lcd420.c>//set up for LCD
#use rs232(baud=9600,xmit=PIN_C6,rcv=PIN_C7,bits=8,pari ty=N)
//#use spi(MASTER,DI=pin_A0,DO=pin_A1,CLK=pin_A2,BAUD=100 0000,IDLE=1,SAMPLE_RISE,ENABLE_ACTIVE=1,MODE=1,BIT S=16,MSB_FIRST,STREAM=SPI_STREAM,ENABLE=PIN_D2)
#define GYRO_OFFSET 123 //offset for 8 bits is 123 and for 10 bit it is 507
#int_timer0
void main ()
{
float dt,angle;
double gz_adc,last_angle; /* Analog values */
int gz_deg,gyro_deg; /* Unit values */
double newgyrovalue = 0;
double oldgyrovalue = 0;
double gyrointegral = 0;
lcd_init ();
delay_us(10);
//Setup the Timer
setup_timer_0(RTCC_INTERNAL | RTCC_8_BIT| RTCC_DIV_256); // sets up timer
set_timer0(0); // intitializes timer to 0
//Setup the ADC
setup_adc(ADC_CLOCK_INTERNAL );//setup clock before setup
setup_adc_ports( ALL_ANALOG );
set_adc_channel( 0 );//check to see why not A0
while(1)
{
newgyrovalue = read_adc();
set_timer0(0);//start the timer
gyro_deg=((newgyrovalue-gyro_offset)*41);
dt=get_Timer0();//get the timer value
gyrointegral = (oldgyrovalue + (gyro_deg*0.000051*dt));
oldgyrovalue = gyrointegral;
angle=gyrointegral;
printf (lcd_putc,"\f Angle is :%f", angle); // d for int and f for
float and double
delay_ms(40);
//set_timer0(0);//reset the timer
}
}
|