![](templates/subSilver/images/CCSLogo.jpg) |
![CCS C Software and Maintenance Offers](templates/subSilver/images/forumAd6.jpg) |
View previous topic :: View next topic |
Author |
Message |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
16f877a and Gyro Idg5oo |
Posted: Sat Apr 28, 2012 5:19 pm |
|
|
hello to all!
I am getting sick day after day, because I want to obtain the angle from the angular velocity of the gyro. The gyro gives me the velocity (analog sensor) and by integrating (by multiplying the velocity to the time that takes to make that change in velocity) I obtain the angle in degrees. My code that doesn't work so good is this one. I'm calculating the time by the delay which increase a variable 0.1 each time, so please if anyone have any other good idea tell me!
Code: |
#include<16f877a.h>
#device adc=8
#Fuses HS,NOPROTECT,NOWDT,NOLVP
#use delay(clock=20000000)
#include<lcd420_b.c>
#byte port_a = 5
#byte port_b = 6
#byte port_c = 7
#byte port_d = 8
#byte port_e = 9
void init()
{
set_tris_c(0x00);
set_tris_a(0xff);
SETUP_ADC(ADC_CLOCK_INTERNAL);
SETUP_ADC_PORTS(AN0_AN1_AN2_AN3_AN4_AN5);
lcd_init();
}
void main()
{
long int a[45];
int j=2;
float g;
int k=1;
float b=0;
float d=0;
float t;
a[1]=68;
init();
while(1)
{
k=k+1;
SET_ADC_CHANNEL(0);
delay_us(10);
a[k]=read_adc();
if ((a[k]>a[k-1]))
{
t=0.1;
while(1){
delay_ms(100);
t=t+0.1;
SET_ADC_CHANNEL(0);
delay_us(10);
k=k+1;
a[k]=read_adc();
if ((a[k]-a[k-1])==0)
{
k=k-1;
}
g=((((a[k]*3.02)/154)-1.33))/(0.002);
if(a[k]<a[k-1])
{break;}
}
d=g*t;
b=d+b;
if (b>360)
{b=b-360;}
}
else if ((a[k]<a[k-1]))
{
t=0.1;
while(1){
delay_ms(100);
t=t+0.1;
SET_ADC_CHANNEL(0);
delay_us(10);
k=k+1;
a[k]=read_adc();
if ((a[k]-a[k-1])==0)
{
k=k-1;
}
g=((((a[k]*3.01)/154)-1.33))/(0.002);
if (a[k]>a[k-1])
{break;}
}
d=g*t;
b=b-d;
if (b<-360)
{b=360+b;}
}
printf(LCD_PUTC,"\f time%f\n",t);
printf(LCD_PUTC,"angle %f\n",b);
printf(LCD_PUTC,"bitangl %ld\n",a[k]);
printf(LCD_PUTC,"end ",);
k=1;
}
}
|
|
|
![](templates/subSilver/images/spacer.gif) |
temtronic
Joined: 01 Jul 2010 Posts: 9174 Location: Greensville,Ontario
|
|
Posted: Sun Apr 29, 2012 7:25 am |
|
|
You might want to email the manufacturer of the Gyro for help. Google only came up with 5 hits for Idg5oo( 1 was yours, the rest garbage).
We have no way of knowing if the data from the gyro is correct. Perhaps you can test with Matlab or some other software to confirm/deny the results you get from known input data ? |
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Sun Apr 29, 2012 9:24 am |
|
|
Also, I think possibly there are problems about the times involved. You have some maths that is going to take a long time. I'd be thinking of actually sampling at a regular interval, rather than using delays (triggering the sample from a timer).
There are then several minor comments:
1) ADC_CLOCK_INTERNAL, is _not_ a legitimate clock rate for a CPU running at 20MHz. This will be reducing the ADC accuracy.
2) Do you really mean to be using ADC=8?. I'd be wanting to get the best possible accuracy on every reading.....
3) Sign. I'd expect the value from a gyro type sensor, to be at a fixed voltage when the unit is stationary, and to go up and down relative to this point, as you move in different directions. Hence you need to be reading this stationary point when you first start, and storing this as the 'base', then reading the adc voltages, and subtracting this base reading (using _signed_ arithmetic), and integrating this.
4) Speed. Seriously see if you can do the calculations in integer, not float. There is almost certainly not time to do float arithmetic. Do the integration, in integer, and only (if necessary) the final maths in float, for output.
I have done a similar thing some years ago, using a 'real' gyro system (Sperry), and used a 16bit ADC on a PIC that was much slower than those we have now, but was still able to get quite reasonable direction change values.
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Sun Apr 29, 2012 6:15 pm |
|
|
Yea you are right about the internal clock and about adc=8. I write this code on morning and I obtain good result via the stupid software Isis Proteus. But in reality it doesn't work (I'm gonna do some modification tomorrow maybe). But I don't know why you talked about calculating the speed in integer, because float are more precise, isn't it !?
Code: |
#include<16f877a.h>
#device adc=10
#Fuses HS,NOPROTECT,NOWDT,NOLVP
#use delay(clock=20000000)
#include<lcd420_b.c>
// #include <math.h>
#byte port_a = 5
#byte port_b = 6
#byte port_c = 7
#byte port_d = 8
#byte port_e = 9
void init()
{
set_tris_c(0x00);
set_tris_a(0xff);
SETUP_ADC(ADC_CLOCK_DIV_32);
SETUP_ADC_PORTS(AN0_AN1_AN2_AN3_AN4_AN5);
lcd_init();
}
void main()
{
long int b1,b2;
int j=2;
float g=0;
float z=0;
//int32 k[2];
float k=0;
float b=0;
long int d=0;
float t=0;
d=272;
//k[1]=272;
init();
while(1)
{
set_adc_channel(0);
delay_us(10);
b2=read_adc();
// d=b2;
// g=((((b2*3.03)/618)-1.33))/(0.002);
if (b2>272)//positive change
{while(1){
delay_ms(100);
t=t+0.1;
k=t;
// k[1]=k[2];
set_adc_channel(0);
delay_us(10);
b2=read_adc();
//k[2]=read_adc();
// k[2]=b2;
if (b2>d)//changing
{d=b2;
}
if (b2<=272)
{t=0;
break;}
printf(LCD_PUTC,"\fkari %f\n",t);
printf(LCD_PUTC," big %ld\n",d);
}
g=((((d*3.02)/618)-1.33))/(0.002);
z=z+(g*k);
if(z>360)
{z=z-360;}}
if (b2<272)
{while(1){
delay_ms(100);
t=t+0.1;
k=t;
// k[1]=k[2];
set_adc_channel(0);
delay_us(10);
b2=read_adc();
//k[2]=read_adc();
// k[2]=b2;
if (b2<d)
{d=b2;
}
if (b2>=272)
{t=0;
break;}
printf(LCD_PUTC,"\fkari %f\n",t);
printf(LCD_PUTC," big %ld\n",d);
}
g=((((d*3.02)/618)-1.33))/(0.002);
z=z+(g*k);
if(z<-360)
{z=z+360;}}
printf(LCD_PUTC,"\fTime %f\n",k);
printf(LCD_PUTC,"Velocity %f\n",g);
printf(LCD_PUTC,"Angle %f\n",z);
printf(LCD_PUTC,"end\n");
}
}
|
|
|
![](templates/subSilver/images/spacer.gif) |
temtronic
Joined: 01 Jul 2010 Posts: 9174 Location: Greensville,Ontario
|
|
Posted: Sun Apr 29, 2012 7:52 pm |
|
|
While in a 'perfect' world floating point numbers are better (for some applications), you're under a time constraint so integer math is better.
It's better because it's a LOT faster to calculate the result.
It also appears you're only doing 8 bit ADC which will speed up calculations as well as opposed to 10 bit data.
As you've also found out Proteus is garbage and should be tossed out of your computer. Consider the amount of time you wasted on it, instead of 'real world' programming. |
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Mon Apr 30, 2012 2:12 am |
|
|
Don't confuse 'precision', with 'range'. A FP number accepts a wide 'range' of values, but is _less_ 'precise' than an integer.
A 4byte FP number gives approximately 6.5 digits of precision. a 4byte integer, gives approx 9.5 digits of precision!. This is why integers are always used for things like financial calculations. If you 'know' you are only dealing with numbers in a particular part of the 'number line', integers are a lot better. However it makes work for you to solve in advance what the range you need to deal with is, and scale the integers so that they cover this. With FP this is handled for you, but at a cost in time. Adding two int32 values for example takes about 1/40th the time it takes to add two FP values....
Now remember that if your numbers are treated as 'signed', and are sampled at a fixed clock based interval , all you need to do to is generate the sum of these. Other scaling to generate a result in degrees etc., can all be done only when you output the value.
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Mon Apr 30, 2012 4:25 am |
|
|
it seems that i'm going to take in consideration new way for programming the integration of a gyro, cuz in this manner is impossible to take good result the voltage changes in the gyro are so fast!!!!
but i can't put the "time" like an integer cuz for example the 1.54 sec it doesn't recognize it!!!
but as i said am gonna look for new way, and if i found a good one (hope so) i ll post the code.
Thx |
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Mon Apr 30, 2012 5:03 am |
|
|
Seriously, I'd be looking at a sampling time interval like perhaps 50uSec, or probably switch to a PIC with a faster ADC, and go even quicker.
You don't need 'time' in the equation at all. Imagine (for instance), that the unit gave one count of the ADC for a degree of movement per 50uSec (the numbers don't matter at all). then you see:
-1,+3, -2, +5, -6, +2, +3, -1, -1, +2.
Simply add these, and the unit has changed angle 4 degrees from it's starting point. So long as your sampling is fast, so you don't miss significant movements, regular, and accurate, the result is always the same, that the sum, represents the total movement. If the resolution was such that the reading was in 1/12th degree steps, the same still applies, except that the angular change would then want to be divided by 12 for output. Unless you want to know the rate of change of position, the total time doesn't have to come into the equation at all. Even if it does, you only need to use float right at the end, just saying:
answer = (float)sum/1.54;
Which converts the integer sum to a float, and then performs the single division by 1.54.
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Mon Apr 30, 2012 6:41 pm |
|
|
Thanks to Ttelmah I worked with a new very simple code (and i feel stupid about the old one). But still bad as result...
Code: |
#include<16f877a.h>
#device adc=8
#Fuses HS,NOPROTECT,NOWDT,NOLVP
#use delay(clock=20000000)
#include<lcd420_b.c>
#byte port_a = 5
#byte port_b = 6
#byte port_c = 7
#byte port_d = 8
#byte port_e = 9
void init()
{
set_tris_c(0x00);
set_tris_a(0xff);
SETUP_ADC(ADC_CLOCK_DIV_32);
SETUP_ADC_PORTS(AN0_AN1_AN2_AN3_AN4_AN5);
lcd_init();
}
void main()
{
float digital;
float value;
float start;//the default voltage of the gyro
float angle=0;
init();
set_adc_channel(0);
delay_ms(100);
digital=read_adc();
set_adc_channel(0);
delay_ms(1);
start=read_adc();//get the default voltage
while(1){
set_adc_channel(0);
delay_ms(100);
digital=read_adc();
value=((((digital*5))/255)-((start*5)/255))/0.002; // 0.002 is the sensitivity of gyro
angle=(value*0.1)+angle;//adding measured angle each 0.1sec
printf(LCD_PUTC,"\fAngle %f\n",angle);
printf(LCD_PUTC,"Velocity %f\n",value);
printf(LCD_PUTC,"Velocity %f\n",start);
}
}
|
|
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Tue May 01, 2012 1:41 am |
|
|
You are still reading far too slowly, made worse by the maths...
You should be looking at reading the ADC in a timer triggered interrupt at fast intervals (I suggested 50uSec), and just maintaining the 'sum' of the reading.
Then in your 'main' code, perhaps once a second or whenever you want, take this sum, perform the maths, and print the output.
Also, I'd still be looking at using 10bit ADC readings. Otherwise your accuracy is going to be foul.
Also, I'd be averaging perhaps 20+ readings to get the start value. Otherwise any noise will lead to long term errors.
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Tue May 01, 2012 5:04 am |
|
|
I put an interval like 50usec but the changes are sooo slowly, and I made one using Timer0 interrupt but the same result, and adc=8 gave me better result, more stable and more fast I think. |
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Tue May 01, 2012 7:59 am |
|
|
I doubt if you did do a reading at 50uSec, if your code is based on the current design, or using timer0 - since this can't trigger the ADC, you would have been doing it manually and taking far too long....
Untested, but the basics:
Code: |
#include<16f877a.h>
#device adc=10
#Fuses HS,NOPROTECT,NOWDT,NOLVP
#use delay(clock=20000000)
#include<lcd420_b.c>
signed int32 adc_sum;
signed int16 origin;
#INT_ADC
void adc_reading(void) {
adc_sum+=(signed int16)read_adc(ADC_READ_ONLY) - origin;
}
//Because the ADC _has already performed the conversion when we
//get here - only read, don't trigger another conversion - much faster...
void init(void) {
set_tris_c(0x00);
set_tris_a(0xff);
SETUP_ADC(ADC_CLOCK_DIV_32);
SETUP_ADC_PORTS(AN0); //Only setup channels you _are_ going
//to read - extra channels potentially increase noise.
SET_ADC_CHANNEL(0);
lcd_init();
SETUP_CCP2(CCP_COMPARE_RESET_TIMER);
SETUP_TIMER1(T1_INTERNAL | T1_DIV_BY_1);
CCP_2=249;
//Setup timer1 to be clocking at 5MHz. CCP2, to trigger ADC,
//Trigger to be on every 250th count
}
void main(void) {
float angle=0;
int16 i_sum;
int8 icount;
int32 local_sum;
init();
for (icount=0;icount<16;icount++) {
delay_us(20);
i_sum+=read_adc();
}
origin=i_sum/16;
set_timer1(0);
clear_interrupt(INT_ADC);
enable_interrupts(INT_ADC);
enable_interrupts(GLOBAL);
while(TRUE){
delay_ms(100); //actual interval will be perhaps 200mSec
//Because of the time spent in the ADC interrupt
//Now mathematically, _multiplication_ is much faster than division
//Hence /0.002, is much better expressed as *500 - are you sure of
//your numbers here - would seem to give much to large a result....
//You still have not given us a reference for this part. I'd perhaps
//suspect the 0.002 figure you have is something like 2mV/degree/sec?
//If so, sampling at 20000times/second, with each step of the ADC
//being about 5mV, 48828 counts would be one degree of rotation.
//Need real data for this, but guess.....
disable_interrupts(GLOBAL);
local_sum=adc_sum;
enable_interrupts(GLOBAL);
//Copy the interrupt driven 'sum' to a local value
angle=local_sum*0.00002048;
printf(LCD_PUTC,"\fAngle %f\n",angle);
printf(LCD_PUTC,"Counts %ld\n",local_sum);
printf(LCD_PUTC,"Start %ld\n",origin);
}
}
|
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Tue May 01, 2012 1:57 pm |
|
|
Thank you Ttelmah, but the code doesn't compile. It gives me error on the #INT_ADC. It tells me "invalid pre-processor directive".
and to be honest I don't understand the functionality of the Timer1 and ccp_2=249!!! |
|
![](templates/subSilver/images/spacer.gif) |
Ttelmah
Joined: 11 Mar 2010 Posts: 19367
|
|
Posted: Tue May 01, 2012 2:18 pm |
|
|
INT_AD, on the 877. It is INT_ADC, on the PIC24 chips. As I said I didn't check it....
Read the data sheet about the CCP, and 'special event trigger'.
Basic 'point' is this. The ADC takes 12 cycles of the ADC clock to perform a conversion. The ADC clock must be slower than 625KHz. So the ADC _at best_ takes 19.2uSec to actually perform the conversion. It takes another just under 20uSec to 're-acquire' before the next one can be performed, so the absolute fastest the ADC can run (on this chip - many of the later chips take less time to acquire, and allow faster ADC clocks), is 25706 conversions per second.
Now, throughout your code, you are _waiting_ for the ADC to do this, then taking the reading, then looping. Even if everything else is done as fast as possible, you would be lucky to manage to take a reading every 100uSec. Worse, you then do the maths, and display things to the LCD (at best an LCD takes about 40uSec/byte to display anything, and clearing the screen a few _mSec_). So even if you had no delays in your loop, you'd probably be straining to get to even a few hundred samples/second.
Now the signal from the gyro, will be a high frequency output reflecting instantaneous movements, and as such needs to be sampled _fast_.
So the point about timer1, and the CCP. The CCP, as well as being able to be used as a counter, can also be used to trigger 'events'. CCP2 (not CCP1), can trigger the ADC, when it fires.
Timer1, is counting cycles of the master clock/4. It counts up to 249, and when it gets here the CCP says 'I have a match'. This starts the ADC converting, and resets the counter to 0. The ADC when it completes it's conversion triggers INT_AD. So when INT_AD triggers, _a conversion has automatically been done by the ADC_. This happens every 250 counts of the master clock/4. So 20000 times per second.
Then in the INT_AD, this is immediately read from the registers (no need to trigger a conversion since one has already been done), and added into the running 'sum'.
The main code then reads the sum, and performs a 'guess' at the arithmetic involved (depends on what the voltage from the sensor actually means - as I said you still have not provided a link to data for this). The sum continually carries on growing or shrinking whenever the unit rotates.
Best Wishes |
|
![](templates/subSilver/images/spacer.gif) |
karimpain
Joined: 15 Feb 2012 Posts: 39 Location: italia
|
|
Posted: Tue May 01, 2012 4:26 pm |
|
|
In two words what I've done is a code without precision in time which needs a gyro that sends datain high freq. Ok I don't know how the ccp trigger event works (I'll read about it now), but the compiler gives me error that the adc_sum and setup_timer1(): "undefined identifier" ?!?!?
and what it refers the "0.00002048" in the final formula (the time??)
And this gyro gives 1.33 volt as a zero output voltage (stability case) and when it rotates the sensitivity is about 0.002volt/deg/s.
but what the differnet between the timer0 and timer1..
"An interrupt may be requested when Timer1 rolls over from 0xFFFF to 0x0000.
- Timer1 Timer1 can be reset when the CCP module is configured to compare mode to can be reset when the CCP module is configured to compare mode to
generate a special event trigger."
but the timer0 doeas the same thing ,doesn't it!??!
this is my timer0 code:
#include<16f877a.h>
#device adc=10
#Fuses HS,NOPROTECT,NOWDT,NOLVP
#use delay(clock=20000000)
#include<lcd420_b.c>
#byte port_a = 5
#byte port_b = 6
#byte port_c = 7
#byte port_d = 8
#byte port_e = 9
float digital;
float start;
float value;
float angle=0;
int i;
void init()
{
set_tris_c(0x00);
set_tris_a(0xff);
SETUP_ADC(ADC_CLOCK_DIV_32);
SETUP_ADC_PORTS(AN0);
lcd_init();
}
#int_timer0
void timer0_isr()
{set_timer0(0);
init();
set_adc_channel(0);
digital=read_adc();//takes 1.6us=32/20M
value=(((digital*5)/1023)-((start*5)/1023))*109;
angle=(value*0.000020)+angle; //obtain the new angle each 20us
printf(LCD_PUTC,"\fAngle %f\n",angle);
// printf(LCD_PUTC,"Velocity %f\n",value);
}
void main()
{
set_tris_d(0x00);
for (i=0;i<10;i++)
{init();
set_adc_channel(0);
start=read_adc()+start;
}
start=start/10;
enable_interrupts(global);
enable_interrupts(int_timer0);
setup_timer_0(rtcc_internal | rtcc_div_256);
set_timer0(0);//do the interrupt program each 0.2us
while(1)
{ }
} |
|
![](templates/subSilver/images/spacer.gif) |
|
|
You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|