Lab 3 - Gyro and Acc lab (1)

Lab$3:$Calibrating$and$Combining$the$
Accelerometer$and$Gyroscope$data$
!
"#!$%&'!()*!+,!+&((!(,)-#!%.+!$.!-,)/!$%,!01--,#$!)#2(,!.3!4.1-!-.*.$!1'&#2!)!0.5*&#)$&.#!.3!)!24-.'0.6,!
)#/!)#!)00,(,-.5,$,-7!8%,!6-.0,''!.1$(&#,/!*,(.+!&'!3.-!$%,!9!:;<!=-/1&#.!'%&,(/!+%&0%!0.#$)&#'!$%,!
=:>?@AB!)00,(,-.5,$,-!)#/!$%,!"8CD@EFF!24-.'0.6,G!*1$!&$!+.1(/!*,!$%,!')5,!3.-!)#4!)00,(,-.5,$,-!
)#/!24-.'0.6,7!!
=#!)00,(,-.5,$,-!&'!1',/!$.!5,)'1-,!)#!.*H,0$I'!)00,(,-)$&.#!-,()$&J,!$.!3-,,3)((!'.!)#!.*H,0$!&#!3-,,3)((!
+.1(/!,K6,-&,#0,!L,-.!2G!+%,-,!)'!)#!.*H,0$!'&$$&#2!.#!$%,!3(..-!+.1(/!,K6,-&,#0,!M2!.3!)00,(,-)$&.#!&#!
$%,!16+)-/'!/&-,0$&.#7!8%&'!)00,(,-)$&.#!N.-!3.-0,O!&'!-,P1&-,/!$.!0.1#$,-)0$!$%,!)00,(,-)$&.#!.3!2-)J&$4!
)0$&#2!/.+#+)-/'!.#!$%,!.*H,0$G!Q,,6&#2!&$!3-.5!3)((&#27!"3!$%,!)00,(,-)$&.#'!&#!J)-&.1'!)K,'!)-,!Q#.+#G!
$-&2.#.5,$-4!0)#!*,!1',/!$.!/,$,-5&#,!$%,!)#2(,!.3!$%,!.*H,0$7!!
=!24-.'0.6,!&'!1',/!$.!5,)'1-,!$%,!)#21()-!J,(.0&$4!.3!)#!.*H,0$!N/,2-,,'R',0.#/O7!8%&'!J)(1,!0)#!*,!
&#$,2-)$,/!$.!2&J,!/,2-,,'!N$%,!)5.1#$!)#!.*H,0$!%)'!-.$)$,/!.-!$%,!01--,#$!)#2(,O7!!
"#!.-/,-!$.!0.#$-.(!4.1-!'4'$,5!)001-)$,(4G!*.$%!$%,!)00,(,-.5,$,-!)#/!$%,!24-.'0.6,!-,)/&#2I'!)-,!
-,P1&-,/7!;#0,!4.1!%)J,!*.$%!.3!$%,!$%,',!-,)/&#2I'!$%,-,!)-,!J)-&.1'!'&2#)(!6-.0,''&#2!5,$%./'!4.1!
0)#!1',!$.!.*$)&#!$%,!)001-)$,!)#2(,7!8%,!$+.!5.'$!)001-)$,!'4'$,5'!)-,!$%,!S)(5)#!3&($,-!)#/!)!
0.56(,5,#$)-4!3&($,-7!"#!$%&'!()*!+,!+&((!1',!)!0.56(,5,#$)-4!3&($,-7!
8%,!'$,6'!3.-!.*$)&#&#2!)#!)001-)$,!)#2(,!5,)'1-,5,#$!3-.5!$%,!-)+!24-.'0.6,!)#/!)00,(,-.5,$,-!
/)$)!)-,!)'!3.((.+'T!
•!U,$!16!$%,!/,J&0,!0.--,0$(4!)#/!/.+#(.)/!$%,!-,P1&-,/!(&*-)-&,'!$.!-,)/!-)+!/)$)!
•!V)(&*-)$,!$%,!=00,(,-.5,$,-!)#/!C4-.'0.6,!'.!4.1!Q#.+!+%)$!$%,4!)-,!.1$61$$&#2!N+%)$!1#&$'G!
+%)$!'.-$!.3!.33',$!,$0O!
•!V.#J,-$!$%,!)00,(,-.5,$,-!-,)/&#2!&#$.!/,2-,,'!N5,$%./'!/&'01'',/!*,(.+O!
•!V.#J,-$!$%,!24-.'0.6,!-,)/&#2!&#$.!/,2-,,'!N*4!&#$,2-)$&.#O!
•!?.+!6)''!3&($,-!$%,!)00,(,-.5,$,-!)#2(,!
•!W&2%!6)''!3&($,-!$%,!24-.'0.6,!)#2(,!
•!V.5*&#,!$%,!$+.!-,)/&#2I'!1'&#2!$%,!0.56(,5,#$)-4!3&($,-!$,0%#&P1,!
!
!
!

Accelerometer)and)Gyroscope)Set2up)
!
8.!',$!16!$%,!)00,(,-.5,$,-!)#/!24-.'0.6,G!')56(,!=-/1&#.!0./,!3.-!$%,!U&K!:.3!U%&,(/!+)'!/.+#(.)/,/!
3-.5!$%,!"#$,-#,$7!8%,!0./,!&'!'6,0&3&0!3.-!$%,!$46,!.3!/,J&0,!)#/!&'!+-&$$,#!3.-!)#!=-/1&#.7!=(5.'$!,J,-4!
5./,(!+&((!%)J,!'.5,!'.-$!.3!,K)56(,!0./,!$%)$!0.#$)&#'!(&*-)-&,'!4.1!0)#!1',7!8%,!(&*-)-&,'!*,(.+!
0-,)$,!$%,!<-,,U&K"XY!0()''!+%&0%!0.#$)&#'!$%,!<-,,U&K"XY!.*H,0$!)#/!J)-&.1'!31#0$&.#'!$%)$!0)#!*,!
6,-3.-5,/!.#!&$7!!8%,!.#(4!31#0$&.#!3-.5!$%&'!0()''!$%)$!+)'!1',/!+)'!2,$Z)+[)(1,'G!+%&0%!-,)/'!$%,!
-)+!N1#0)(&*-)$,/O!J)(1,'!3-.5!$%,!)00,(,-.5,$,-!)#/!24-.'0.6,7!\,!+&((!$%,#!3&($,-!)#/!0)(&*-)$,!$%,!
/)$)!.1-',(J,'7!!
8%,!(&*-)-&,'!3.-!$%,!9/.3!'%&,(/!0)#!*,!3.1#/!)$T!
%$$6TRR+++7/3-.*.$70.5R+&Q&R&#/,K76%6R9]:.3]U%&,(/]N:<ZFEF^O!
8%,!(&*-)-&,'!#,,/!$.!*,!/.+#(.)/,/!3-.5!$%,!6-./10$!6)2,7!8%&'!(&#Q!&'!.#!$%,!\&Q&6,/&)!6)2,!1#/,-!
,K)56(,!0./,7!8%,4!51'$!$%,#!*,!)//,/!$.!$%,!=-/1&#.!(&*-)-&,'!3.(/,-!'.!$%,!31#0$&.#!0)#!)00,''!$%,5!
N',,!()*!M!&3!4.1!3.-2.$!%.+!$.!/.!$%)$O7!_.1!+&((!#,,/!$%,!<-,,U&K"XYG!<"XY]=:>?@AB!)#/!$%,!
<"XY]"8C@EFF!7%!)#/!7066!3&(,'7!!
;#0,!$%,!(&*-)-&,'!)-,!&#!6()0,G!4.1!0)#!-,3,-,#0,!$%,5!3-.5!)#4!31#0$&.#!.-!0()''!*4!1'&#2!$%,!`&#0(1/,!
)-215,#$7!U,,!,K)56(,7!
#include <FreeSixIMU.h>!
#include <FIMU_ADXL345.h>!
#include <FIMU_ITG3200.h>!
!=3$,-!4.1!%)J,!',$!16!$%,!(&*-)-&,'!)#/!0)#!-,)/!-)+!/)$)!3-.5!$%,!*.)-/G!4.1!)-,!-,)/4!$.!'$)-$!
0)(&*-)$&#2!)#/!3&($,-&#2!$%,!/)$)7!8%,!#,K$!',0$&.#'!/&'01''!%.+!$.!0)(&*-)$,!$%,!)00,(,-.5,$,-!)#/!
24-.'0.6,!/)$)!'.!$%,4!-,$1-#!$%,!-.*.$!)#2(,!&#!/,2-,,'!&#'$,)/!.3!)!J)(1,!&#!?Ua'!N(,)'$!'&2#&3&0)#$!
*&$'O7!!
! !

Accelerometer)
!
\%,#!',$$&#2!16!$%,!)00,(,-.5,$,-!5)Q,!'1-,!$.!0%,0Q!$%,!-)#2,!)#/!',#'&$&J&$47!;#!$%,!=:>?@AB!$%,!
/,3)1($!-)#2,!&'!DEFFF!$.!EFFF!/,2R'7!8%&'!0)##.$!*,!5./&3&,/G!*1$!&3!4.1!0)#!0%)#2,!$%,!-)#2,!.#!4.1-!
)00,(,-.5,$,-!4.1!5&2%$!+)#$!$.!/,0-,)',!&$!'.!&$!&'!5.-,!',#'&$&J,!$.!'5)((,-!0%)#2,'!)'!4.1!+&((!(&Q,(4!
#.$!',,!-.$)$&.#!J)(1,'!)'!()-2,!)'!EFFF!/,2R'7!!
"#!.-/,-!$.!.*$)&#!$%,!)#2(,!3-.5!$%,!)00,(,-.5,$,-!/)$)G!$%,-,!)-,!)!3,+!5,$%./'!4.1!0)#!1',7!\,!+&((!
2.!.J,-!$%-,,!5,$%./'!&#!$%&'!()*7!8%,',!5,$%./'!)-,!/&'01'',/!*,(.+7!
!
Small)Angle)Approximation)
!
8%,!'5)((!)#2(,!)66-.K&5)$&.#!5,$%./!1','!$-&2!$.!0)(01()$,!$%,!)#2(,G!*1$!)''15,'!$%)$!'&#b!c!b7!8%&'!&'!
)#!)001-)$,!,'$&5)$,!3.-!)#2(,'!(,''!$%)#!@Fd7!C&J,#!)!#.-5)(!5.$.-!0.#$-.((,-G!&3!$%,!$&($!)#2(,!&'!2-,)$,-!
$%)#!@F!/,2-,,'G!$%,!0.#$-.((,-!+&((!(&Q,(4!#,,/!$.!*,!-1##&#2!)$!31((!'6,,/!$.!0.--,0$!$%&'!$&($!)#/!
6-,0&'&.#!)#2(,!5,)'1-,5,#$!&'!#.$!-,P1&-,/!3.-!$%&'7!8%,-,3.-,!1'&#2!$%,!'5)((!)#2(,!)66-.K&5)$&.#!+&((!
+.-Q!+,((7!8%,!3.-51()!*,(.+!1','!$%&'!)''156$&.#!$.!0)(01()$,!$%,!KD)#2(,!3-.5!$%,!)00,(,-.5,$,-!
-,)/&#2G!$%,!4D)#2(,!&'!0)(01()$,/!$%,!')5,!+)4T!!
K])00!e!N3(.)$ONK])00]=:V!D!K])00].33',$O!f!K])00]'0)(,fMgFR6&h!
\%,-,!$%,!K])00]=:V!&'!$%,!-)+!)00,(,-.5,$,-!-,)/&#27!"$!+)'!3.1#/!1'&#2!)!31#0$&.#!0)((,/!
2,$Z)+:)$)NO!$%)$!0)5,!&#!$%,!/.+#(.)/,/!(&*-)-&,'!3.-!$%&'!)00,(,-.5,$,-7!
8%,!K])00]'0)(,!&'!$%,!0.#J,-'&.#!3)0$.-!$.!0.#J,-$!$%,!?Ua!-,)/&#2!&#$.!2I'7!"$!0)#!*,!3.1#/!&#!$%,!
)00,(,-.5,$,-!/)$)!'%,,$7!!8%,!=:>?!@AB!0.#J,-'&.#!3)0$.-!&'!A52R?Ua!3.-!)((!2!-)#2,'G!$%,-,3.-,!&$I'!
K])00]'0)(,!e!F7FFA7!"#!.-/,-!$.!1',!$%,!$-&2!0)(01()$&.#'!)#/!'5)((!)#2(,!)66-.K&5)$&.#G!$%,!)00,(,-)$&.#!
J)(1,!51'$!*,!&#!2I'7!!
8%,!.33',$!J)(1,!&'!$%,!J)(1,!$%,!)00,(,-.5,$,-!-,)/'!+%,#!&$!&'!'$)$&.#)-4!)#/!(4&#2!3()$7!"$!+)'!.*$)&#,/!
*4!)J,-)2&#2!$%,!-,)/&#2I'!.3!$%,!)#2(,!+%,#!$%,!)00,(,-.5,$,-!+)'!(4&#2!3()$!3.-!E!',0.#/'7!!
8%,!MgFR6&!0.#J,-$'!$%,!2!J)(1,!&#$.!)#!)#2(,!&#!/,2-,,'7!!
8%&'!5,$%./!2&J,'!)!3)&-(4!)001-)$,!-,)/&#2!3.-!$%,!)#2(,7!W.+,J,-G!*,0)1',!&$!.#(4!1','!.#,!)K&'!$.!
0)(01()$,!$%,!)#2(,!&$!+&((!2&J,!)#!)#2(,!*,$+,,#!D^F!)#/!^F!/,2-,,'7!8%&'!'%.1(/!*,!3&#,!3.-!)!*)()#0&#2!
)66(&0)$&.#!*,0)1',!&3!4.1!)-,!.33!*4!5.-,!$%)#!^F!/,2-,,'!$%)#!4.1!(&Q,(4!+.#I$!*,!)*(,!$.!0.--,0$!&$!
)#4+)47!"3!)#!)#2(,!*,$+,,#!D@9F!)#/!@9F!/,2-,,'!&'!-,P1&-,/G!$%,#!$%,!)00,(,-.5,$,-!-,)/&#2!3-.5!
$+.!)K,'!51'$!*,!1',/!$.!0)(01()$,!$%,!)#2(,7!!
!

Manual)Calibration)
!
=#.$%,-!5,$%./!$%)$!&'!J,-4!'&56(,!)#/!',,5'!$.!+.-Q!P1&$,!+,((!&'!$.!/,J,(.6!4.1-!.+#!K])00]'0)(,!
J)(1,!$.!-,()$,!/,2-,,'!$.!?Ua'7!_.1!0)#!$%,#!1',!$%,!3.((.+&#2!3.-51()T!
K])00!e!3(.)$NK])00]=:V!!D!K])00].33',$O!f!K])00]'0)(,!
=!2../!+)4!$.!3&#/!$%,!K])00]'0)(,!3.-!$%&'!0)',!&'!$.!61$!$%,!)00,(,-.5,$,-!&#!)!6.'&$&.#!+%,-,!&$!'%.1(/!
-,)/!^F!/,2-,,'!.#!)!2&J,#!)K&'!!N&7,7!%.(/!$%,!)00,(,-.5,$,-!'.!$%,!KD)K&'!&'!6.&#$&#2!16O7!_.1-!!
0.#J,-'&.#!3)0$.-!!+.1(/!$%,#!*,!,P1)(!$.!^FdR?Ua!-,)/&#27!8%,!?Ua!-,)/&#2!&'!$%,!-)+!)00,(,-.5,$,-!
.1$61$!+%,#!&$!&'!-.$)$,/!$.!^F!/,2-,,'7!
8%&'!5,$%./!+.-Q'!+,((7!W.+,J,-G!(&Q,!$%,!'5)((!)#2(,!)66-.K&5)$&.#!5,$%./G!&$!+&((!.#(4!2&J,!)#!)#2(,!
*,$+,,#!D^F!)#/!^F!/,2-,,'7!!
"#!.-/,-!$.!2,$!$%,!K])00]'0)(,G!4.1!+&((!#,,/!$.!1',!)!0)(&*-)$,!31#0$&.#!(&Q,!$%,!.#,!*,(.+7!!8%&'!
31#0$&.#!+&((!-,)/!$%,!-)+!J)(1,'!.3!$%,!KD)K&'!.3!$%,!)00,(,-.5,$,-!+%,#!&$!&'!6.'&$&.#,/!)$!^F!/,2-,,'!
NKD)K&'!6.&#$&#2!16O7!Z,5,5*,-!)K!e!M2!+%,#!$%,!KD)K&'!&'!6.&#$&#2!16!*,0)1',!$%,!2!3.-0,!+&((!*,!16!
N0.1#$,-!)0$&#2!3-,,!3)((!+%&0%!+.1(/!0)1',!&$!$.!5.J,!/.+#O7!!
!
Figure'1)'Calibrate_Accleration'function'
8%,!)00]'0)(,!)--)4!%)'!)!(,#2$%!.3!$+.7!i.'&$&.#!L,-.!&'!3.-!$%,!K!'0)(,!)#/!6.'&$&.#!.#,!&'!3.-!$%,!4!
'0)(&#2!J)(1,7!"$!&'!)!2(.*)(!)--)4!'.!4.1!.#(4!#,,/!$.!-1#!$%,!0)(&*-)$,])00,(,-)$&.#NO!31#0$&.#!.#0,!)$!$%,!
*,2&##&#2!)#/!$%,#!$%)$!J)(1,!0)#!*,!1',/!$%-.12%.1$!$%,!31#0$&.#7!!;#0,!4.1!%)J,!3.1#/!$%,!'0)(&#2!
3)0$.-G!4.1!0)#!1',!&$!$.!3&#/!$%,!)#2(,!3-.5!$%,!)00,(,-.5,$,-!-)+!/)$)7!!

!
Figure'2)'Angle'calculation'from'accelerometer'data.'Using'manual'calibration'to'find'scaling'factor'
!
8%,!)*.J,!31#0$&.#!)('.!1','!$%,!&#&$&)(jk!)--)47!8%&'!&'!)#!)--)4!.3!)((!$%,!&#&$&)(!J)(1,'!-,)/!3-.5!$%,!
-.*.$!+%,#!&$!&'!(4&#2!3()$!N&7,7!$%,!.33',$O7!!
!
Trigonometry))
!
=#.$%,-!5,$%./!$.!0)(01()$,!$%,!)#2(,!&'!$.!1',!$-&2.#.5,$-4!!+&$%.1$!$%,!'5)((!)#2(,!)66-.K&5)$&.#7!
8%&'!&'!'.5,$&5,'!#.$!&/,)(!*,0)1',!&$!-,P1&-,'!$..!510%!6-.0,''&#2!$&5,7!"3!4.1!+&'%!$.!/.!$%&'G!4.1!0)#!
1',!)!31#0$&.#!(&Q,!$%,!.#,!*,(.+7!!8%&'!0./,!+)'!3.1#/!.#(&#,7!"$!&#61$'!$%,!-)+!K!)#/!4!)00,(,-.5,$,-!
-,)/&#2'!&#!@ED*&$!&#$,2,-'!)#/!0)(01()$,'!$%,!)#2(,7!!
!
Figure'3)'Function'to'calculate'angle'using'arc'tan'
8%&'!31#0$&.#!1','!*.$%!$%,!K!)#/!4!)K&'!)00,(,-.5,$,-!-,)/&#2I'7!8%,-,3.-,!&$!+&((!2&J,!)#!)#2(,!*,$+,,#!
D@9F!)#/!@9F!/,2-,,'7!!