/* Ċ Ġ* ĠCopyright Ġ 2 0 2 0 ĠWin T Ġ 3 7 9 4 Ġ( Man uel ĠD ia z ĠRo jo Ġand ĠAlex is ĠO bed ĠG arc ia ĠH ern ande z ) Ċ Ġ* Ċ Ġ* ĠLicensed Ġunder Ġthe ĠApache ĠLicense , ĠVersion Ġ 2 . 0 Ġ( the Ġ" License "); Ċ Ġ* Ġyou Ġmay Ġnot Ġuse Ġthis Ġfile Ġexcept Ġin Ġcompliance Ġwith Ġthe ĠLicense . Ċ Ġ* ĠYou Ġmay Ġobtain Ġa Ġcopy Ġof Ġthe ĠLicense Ġat Ċ Ġ* Ċ Ġ* Ġhttp :// www . apache . org / licenses / LICENSE - 2 . 0 Ċ Ġ* Ċ Ġ* ĠUnless Ġrequired Ġby Ġapplicable Ġlaw Ġor Ġagreed Ġto Ġin Ġwriting , Ġsoftware Ċ Ġ* Ġdistributed Ġunder Ġthe ĠLicense Ġis Ġdistributed Ġon Ġan Ġ" AS ĠIS " ĠBASIS , Ċ Ġ* ĠWITHOUT ĠWARRANTIES ĠOR ĠCONDITIONS ĠOF ĠANY ĠKIND , Ġeither Ġexpress Ġor Ġimplied . Ċ Ġ* ĠSee Ġthe ĠLicense Ġfor Ġthe Ġspecific Ġlanguage Ġgoverning Ġpermissions Ġand Ċ Ġ* Ġlimitations Ġunder Ġthe ĠLicense . Ċ Ġ* Ċ Ġ*/ Ċ package Ġorg . w int 3 7 9 4 . ft c . path follow er . controllers Ċ Ċ import Ġorg . w int 3 7 9 4 . ft c . path follow er . hardware . Dc Motor Base Ċ import Ġorg . w int 3 7 9 4 . ft c . path follow er . models . PID F Coefficient s Ċ import Ġorg . w int 3 7 9 4 . ft c . path follow er . models . PID Values Ċ Ċ /** Ċ Ġ* ĠA ĠPID Ġcontroller ĠClass . ĠThis Ġclass Ġwas Ġinspired Ġby ĠArc robot ics ĠPID ĠController . Ċ Ġ*/ Ċ class ĠPID Controller ( coefficient s : ĠPID F Coefficient s ) Ġ{ ĊĠĠĠ Ġprivate Ġval Ġk P : ĠDouble ĊĠĠĠ Ġprivate Ġval Ġk I : ĠDouble ĊĠĠĠ Ġprivate Ġval Ġk D : ĠDouble ĊĠĠĠ Ġprivate Ġval Ġk F : ĠDouble ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġperiod Ġof Ġtime Ġbetween Ġthe Ġinterval ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġperiod : ĠDouble ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠReturns Ġthe Ġcurrent Ġse tpoint Ġof Ġthe ĠPID F Controller . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ return ĠThe Ġcurrent Ġse tpoint . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠSets Ġthe Ġse tpoint Ġfor Ġthe ĠPID F Controller ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġsp ĠThe Ġdesired Ġse tpoint . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġset Point : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġmeasured Value : ĠDouble ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġpositional Ġerror Ġe ( t ) ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġposition Error : ĠDouble ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġvelocity Ġerror Ġe '( t ) ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġvar Ġvelocity Error Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġprivate Ġset ĊĠĠĠ Ġprivate Ġvar Ġtotal Error Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġprev Error Val Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġerror Tolerance _ p Ġ= Ġ 0 . 0 5 ĊĠĠĠ Ġprivate Ġvar Ġerror Tolerance _ v Ġ= ĠDouble . POSITIVE _ INFINITY ĊĠĠĠ Ġfun Ġreset () Ġ{ ĊĠĠĠĠĠĠĠ Ġtotal Error Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġprev Error Val Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ} ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠImplements Ġcalculation Ġonto Ġthe ĠD c Motor Base . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġmotor Ġ ĠD c Motor Base ĊĠĠĠĠ Ġ* Ġ@ param Ġvalues ĠPID Values ĊĠĠĠĠ Ġ* Ġ@ param Ġspeed Ġ ĠThe Ġmaximum Ġspeed Ġthe Ġmotor Ġshould Ġrotate . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠImplements Ġcalculation Ġonto Ġthe ĠD c Motor Base . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġmotor Ġ ĠD c Motor Base ĊĠĠĠĠ Ġ* Ġ@ param Ġvalues ĠPID Values ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Jvm Over loads ĊĠĠĠ Ġfun Ġcontrol ( motor : ĠD c Motor Base , Ġvalues : ĠPID Values , Ġspeed : ĠDouble Ġ= Ġ 1 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġsp Ġ= Ġvalues . sp ĊĠĠĠĠĠĠĠ Ġval Ġpv Ġ= Ġvalues . pv ĊĠĠĠĠĠĠĠ Ġif Ġ( Math . abs ( sp ) Ġ> ĠMath . abs ( pv )) Ġmotor . power Ġ= Ġspeed Ġ* Ġcalculate ( pv , Ġsp ) Ġelse Ġmotor . stop () ĊĠĠĠ Ġ} ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠImplements Ġcalculation Ġonto Ġthe ĠD c Motor Base . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġmotor ĠThe Ġaffected ĠD c Motor Base . ĊĠĠĠĠ Ġ* Ġ@ param Ġpv ĠĠĠ ĠThe Ġse tpoint Ġof Ġthe Ġcalculation ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠImplements Ġcalculation Ġonto Ġthe ĠD c Motor Base . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġmotor ĠD c Motor Base ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Jvm Over loads ĊĠĠĠ Ġfun Ġcontrol ( motor : ĠD c Motor Base , Ġpv : ĠDouble Ġ= Ġprev Error Val ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġvalues Ġ= ĠPID Values ( set Point , Ġpv ) ĊĠĠĠĠĠĠĠ Ġcontrol ( motor , Ġvalues ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠSets Ġthe Ġerror Ġwhich Ġis Ġconsidered Ġt oler able Ġfor Ġuse Ġwith Ġ[. at Set Point ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġposition Tolerance ĠP ose 2 d Ġerror Ġwhich Ġis Ġt oler able . ĊĠĠĠĠ Ġ* Ġ@ param Ġvelocity Tolerance ĠVe locity Ġerror Ġwhich Ġis Ġt oler able . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Tolerance ( position Tolerance : ĠDouble , Ġvelocity Tolerance : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġerror Tolerance _ p Ġ= Ġposition Tolerance ĊĠĠĠĠĠĠĠ Ġerror Tolerance _ v Ġ= Ġvelocity Tolerance ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠReturns Ġtrue Ġif Ġthe Ġerror Ġis Ġwithin Ġthe Ġpercentage Ġof Ġthe Ġtotal Ġinput Ġrange , Ġdetermined Ġby ĊĠĠĠĠ Ġ* Ġ[. set Tolerance ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ return ĠWhether Ġthe Ġerror Ġis Ġwithin Ġthe Ġacceptable Ġbounds . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġat Set Point (): ĠBoolean Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ( Math . abs ( position Error ) Ġ< Ġerror Tolerance _ p ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ&& ĠMath . abs ( velocity Error ) Ġ< Ġerror Tolerance _ v ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġthe ĠPID F Ġcoefficients ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġcoefficients : ĠDouble Array ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġdouble ArrayOf ( k P , Ġk I , Ġk D , Ġk F ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġt oler ances Ġof Ġthe Ġcontroller ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġval Ġtolerance : ĠDouble Array ĊĠĠĠĠĠĠĠ Ġget () Ġ= Ġdouble ArrayOf ( error Tolerance _ p , Ġerror Tolerance _ v ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠSets Ġthe Ġerror Ġwhich Ġis Ġconsidered Ġt oler able Ġfor Ġuse Ġwith Ġ[. at Set Point ]. ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġposition Tolerance ĠP ose 2 d Ġerror Ġwhich Ġis Ġt oler able . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġset Tolerance ( position Tolerance : ĠDouble ) Ġ{ ĊĠĠĠĠĠĠĠ Ġset Tolerance ( position Tolerance , ĠDouble . POSITIVE _ INFINITY ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠCalculates Ġthe Ġnext Ġoutput Ġof Ġthe ĠPID F Ġcontroller . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġpv ĠThe Ġgiven Ġmeasured Ġvalue . ĊĠĠĠĠ Ġ* Ġ@ param Ġsp ĠThe Ġgiven Ġse tpoint . ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġnext Ġoutput Ġusing Ġthe Ġgiven Ġmeas ur d Ġvalue Ġvia ĊĠĠĠĠ Ġ* Ġ[. calculate ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġcalculate ( pv : ĠDouble , Ġsp : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġ// Ġset Ġthe Ġse tpoint Ġto Ġthe Ġprovided Ġvalue ĊĠĠĠĠĠĠĠ Ġset Point Ġ= Ġsp ĊĠĠĠĠĠĠĠ Ġreturn Ġcalculate ( pv ) ĊĠĠĠ Ġ} ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠCalculates Ġthe Ġcontrol Ġvalue , Ġu ( t ). ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġpv ĠThe Ġcurrent Ġmeasurement Ġof Ġthe Ġprocess Ġvariable . ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġvalue Ġproduced Ġby Ġu ( t ). ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠCalculates Ġthe Ġnext Ġoutput Ġof Ġthe ĠPID F Ġcontroller . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġnext Ġoutput Ġusing Ġthe Ġcurrent Ġmeasured Ġvalue Ġvia ĊĠĠĠĠ Ġ* Ġ[. calculate ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ@ Jvm Over loads ĊĠĠĠ Ġfun Ġcalculate ( pv : ĠDouble Ġ= Ġmeasured Value ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġprev Error Val Ġ= Ġposition Error ĊĠĠĠĠĠĠĠ Ġif Ġ( meas ured Value Ġ== Ġpv ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġposition Error Ġ= Ġset Point Ġ- Ġmeasured Value ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġposition Error Ġ= Ġset Point Ġ- Ġpv ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmeasured Value Ġ= Ġpv ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġif Ġ( period Ġ!= Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvelocity Error Ġ= Ġ( position Error Ġ- Ġprev Error Val ) Ġ/ Ġperiod ĊĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvelocity Error Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġtotal Error Ġ= Ġperiod Ġ* Ġ( set Point Ġ- Ġmeasured Value ) ĊĠĠĠĠĠĠĠ Ġreturn Ġk P Ġ* Ġposition Error Ġ+ Ġk I Ġ* Ġtotal Error Ġ+ Ġk D Ġ* Ġvelocity Error Ġ+ Ġk F Ġ* Ġset Point ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠCreates Ġan Ġinstance Ġof ĠPID ĠController ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġcoefficients ĠPID F Coefficient s ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġk P Ġ= Ġcoefficients . k P ĊĠĠĠĠĠĠĠ Ġk I Ġ= Ġcoefficients . k I ĊĠĠĠĠĠĠĠ Ġk D Ġ= Ġcoefficients . k D ĊĠĠĠĠĠĠĠ Ġk F Ġ= Ġcoefficients . k F ĊĠĠĠĠĠĠĠ Ġval Ġvalues Ġ= Ġcoefficients . values ĊĠĠĠĠĠĠĠ Ġset Point Ġ= Ġvalues . sp ĊĠĠĠĠĠĠĠ Ġmeasured Value Ġ= Ġvalues . pv ĊĠĠĠĠĠĠĠ Ġperiod Ġ= Ġvalues . period ĊĠĠĠĠĠĠĠ Ġposition Error Ġ= Ġset Point Ġ- Ġmeasured Value ĊĠĠĠĠĠĠĠ Ġreset () ĊĠĠĠ Ġ} Ċ }