Im currently trying out kalman's filter but got stuck at the very beginning. I cant even display any angle output by my 5DOF. When
accDegreeY[ 0 ] = (adcValue[ 4 ]*5.0)/1023;
accDegreeY[ 1 ] = (accDegreeY[ 0 ]-1.6)/0.3;
accDegreeZ[ 0 ] = (adcValue[ 5 ]*5.0)/1023;
accDegreeZ[ 1 ] = (accDegreeZ[ 0 ]-1.6)/0.3;
is added, i cant compile it. This means that there is a problem with my declaration but i cant find it. My lcd display can only display positive numbers. As you can see in my code, i randomly put in my gyro and accelerometer to know if it is working. I still have one problem because i dont know how to include math calculations in programs. Examples : sin, cos, tan, arcsin,atan2, square root. Im still new in programming PIC. Help needed. Thank you.
Using this math equation to solve my kalman filter.
A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications. « Starlino Electronics
This is my code.
#include <stdio.h>
#include <htc.h>
#include <math.h>
#include "lcd.h"
__CONFIG(0x3f71);
void main ( void )
{
char LCDString [ 17 ];
unsigned int adcValue[7];
float accDegreeX[ 3 ],accDegreeY[ 3 ],accDegreeZ[ 3 ],gyroRateX[ 3 ],gyroRateY[ 3 ];
unsigned char tmp;
// Initializations
lcd_init();
lcd_goto ( 0x00 );
lcd_puts ( "ADC:" );
lcd_goto ( 0x40 );
lcd_puts ( "Degree:" );
PORTB = 0x00;
TRISA = 0xff;
TRISB = 0x00;
ADCON1 = 0xc8;
while ( 1 )
{
// Body
ADCON0 = 0x81;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 0 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0x89;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 1 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xa1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 2 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xa9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 3 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xb1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 4 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xb9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 5 ] = ( ADRESH << 8 ) | ADRESL;
gyroRateX[ 0 ] = (adcValue[ 0 ]*5.0)/1023;
gyroRateX[ 1 ] = (gyroRateX[ 0 ]-1.5)/0.002;
gyroRateY[ 0 ] = (adcValue[ 1 ]*5.0)/1023;
gyroRateY[ 1 ] = (gyroRateY[ 0 ]-1.5)/0.002;
accDegreeX[ 0 ] = (adcValue[ 3 ]*5.0)/1023;
accDegreeX[ 1 ] = (accDegreeX[ 0 ]-1.6)/0.3;
lcd_goto ( 0x05 );
sprintf ( LCDString, "%d ", adcValue[ 0 ] );
lcd_puts ( LCDString );
lcd_goto ( 0x48 );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_putch ( '.' );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_puts ( " G" );
}
}
accDegreeY[ 0 ] = (adcValue[ 4 ]*5.0)/1023;
accDegreeY[ 1 ] = (accDegreeY[ 0 ]-1.6)/0.3;
accDegreeZ[ 0 ] = (adcValue[ 5 ]*5.0)/1023;
accDegreeZ[ 1 ] = (accDegreeZ[ 0 ]-1.6)/0.3;
is added, i cant compile it. This means that there is a problem with my declaration but i cant find it. My lcd display can only display positive numbers. As you can see in my code, i randomly put in my gyro and accelerometer to know if it is working. I still have one problem because i dont know how to include math calculations in programs. Examples : sin, cos, tan, arcsin,atan2, square root. Im still new in programming PIC. Help needed. Thank you.
Using this math equation to solve my kalman filter.
A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications. « Starlino Electronics
This is my code.
#include <stdio.h>
#include <htc.h>
#include <math.h>
#include "lcd.h"
__CONFIG(0x3f71);
void main ( void )
{
char LCDString [ 17 ];
unsigned int adcValue[7];
float accDegreeX[ 3 ],accDegreeY[ 3 ],accDegreeZ[ 3 ],gyroRateX[ 3 ],gyroRateY[ 3 ];
unsigned char tmp;
// Initializations
lcd_init();
lcd_goto ( 0x00 );
lcd_puts ( "ADC:" );
lcd_goto ( 0x40 );
lcd_puts ( "Degree:" );
PORTB = 0x00;
TRISA = 0xff;
TRISB = 0x00;
ADCON1 = 0xc8;
while ( 1 )
{
// Body
ADCON0 = 0x81;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 0 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0x89;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 1 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xa1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 2 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xa9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 3 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xb1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 4 ] = ( ADRESH << 8 ) | ADRESL;
ADCON0 = 0xb9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 5 ] = ( ADRESH << 8 ) | ADRESL;
gyroRateX[ 0 ] = (adcValue[ 0 ]*5.0)/1023;
gyroRateX[ 1 ] = (gyroRateX[ 0 ]-1.5)/0.002;
gyroRateY[ 0 ] = (adcValue[ 1 ]*5.0)/1023;
gyroRateY[ 1 ] = (gyroRateY[ 0 ]-1.5)/0.002;
accDegreeX[ 0 ] = (adcValue[ 3 ]*5.0)/1023;
accDegreeX[ 1 ] = (accDegreeX[ 0 ]-1.6)/0.3;
lcd_goto ( 0x05 );
sprintf ( LCDString, "%d ", adcValue[ 0 ] );
lcd_puts ( LCDString );
lcd_goto ( 0x48 );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_putch ( '.' );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
tmp = ( unsigned char ) gyroRateX[ 1 ];
gyroRateX[ 1 ] = ( gyroRateX[ 1 ] - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_puts ( " G" );
}
}