Documente Academic
Documente Profesional
Documente Cultură
times dt. The overall change in angle is found by summing this quantity
over many samples.
11
Since there is already a significant time overhead involved in running the MATLAB code to
retrieve a value from the gyro (around 50 ms), were not implementing any further delay at this
point. The timer simply keeps track of this time as accurately as possible. Note: the gyro
should be able to operate at significantly higher frequencies when running directly from the
Arduino, which should improve accuracy.
%%Angl e Measur ement
%To st op measur i ng, r ot at e your r over 360 degr ees or use Ct r l +C i n t he command
wi ndow.
dc_of f set = 0;
gyr o_dr i f t = 0;
ang = 0;
sensi t i vi t y = 250; %adj ust f or your gyr o, shoul d be 250.
t hr eshol d = 0; %Thr eshol d DN f r omst at i onar y t est
t i mer = t i c; %st ar t t i mer
whi l e abs( ang) < 360 %pr ogr amst ops af t er 360 degr ee t ur n
f or n = 1: 20
v = a. gyr oRead( ' z' ) - dc_of f set ; %r ead z axi s, r equi r es sr v_gyr o. pde on boar d
i f abs( v) > t hr eshol d %appl y dr i f t t hr eshol d t o r aw dat a ( DN)
dt = t oc( t i mer ) ; %dt = el apsed t i me si nce t i mer st ar t ed
t i mer = t i c;
ang = ang + ( v/ 32768) *sensi t i vi t y*dt - gyr o_dr i f t ; %i nt egr at e angl e f r om
vel oci t y
end
end
%di spl ay angl e af t er ever y 20 l oops, t r yi ng t o conser ve pr ocessi ng t i me
cl c
f pr i nt f ( ' Angl e: %2. 4f \ n' , ang)
end
7.2.2 Observe Drift
Copy the Angle Measurement code above and set an appropriate threshold level as
determined from the Gyro Noise at Rest experiment. Leave your rover perfectly still and watch
the calculated angle measurement; it should jump up and down a little bit, but stay very near
zero.
7.2.3 Correct for Drift
If, after a few seconds the observed angle starts to steadily increase or decrease, you may want
to consider changing the constant gyro_drift from zero to something like 0.0018. This value is
used in the angle calculation (integrator).
ang = ang + ( v/ 32768) *sensi t i vi t y*dt - gyr o_dr i f t ; %i nt egr at e angl e f r omvel oci t y
For my gyro I did not observe any significant drift. Specifically, I measured a drift of only 0.3
degrees per minute. This should be more than adequate for our application.
12
7.3 Combined Experiment
In this experiment we plot both gyro noise and drift as a function of time. This cuts down time
because it measures both the drift and shows the noise graph with one run through.
%%Angl e Measur ement
%To st op measur i ng, r ot at e your r over 360 degr ees or use Ct r l +C i n t he command
%wi ndow.
dc_of f set = 0;
gyr o_dr i f t = 0;
ang = 0;
sensi t i vi t y = 300; %adj ust f or your gyr o, shoul d be 250.
t hr eshol d = 0; %Thr eshol d DN f r omst at i onar y t est
t i mer = t i c; %st ar t t i mer
z = zer os( 1, 100) ; %set up an ar r ay f or 100 sampl es
x = 1: 100; %x- axi s f or pl ot
f i gur e( 1) %br i ng pl ot wi ndow t o f r ont
whi l e abs( ang) < 360 %pr ogr amst ops af t er 360 degr ee t ur n
f or n = 1: 100
z( n) = a. gyr oRead( ' z' ) - dc_of f set ; %sampl e gyr o z- axi s val ue
%l i ve pl ot of dat a
pl ot ( x, z)
axi s( [ 1 100 - 100 100] )
dr awnow %f or ces MATLAB t o r edr aw f i gur es
v = a. gyr oRead( ' z' ) ; %r ead z axi s, r equi r es sr v_gyr o. pde on boar d
i f abs( v) > t hr eshol d %appl y dr i f t t hr eshol d t o r aw dat a ( DN)
dt = t oc( t i mer ) ; %dt = el apsed t i me si nce t i mer st ar t ed
t i mer = t i c;
ang = ang + ( v/ 32768) *sensi t i vi t y*dt - gyr o_dr i f t ; %i nt egr at e angl e f r omvel oci t y
end
end
%di spl ay angl e af t er ever y 20 l oops, t r yi ng t o conser ve pr ocessi ng t i me
cl c
f pr i nt f ( ' Angl e: %2. 4f \ n' , ang)
f pr i nt f ( ' Max val ue: %d\ n' , max( z) )
f pr i nt f ( ' Mi n val ue: %d\ n' , mi n( z) )
f pr i nt f ( ' Mean val ue: %f \ n' , mean( z) )
end
8 Measure Rotation
Print the Compass Drawing provided in Appendix B. Try running the Angle Measurement
program and watching the value as you slowly rotate your rover over a fixed angle.
I found that my gyro was consistently measuring lower than it should be (i.e. rotating to 90
degrees measured only 77). If you experience similar results, try adjusting the sensitivity value
(in my case approx. 310 worked well) and rerunning the program. - Nik
The instructor believes the difference in observed and calculated angle is probably a MATLAB
artifact. Until we complete the PID lab we can not be sure.
13
9 Add a Smoothing Filter
This lab experiment shows us the potential for filtering our data.
1 Copy the Filtering code below.
2 Run the program leaving the gyro at rest.
3 Observe the noise from the gyro in the top graph. The bottom graph implements a
simple 5-point moving average filter to smooth the data. You may wish to modify the
filter parameters by changing the highlighted sections of code.
%%DN Fi l t er i ng
f i gur e( 1)
sampl es = 300;
f i l t = 5;
val 1 = zer os( 1, sampl es + f i l t ) ;
val 2 = zer os( 1, sampl es + f i l t ) ;
v = zer os( 1, sampl es + f i l t ) ;
x = 1: sampl es+f i l t ; %x- axi s f or pl ot
sensi t i vi t y = 310; %adj ust f or your gyr o, shoul d be 250.
t hr eshol d = 0; %Thr eshol d DN f r omst at i onar y t est
t i mer = t i c; %st ar t t i mer
f or n = f i l t : sampl es+f i l t
v( n) = a. gyr oRead( ' z' ) ; %r ead z axi s, r equi r es sr v_gyr o. pde on boar d
i f abs( v( n) ) > t hr eshol d %appl y dr i f t t hr eshol d t o r aw dat a ( DN)
%i mpl ement 5- poi nt movi ng aver ager
val 1( n) = ( 1/ f i l t ) *( v( n) + v( n- 1) + v( n- 2) + v( n- 3) + v( n- 4) ) ;
val 2( n) = v( n) ;
el se
val 1( n) = 0;
val 2( n) = 0;
end %i f
%di spl ay angl e af t er ever y 20 l oops, t r yi ng t o conser ve pr ocessi ng t i me
subpl ot ( 2, 1, 1)
pl ot ( x, val 1)
axi s( [ 1 sampl es+f i l t - 50 50] )
t i t l e( ' Wi t h Fi l t er i ng' )
subpl ot ( 2, 1, 2)
pl ot ( x, val 2)
axi s( [ 1 sampl es+f i l t - 50 50] )
t i t l e( ' Wi t hout Fi l t er i ng' )
dr awnow
14
end %f or
Figure 4.
4 Repeat Steps 2-3 except rotate the gyro but keep below the threshold of 50/sec. A
graph similar to the following figure (Figure 5) is displayed.
Figure 5.
15
Appendix A Arduino Sample Code
Pololu have written a basic Arduino library for the L3G4200D to configure the L3G4200D and
read the raw gyro data through IC.
Download the archive from "GitHub":https://github.com/pololu/L3G4200D, decompress it, and
drag the "L3G4200D" folder to your arduino-00xx/libraries directory. Then restart the Arduino
environment, so the libraries can be updated to include the L3G4200D library and its examples.
Step by Step Instructions
Open an example code sketch by selecting FileExamplesL3G4200DSerial
Serial
#i ncl ude <Wi r e. h>
#i ncl ude <L3G4200D. h>
L3G4200D gyr o;
16
voi d set up( ) {
Ser i al . begi n( 9600) ;
Wi r e. begi n( ) ;
gyr o. enabl eDef aul t ( ) ;
}
voi d l oop( ) {
gyr o. r ead( ) ;
Ser i al . pr i nt ( " G " ) ;
Ser i al . pr i nt ( " X: " ) ;
Ser i al . pr i nt ( ( i nt ) gyr o. g. x) ;
Ser i al . pr i nt ( " Y: " ) ;
Ser i al . pr i nt ( ( i nt ) gyr o. g. y) ;
Ser i al . pr i nt ( " Z: " ) ;
Ser i al . pr i nt l n( ( i nt ) gyr o. g. z) ;
del ay( 100) ;
}
This program continuously reads the gyro, communicating the readings over the serial interface.
You can display the readings with the Arduino Serial Monitor.
Example output:
G X: 188 Y: -10 Z: -47
G X: 138 Y: -40 Z: -26
G X: 110 Y: -55 Z: 4
17
Appendix B Compass Drawing
18