Embed Size (px)
Citation preview
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 1/15
//***********************************************************************
// Matlab .fs to arduino C converter v2.0.0.29032014
// - Karthik adi!" #$%
// &lease re'ort bu!s to( karthiknadi!)!ail.co
//***********************************************************************
+defne ,$& oat
+defne ,$$5#$ 101
+defne ,$M -32678
+defne ,$M% 32676
t:'ede; ,$&<*,$M,=<,$&" ,$&*=>
t:'ede; ,$&<*,$%&=<,$&" ,$&=>
t:'ede; ,$&<*,$%=<,$&*" int" ,$%&=>
//***********************************************************************
// Matlab .fs to arduino C converter v2.0.0.29032014
// - Karthik adi!" #$%
// &lease re'ort bu!s to( karthiknadi!)!ail.co
//***********************************************************************
+include ?fsheader.h?
// uber o; in'uts to the ;u@@: in;erence s:ste
const int fs!c A 2>
// uber o; out'uts to the ;u@@: in;erence s:ste
const int fs!c A 2>
// uber o; rules to the ;u@@: in;erence s:ste
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 2/15
const int fs!c A 2B>
,$& !fsn'utfs!cD>
,$& !fsut'utfs!cD>
// $etu' routine runs once Ehen :ou 'ress reset(
void setu'<=
F
// initiali@e the %nalo! 'ins ;or in'ut.
// &in ode ;or n'ut( Gelocidad
'inMode<0 " &#=>
// &in ode ;or n'ut( Histancia
'inMode<1 " &#=>
// initiali@e the %nalo! 'ins ;or out'ut.
// &in ode ;or ut'ut( ,reno
'inMode<2 " #&#=>
// &in ode ;or ut'ut( %celerador
'inMode<3 " #&#=>
I
// 5oo' routine runs over and over a!ain ;orever(
void loo'<=
F
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 3/15
// ead n'ut( Gelocidad
!fsn'ut0D A analo!ead<0=>
// ead n'ut( Histancia
!fsn'ut1D A analo!ead<1=>
!fsut'ut0D A 0>
!fsut'ut1D A 0>
fsevaluate<=>
// $et out'ut vlaue( ,reno
analo!Jrite<2 " !fsut'ut0D=>
// $et out'ut vlaue( %celerador
analo!Jrite<3 " !fsut'ut1D=>
I
//***********************************************************************
// $u''ort ;unctions ;or ,u@@: n;erence $:ste
//***********************************************************************
// rian!ular Meber ,unction
,$& fstri;<,$& " ,$&* '=
F
,$& a A '0D" b A '1D" c A '2D>
,$& t1 A < - a= / <b - a=>
,$& t2 A <c - = / <c - b=>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 4/15
i; <<a AA b= LL <b AA c== return <,$&= < AA a=>
i; <a AA b= return <,$&= <t2*<b A =*< A c==>
i; <b AA c= return <,$&= <t1*<a A =*< A b==>
t1 A in<t1" t2=>
return <,$&= a<t1" 0=>
I
,$& fsin<,$& a" ,$& b=
F
return in<a" b=>
I
,$& fsa<,$& a" ,$& b=
F
return a<a" b=>
I
,$& fsarra:o'eration<,$& *arra:" int si@e" ,$%& ';n'=
F
int i>
,$& ret A 0>
i; <si@e AA 0= return ret>
i; <si@e AA 1= return arra:0D>
ret A arra:0D>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 5/15
;or <i A 1> i si@e> iNN=
F
ret A <*';n'=<ret" arra:iD=>
I
return ret>
I
//***********************************************************************
// Hata ;or ,u@@: n;erence $:ste
//***********************************************************************
// &ointers to the i'leentations o; eber ;unctions
,$M, fs!M,D A
F
fstri;
I>
// Count o; eber ;unction ;or each n'ut
int fs!M,CountD A F B" B I>
// Count o; eber ;unction ;or each ut'ut
int fs!M,CountD A F B" B I>
// CoeOcients ;or the n'ut Meber ,unctions
,$& fs!M,0CoeP1D A F 0" 0" 20 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 6/15
,$& fs!M,0CoeP2D A F B0" 60" 100 I>
,$& fs!M,0CoeP3D A F 170" 169.72972972973" 200 I>
,$& fs!M,0CoeP4D A F 100" 130" 170 I>
,$& fs!M,0CoePBD A F 20" 3B" B0 I>
,$&* fs!M,0CoePD A F fs!M,0CoeP1" fs!M,0CoeP2"
fs!M,0CoeP3" fs!M,0CoeP4" fs!M,0CoePB I>
,$& fs!M,1CoeP1D A F 0" 10" 20 I>
,$& fs!M,1CoeP2D A F 20" 30" 40 I>
,$& fs!M,1CoeP3D A F 90" 9B" 100 I>
,$& fs!M,1CoeP4D A F 40" BB" 60 I>
,$& fs!M,1CoePBD A F 60" 80" 90 I>
,$&* fs!M,1CoePD A F fs!M,1CoeP1" fs!M,1CoeP2"
fs!M,1CoeP3" fs!M,1CoeP4" fs!M,1CoePB I>
,$&** fs!M,CoePD A F fs!M,0CoeP" fs!M,1CoeP I>
// CoeOcients ;or the n'ut Meber ,unctions
,$& fs!M,0CoeP1D A F 0" 0" 20 I>
,$& fs!M,0CoeP2D A F 40" BB" 60 I>
,$& fs!M,0CoeP3D A F 90" 9B" 100 I>
,$& fs!M,0CoeP4D A F 60" 80" 90 I>
,$& fs!M,0CoePBD A F 20" 30" 40 I>
,$&* fs!M,0CoePD A F fs!M,0CoeP1" fs!M,0CoeP2"
fs!M,0CoeP3" fs!M,0CoeP4" fs!M,0CoePB I>
,$& fs!M,1CoeP1D A F 0" 10" 20 I>
,$& fs!M,1CoeP2D A F 40" B0" 70 I>
,$& fs!M,1CoeP3D A F 80" 90" 100 I>
,$& fs!M,1CoeP4D A F 70" 60" 80 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 7/15
,$& fs!M,1CoePBD A F 20" 30" 40 I>
,$&* fs!M,1CoePD A F fs!M,1CoeP1" fs!M,1CoeP2"
fs!M,1CoeP3" fs!M,1CoeP4" fs!M,1CoePB I>
,$&** fs!M,CoePD A F fs!M,0CoeP" fs!M,1CoeP I>
// n'ut ebershi' ;unction set
int fs!M,0D A F 0" 0" 0" 0" 0 I>
int fs!M,1D A F 0" 0" 0" 0" 0 I>
int* fs!M,D A F fs!M,0" fs!M,1I>
// ut'ut ebershi' ;unction set
int fs!M,0D A F 0" 0" 0" 0" 0 I>
int fs!M,1D A F 0" 0" 0" 0" 0 I>
int* fs!M,D A F fs!M,0" fs!M,1I>
// ule Jei!hts
,$& fs!Jei!htD A F 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1"
1" 1" 1" 1" 1" 1 I>
// ule :'e
int fs!:'eD A F 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1" 1"
1" 1 I>
// ule n'uts
int fs!0D A F 1" 1 I>
int fs!1D A F 1" 2 I>
int fs!2D A F 1" 3 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 8/15
int fs!3D A F 1" 4 I>
int fs!4D A F 1" B I>
int fs!BD A F 2" 1 I>
int fs!7D A F 2" 2 I>
int fs!6D A F 2" 3 I>
int fs!8D A F 2" 4 I>
int fs!9D A F 2" B I>
int fs!10D A F 3" 1 I>
int fs!11D A F 3" 2 I>
int fs!12D A F 3" 3 I>
int fs!13D A F 3" 4 I>
int fs!14D A F 3" B I>
int fs!1BD A F 4" 1 I>
int fs!17D A F 4" 2 I>
int fs!16D A F 4" 3 I>
int fs!18D A F 4" 4 I>
int fs!19D A F 4" B I>
int fs!20D A F B" 1 I>
int fs!21D A F B" 2 I>
int fs!22D A F B" 3 I>
int fs!23D A F B" 4 I>
int fs!24D A F B" B I>
int* fs!D A F fs!0" fs!1" fs!2" fs!3" fs!4" fs!B"
fs!7" fs!6" fs!8" fs!9" fs!10" fs!11" fs!12" fs!13"
fs!14" fs!1B" fs!17" fs!16" fs!18" fs!19" fs!20"
fs!21" fs!22" fs!23" fs!24 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 9/15
// ule ut'uts
int fs!0D A F 1" 1 I>
int fs!1D A F 1" 1 I>
int fs!2D A F 2" 2 I>
int fs!3D A F 2" 3 I>
int fs!4D A F 4" 3 I>
int fs!BD A F 1" 2 I>
int fs!7D A F B" 2 I>
int fs!6D A F 4" 3 I>
int fs!8D A F 2" 2 I>
int fs!9D A F 4" 4 I>
int fs!10D A F B" 4 I>
int fs!11D A F B" 4 I>
int fs!12D A F 3" 3 I>
int fs!13D A F 4" 4 I>
int fs!14D A F 4" 3 I>
int fs!1BD A F B" 2 I>
int fs!17D A F 2" 4 I>
int fs!16D A F 3" 3 I>
int fs!18D A F 2" 4 I>
int fs!19D A F 4" 3 I>
int fs!20D A F 1" 1 I>
int fs!21D A F 1" B I>
int fs!22D A F 4" 3 I>
int fs!23D A F B" 2 I>
int fs!24D A F 2" 4 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 10/15
int* fs!D A F fs!0" fs!1" fs!2" fs!3" fs!4" fs!B"
fs!7" fs!6" fs!8" fs!9" fs!10" fs!11" fs!12"
fs!13" fs!14" fs!1B" fs!17" fs!16" fs!18"
fs!19" fs!20" fs!21" fs!22" fs!23" fs!24 I>
// n'ut ran!e Min
,$& fs!MinD A F 0" 0 I>
// n'ut ran!e Ma
,$& fs!MaD A F 200" 100 I>
// ut'ut ran!e Min
,$& fs!MinD A F 0" 0 I>
// ut'ut ran!e Ma
,$& fs!MaD A F 100" 100 I>
//***********************************************************************
// Hata de'endent su''ort ;unctions ;or ,u@@: n;erence $:ste
//***********************************************************************
,$& fsM,out<,$&** ;u@@:ule$et" ,$& " int o=
F
,$& ;ut>
int r>
;or <r A 0> r fs!c> NNr=
F
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 11/15
int inde A fs!rDoD>
i; <inde Q 0=
F
inde A inde - 1>
;ut A <fs!M,fs!M,oDindeDD=<" fs!M,CoePoDindeD=>
I
else i; <inde 0=
F
inde A -inde - 1>
;ut A 1 - <fs!M,fs!M,oDindeDD=<" fs!M,CoePoDindeD=>
I
else
F
;ut A 0>
I
;u@@:ule$et0DrD A fsin<;ut" ;u@@:ule$et1DrD=>
I
return fsarra:o'eration<;u@@:ule$et0D" fs!c" fsa=>
I
,$& fsde;u@@centroid<,$&** ;u@@:ule$et" int o=
F
,$& ste' A <fs!MaoD - fs!MinoD= / <,$$5#$ - 1=>
,$& area A 0>
,$& oentu A 0>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 12/15
,$& dist" slice>
int i>
// calculate the area under the curve ;ored b: the M, out'uts
;or <i A 0> i ,$$5#$> NNi=F
dist A fs!MinoD N <ste' * i=>
slice A ste' * fsM,out<;u@@:ule$et" dist" o=>
area NA slice>
oentu NA slice*dist>
I
return <<area AA 0= R <<fs!MaoD N fs!MinoD= / 2= ( <oentu /
area==>
I
//***********************************************************************
// ,u@@: n;erence $:ste
//***********************************************************************
void fsevaluate<=
F
,$& ;u@@:n'ut0D A F 0" 0" 0" 0" 0 I>
,$& ;u@@:n'ut1D A F 0" 0" 0" 0" 0 I>
,$&* ;u@@:n'utfs!cD A F ;u@@:n'ut0" ;u@@:n'ut1" I>
,$& ;u@@:ut'ut0D A F 0" 0" 0" 0" 0 I>
,$& ;u@@:ut'ut1D A F 0" 0" 0" 0" 0 I>
,$&* ;u@@:ut'utfs!cD A F ;u@@:ut'ut0" ;u@@:ut'ut1" I>
,$& ;u@@:ulesfs!cD A F 0 I>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 13/15
,$& ;u@@:,iresfs!cD A F 0 I>
,$&* ;u@@:ule$etD A F ;u@@:ules" ;u@@:,ires I>
,$& sJ A 0>
// rans;orin! in'ut to ;u@@: n'ut
int i" S" r" o>
;or <i A 0> i fs!c> NNi=
F
;or <S A 0> S fs!M,CountiD> NNS=
F
;u@@:n'utiDSD A
<fs!M,fs!M,iDSDD=<!fsn'utiD" fs!M,CoePiDSD=>
I
I
int inde A 0>
;or <r A 0> r fs!c> NNr=
F
i; <fs!:'erD AA 1=
F
;u@@:,iresrD A ,$M%>
;or <i A 0> i fs!c> NNi=
F
inde A fs!rDiD>
i; <inde Q 0=
;u@@:,iresrD A fsin<;u@@:,iresrD" ;u@@:n'utiDinde - 1D=>
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 14/15
else i; <inde 0=
;u@@:,iresrD A fsin<;u@@:,iresrD" 1 - ;u@@:n'utiD-inde - 1D=>
else
;u@@:,iresrD A fsin<;u@@:,iresrD" 1=>
I
I
else
F
;u@@:,iresrD A ,$M>
;or <i A 0> i fs!c> NNi=
F
inde A fs!rDiD>
i; <inde Q 0=
;u@@:,iresrD A fsa<;u@@:,iresrD" ;u@@:n'utiDinde - 1D=>
else i; <inde 0=
;u@@:,iresrD A fsa<;u@@:,iresrD" 1 - ;u@@:n'utiD-inde - 1D=>
else
;u@@:,iresrD A fsa<;u@@:,iresrD" 0=>
I
I
;u@@:,iresrD A fs!Jei!htrD * ;u@@:,iresrD>
sJ NA ;u@@:,iresrD>
I
i; <sJ AA 0=
7/23/2019 carro_vel
http://slidepdf.com/reader/full/carrovel 15/15
F
;or <o A 0> o fs!c> NNo=
F
!fsut'utoD A <<fs!MaoD N fs!MinoD= / 2=>
I
I
else
F
;or <o A 0> o fs!c> NNo=
F
!fsut'utoD A fsde;u@@centroid<;u@@:ule$et" o=>
I
I
I
