From d6e36a556cd7b5cefbb1e25008cf5e773433b740 Mon Sep 17 00:00:00 2001
From: martinvrican <56957445+martinvrican@users.noreply.github.com>
Date: Tue, 23 May 2023 17:40:41 +0200
Subject: [PATCH] LQ tune

---
 .../LinkShield_Identification.ino             |   4 +-
 .../LinkShield_LQ/LinkShield_LQ.ino           |  18 ++--
 .../LinkShield/LinkShield_Identification_TF.m |  17 ++--
 .../LinkShield/LinkShield_LQ_calculation.m    |   6 +-
 matlab/examples/LinkShield/sys.mat            | Bin 4532 -> 4622 bytes
 src/LinkShield.h                              |  82 ++++++++----------
 6 files changed, 64 insertions(+), 63 deletions(-)

diff --git a/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino b/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino
index b31937a3..5eaacf65 100644
--- a/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino
+++ b/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino
@@ -27,7 +27,7 @@ bool endExperiment = false;           	// Boolean flag to end the experiment
 float y_1 = 0.0;                        	// Output variable
 float y_2 = 0.0;                        	// Output variable
 float u = 0.0;                        	// Input (open-loop), initialized to zero
-float U[]={0.00, 5.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};   	// Input trajectory
+float U[]={0.00, -5.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 5.00, 0.00};   	// Input trajectory
 int T = 25;                         	// Section length (appr. '/.+2 s)
 unsigned long int i = 0;                            	// Section counter
 
@@ -87,7 +87,7 @@ void step(){
 	//LinkShield.actuatorWritePercent(u);	// Actuate with percent of PWM duty cicle
 
 	//Actuate with voltage converted to PWM duty cicle with square root and direction(+/-5V) 
-	LinkShield.actuatorWrite(u);	
+	LinkShield.actuatorWriteNew(u);	
 
 	Serial.print(y_1,8);                     // Print outputs
 	Serial.print(", ");
diff --git a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino
index f7b03d11..589429e7 100644
--- a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino
+++ b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino
@@ -51,7 +51,10 @@ BLA::Matrix<4, 1> Xk = {0, 0, 0, 0};
 BLA::Matrix<4, 1> xIC = {0, 0, 0, 0};
 
 
-BLA::Matrix<1, 4> K = {6.0679*4, -15.234/10, 0.40104, -3.6504/5};
+//BLA::Matrix<1, 4> K = {6.0679, -15.234, 0.40104, -3.6504};
+BLA::Matrix<1, 4> K = {21.927, -116.4, 0.90889, -4.8342};
+
+
 BLA::Matrix<4, 4> A = {1, 0.03235, 0.00475, 5e-05, 0, 0.9043, 6e-05, 0.00484, 0, 12.508, 0.90345, 0.03235, 0, -37.604, 0.02245, 0.9043};
 BLA::Matrix<4, 1> B = {0.00068, -0.0004, 0.26726, -0.15689};
 BLA::Matrix<2, 4> C = {1, 0, 0, 0, 0, 1, 0, 0};
@@ -84,7 +87,7 @@ void loop() {
 
 void stepEnable(){                                     // ISR
   if(endExperiment == true){                           // If the experiment is over
-	LinkShield.actuatorWrite(0.00);
+	LinkShield.actuatorWriteNew(0.00);
     while(1);      								// Do nothing
   }
   if(nextStep) {                               // If previous sample still running
@@ -104,10 +107,13 @@ if (i > sizeof(R) / sizeof(R[0])) {      // If at end of trajectory
 		endExperiment = true;           // Stop program execution at next ISR
     }
 	else if (k % (T*i) == 0) {      // If at the end of section
-        Xr(0) = R[i];                     // Progress in trajectory
+       Xr(0) = R[i];                     // Progress in trajectory
+	   
         i++;                          // Increment section counter
     }
 
+//Xr(0) = LinkShield.referenceRead();
+
 y_1 = LinkShield.servoPotRead();          // Read sensor
 y_2 = LinkShield.flexRead();
 
@@ -146,13 +152,13 @@ y_2prev = y_2;
 
 //LinkShield.actuatorWritePWM(u);          // Actuate
 //LinkShield.actuatorWritePercent(u);
-LinkShield.actuatorWrite(u);
+LinkShield.actuatorWriteNew(u);
 
 
 Serial.print(y_1,4);                      // Print output
 Serial.print(", ");
-//Serial.print(X(0),4);
-//Serial.print(", "); */
+Serial.print(X(0),4);
+Serial.print(", "); 
 Serial.print(y_2,4);
 //Serial.print(", ");
 //Serial.println(X(1),4);
diff --git a/matlab/examples/LinkShield/LinkShield_Identification_TF.m b/matlab/examples/LinkShield/LinkShield_Identification_TF.m
index a1f2880b..0d3d63bf 100644
--- a/matlab/examples/LinkShield/LinkShield_Identification_TF.m
+++ b/matlab/examples/LinkShield/LinkShield_Identification_TF.m
@@ -22,13 +22,13 @@
 startScript;                                    % Clears screen and variables, except allows CI testing
 
 %% Data preprocessing
-load resultID.mat                              % Read identificaiton results
-Ts=0.003;                                      % Sampling
-data=iddata(y,u,Ts)                            % Create identification data object
+load LinkShield_ID_Data.mat                             % Read identificaiton results
+Ts=0.005;                                      % Sampling
+data=iddata(Alpha,U,Ts)                            % Create identification data object
                                  
 % Select an appropriate section                   
-dataS  = data([1992:3971]);                    % Select data section (S)
-dataSR = dataS([136:1920]);                    % Refine selection (R)
+%dataS  = data([1992:3971]);                    % Select data section (S)
+%dataSR = dataS([136:1920]);                    % Refine selection (R)
                                        
 %% Transfer function estimation       
 
@@ -36,9 +36,10 @@
 Options = tfestOptions;                        % Create an options object
 Options.Display = 'on';                        % Display procedure
 Options.WeightingFilter = [];                  % No weighting filter
+Options.InitialState = 'zero';
                        
 % Launch and save identification
-sys = tfest(dataSR, 2, 0, Options)              % Identify transfer function
+sys = tfest(data, 2, 0, Options)              % Identify transfer function
 save sys sys                                   % Save transfer function
  
 %% Data and display
@@ -50,6 +51,6 @@
 c=sys.Numerator(1)/(-omega^2)                 % Actuator constant
 
 % Comparison
-compare(sys,dataS)                            % Comparison of data to model
+compare(sys,data)                            % Comparison of data to model
 xlabel('Time')                                % X axis label
-ylabel('Accel. y(t) ms^{-2}')                 % Y axis label
\ No newline at end of file
+ylabel('Angle (rad)')                 % Y axis label
\ No newline at end of file
diff --git a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m
index 6973f8bf..743f66cf 100644
--- a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m
+++ b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m
@@ -47,15 +47,15 @@
 %% LQ gain calculation
 
 if integrator
-Rlq= 1e-2;
+Rlq= 1e-4;
 %Qlq=diag([1 1.2 0 0 0 0]);
 Qlq=diag([1e-2 1e-10 1e2 1e3 1e-2 1e-2]);
 
 [K, P] = dlqr(Ai, Bi, Qlq, Rlq);
 else 
-Rlq = 1;
+Rlq = 1e-2;
 %Qlq = diag([1e0 3e2 1e-15 1e-15]);
-Qlq = diag([125 1 1 50]);
+Qlq = diag([1250 1 1 75]);
 
 
 [K, P] = dlqr(AD, BD, Qlq, Rlq);
diff --git a/matlab/examples/LinkShield/sys.mat b/matlab/examples/LinkShield/sys.mat
index ddb32b18c05c7c4d215f761b3c4aa6a8848695f3..728f312c8649547ae578270ed1903c11e1750d07 100644
GIT binary patch
delta 4484
zcmV-~5qs{mBaS4HG#E{9ZXiuzc_1<}ATT*PGB7$ZIUq7HGBc4;BavVQk#u2yH4y*+
zc%0-}OLG)e6h0FYFbN4kM8eAlh);xosHh;pyco>OBqWGRb2D?3X`ShAyKfIke4#6s
zR=L!&T)J}2!UY?v_!I72=+5#Ncux1Z(<kj15F#ncTXpJw=bU@b^WN^xq#FRPI{=$;
zegLT*($U6z0qIL8ooTc%4~61?81damzg^Duw=~&rDV4V-x&p5m8<?23cD@&Ow)6Tz
zV{T2>65oY1>{j($sDeO;j>G$}i2a$rl>agp2d%%+^UryI)w6ni3(Hz@{+N8>!am%5
z6X_am{$X3Hw_{d?u9~lE2QC=5>Y8P^5b1DG83jczh|RdFp-y?r5Sh?_^%hg<1<y4O
zOvI+KBE_k^I5F+6plrsi=~P1Y9GEs?TrzQ}VLYzsP#HgjG1qfzam|4-wUmP4P~+a|
zlA@qeL$j(H;}jGJfxI<>hCqd?=9Xd9FDc{tUIdfMRA5$z5po#og73h9?-|#N{Wwb3
z!;D&lLGAf9*Gpn3P7LaQs#*rK071<!>#8+?sSLnb2QK!Z6$da><Ekp!E4$<kW+GG#
z_;r{{qK08rLVLQ5B8t!}X>iMdDt!PmFz&mNp0(nlXLLBNY5^u8Ru4tSttFXE1Zh6S
z)F+l<Oq+^dhVc_~+Fh(*QO8f5^Fw!q!o*XC_Nd+%Tx67Il7be0QHX8ut5|F_@4(sE
z_+)XW66&bpSIaQyMgirHtEtv76o!77sYhK;t8m({)-~2?fQnhGDX*NgAvp>Y3k#7p
z+<#^`4J|U8(u;8wnUy3v2*PQ!TdJf<ISoEd8v`xZX<NAgq&mW?`YAdRC2|<?CS9*?
zlebkH@l+#u7#*8`HYzm9SxOikP}Xl?TS}=_<c4D}VTS6sjE@u^(MdWt8ml=_QQjhD
zQ$=N67i&T2*L6*!Pck^A%W<+FBScx0)-8*Hd%}h>p84d?W%W+Wn06S?qmE3LYg%u4
za*EL;o;J6X)-2tHcQic>4vcENkXVr9dJip`P}%xT>A(+vP2((OM+r}ofkw6w@CJM)
zlwk_{jv*VYo{G#wz<JQURt7H@A5gmM$@&=`CO3@AA-jzkVY+eEL@b_J4&>2ibM{V_
z=~<K{jH!);ZgN`c!zC{%NAjGIf&onhB1u}y#)6b(wOhu^o~$1U{W!4CFJ{Jlqaz2>
zCmU`Z_rn^0-N7WhB-*B9v%tMby9ECT(tg1oL^_b$|BS##k&X!dJW?^o&j|h!(%XWs
z<#@99Vb0%G`1`Q$JDNA;`24&j$G7MBPjmcdIsRIX|2)Tk(Ih@Qwl;}-A7t-A0QpW{
zEpHLCly4HVl(&kxGv}9VJS^5oTb;tDw6$5pU`HE&Q+|E^d&Q0|O!@V+wxh}VcBZzR
zoYr>1>az<R`_?9Ml66D&9byOB*v|gtptU_5s~p>yI&yMayR(UXvi(pK`wt@RXkvc{
z>tr8o?z?hwn`3WIZf)+)$!!dKa&jB<ULo5CfB!zSygg?mYh~OD*}anAi1yo$a0e6N
zPA0;CTbT$iA%%>Wks`(`NQUu8NHJsjR;)Argl%>){~XdG#>bclA7>)`0u$kvkxnok
zM{*cXAYEZhG5CxrhJZ1}@GRr2T%&I0>7G5tnC8bBKg;#8wWhdx7}MVEj7e|L1nGA$
zwr8ur`kjnPW}hiLxqmi}0`Hf8cL~3}!tZW>;dhVlyI1(#C;UDl{65;m@BW<M$A#Ym
z!tX)h_X*+mknsDY@cXpzYu`oE?^E25Cz(IYGQ!XEe!}$LYGFO`M_Es}kL_*}`V%ZC
z{GwR<636x;^CwwG_!UUc8R1u1NBA|?5kAE_!lzkB_;uD1KEpb~{j4K=mUV>RU>)Iq
z0oD;7WF6rl))5|N9pMqy5kAK{!bR2*eiLbq@hH+o#$#e1&F3K--J&j&NQ&`WNb`)R
zMBMbpYl$&^H<TG)K+=q>NDGYV`(cr>hg4xqch+T0clLe8^lkA0<B)9;jyPV~$63+G
zInl?9qK|KjK3)=id`I;0UD3z)L?17Ii#|G{k5@!Jl<4EU=wnIru`K$iMIRSL9~VU*
zUD3z)MIUWlY#%G|{~O(6rn_$6ZT8)3f6uU^z;pWs?z<xPZOiQ|u+Hx5g6tVa!+w7i
za<i4k$y+q_b58i_%K5QA?0({VQ}~Pgmav%YUA#ryZLm7-r{UId+p%8p+HaqK|GLi0
z>s)91op!&}*~UQj`r)7NfjmPr=pJN06aI0=&iB*(z1REt*Vy-O>U;7Qb_zK=BtN|W
z=dVwFv-?Iqj@Bmo+r|DK=Cfa9vwYU${Izr5WbdZ1cRRnoTOaf{&8V8!RiIy!v-ob6
zX9uj#vqR?Dm)YrC!;T%&&U)5=ht}G^aW?Xtx3{d`C!f(YT8JwYHu!!#(4Vh+Gdr7d
zc6x=KrMX|f{n0;m`p=)H_I&sIcm20>jyhp=|B09XKd||&$9d?4?7mzttbJc(p0{&f
z?)04B>AwCo_Srb)KH^1Pk8QC3<9F+SJ<bC?v+VQXPTmW4{Fl!O>9>V{(f#@!wIjd3
z+4<Q5tNTv8{a@mG^}h?U=fMqF`+4wh-=BAz-@UvSe*pjh|Nrb)Z)hAv6rcNRW7G2z
zy_gmar?%Q?iEU7n67KeL=4|B>kL0joBW7=Jn=J0_UUxTVk}Bzk(p1}Ou~O+D{9qAE
zl@_59>%Y4Xwp3#Ai&&I@Midk*Vl6`YK@gm|o$=<jo8H~tYCm`bkDWI=zc+8*ym|8;
z0ASt;0B~SxhU9|egR}vX7t#_)%YfcInD<f`fcbX4TPE)&NNy}ILCNzP<T)`Bc}<Xl
zkR~?$bmWA*?o)k~VgC4JaK&4f1`2puQ7%(KF0Ch$>xXpZx9OvQ)4%Kse;>H;#=`wa
z!j<GWO!V`EQd@K9nf*P7zs(~#Yp)}RG+GYXhgF;%=j6i}^nJ8~9gOxw<0Fsl4IIDY
z-4Mc$P<TfLym8-dlld-cz8`W^@>mh)ca(4*co?@(I1eUz{T>rLl5?f|E)$;CtDXAq
z!n)rK^!1TFCLmpZJeBAlnfpC_e*N6cu}(Sv#HF)62fsTM{`s@E=sD%1{N5k#TfWu%
zReoXIy>8}#h47zyUReIl+z@m5#hnj-_~0fcl2a$c+`Y`zjz`+J9gh`GihF*2?XQ!}
zH=S!vpWF3b!8gl?E?#-PaG)2Qie8v3%)a#U<xtz31!R|h?V#K)F3K*XPur#0U<b2(
z5<3uTqW;6%ls9~f_vvX`kJfTM+@RbKh#tl>q5a=g9CrbS!(p4>`|-bO{E)u?e#i;A
z>TvEdoExy?np_Xg%Kbca5Vg}`t9ZSI7_0bgx1D(W$Eh8b*FXOyYPw(5dQP%rJ+7g)
zeTOenpWXC-^;f$VkBi24%D7LXe;e2H>Xw*CxPr5UGu|U3&I56N%rlTOtR}K4MV2-F
zzshG>h1FyQ5MI&*i5JPfwv0=hCdkrrW2}@G`D@;Jv-9Zy^!ey~bp3Ebkw+Ca6O0KW
zuMYP|lJQ9I@L>1a?(S4|_smfy*8zr2>{tzs!MF*3M{Ra&!u(3gE<l^i^17wqwu&82
zWL1^e&zN_Z>E|=p3-Lla%W}_mWd$k6$+_y}EH%hkN|aHBOcpCw$zG(<atLn2b#Q2Y
zivibC+=rY+c2c{tvN*}B8YRz;_-KanfO0($uWixSz7hEwjcamVt1s<mPS_(pv7|=-
zB;)9R9gMS?RGI}~5dR;LGlIm5b;U`tAM5z);=A$Or+B5V_tcI&;#Q#7I(Erx#q*2t
z9MpEsBwt1t7-*&dsP|mz!n@7R^}nufTChyJ?rAOgAyWZo7T*VCEy!lGqQJ2!6q@an
zGe*fV<{`#&tR|k)a{QQw(NuuB1ds6hYRpG}$UBAVhnS!xc!h&m2Z+n68sj81tAD5u
z&j+!@Ki$a2DAna~S1JwnmL^#B=fL}ckN3~6vYZDmTs_d-J-^a&9z3)2)uiWZ>-Z-h
z>d1V)Yn5gEMCglqx^^EfSjE5nfwS}3SFGcYZas7NbmlJ0_}!a5XIDh@^^eu~ezszN
z{uxLKR(Xo&N_jDKM1|);S&OqNUR3E3GzTK1a*kC5R^oWDQ4%zP6{E707K-^IZZ{$i
zSq&DC3r5_(8Aq+Y?~VGrQJ**J^G1EXm>nDSd80mW)aU=t`uuiaZ++f|=Qrwed+Zqi
z_SWZC^*8g&0HFO+=lgJdn+}evf;1X`Ovd_#lAyYJ*(|=%c&5huU>W8&LmFT+r79#z
z-Zs?Yyfmtfkw4^t`x^iN|Nre-&2QX96rbH~0xA7iP!XZz;0sbOM6J{!C=x6fl3L1U
zSKSTGEt*}=#v^QxH6E`>km#j{9{5L)5aPzQ5*&I%^wbLnxN<2H+&C~Z9<$?r$u@~U
zss@yiM)rI5@4cCM^Pb;40|0;%GDHV3h<qf7C}F+HBt{H?Ucxj=v<tjn<$aO&7kNLE
zvcZC)8;aLZ{5Gdt$e_S=j=;5ZxU>&(fGn2;5QkPK=N(PX;v6{*P0o_y%l3eF%HqGc
z^zHhu`|N|`%)gbJM+@u|@4&i$nR>IP>lLjEZo4_dHIJC*0-wpNivE)KJKo-akqG3P
z!}kefn1r#$T*EYGJ?bLbix>`|kJ%nr8;3SYLY9MXMcjgDlh8n8Zors1K}5+E1!xA4
z;|GVY^P3>QUj*HSpW`42tS|{ih+1S~;eQ8hQ3S^xa#xl7()$QpFTIa{-uudK-TZ62
zG+wp`_e$gc{F(ju&AIPO<3GFGxe@ekyk8~`z=RQjyQSgI8@NS7ub`LH+h3a6vrD|6
z1y$|%MEdhXP|QzWP_)3xaG3?l%Hug1_&V$(_%DHK@$3JN>ub1+2F~;!VFyd2vwdpH
zKb8CJ<$A4|+*w?h`AfrpcwhI&dGQ70<eWLhpW;vP8s5|J=M22*Z-Rdvly5@c@+<!_
z|85k^U(PD}3hx_atTOsd7@+i>K4e~c_bH+=f2j0a0Z(6-Y>fK24$BXLh7qm(jB(~V
zwE5jNg?m_V{W8ALH-x*AKfwEFN30n16LAwcALmxk2`6Ny;`};);OTirLycX}Gcv5+
zo_Bs!#4cs^-3P_&xuj@uRrcBen!Kv#i^m1WljYbV)(z!Rd;6+QrZtSGzWn~8dRFHA
zl<{SKNAWX1sbjs%<8R7y@vT;|`Mjp+pLkyn`#7B1JfJeDl|P&iGYtLp5DtltC=ACk
z)^2nM3{ob82ADN}!A3^y1<2aQM>s}fqXNU(Ye^`%pG>(V70<gk>_f_fIz!a<V7QxV
zxQ%@N!W0=b7oqSrauX>=(^+Xf@BdJ`&hVDK^80UZUoLIWzrX(DQFqj@xF0~ZIt_TY
zBL2zUlTZ(+x_(9D-vvdl@SbxQvDJ+lWVL$ky^8xItpk34S1^BgS<zeI;P)miDCre>
zC+<_^V(FOjwL1v!dJKoVYuM+n?SKu$wnV%c`KYGvRvHJ(*7tPWQuA=VO@7w^_2m~u
z^tB7t$Fto3L*5;GW-=Z;AxpA|+(pbIZuSI0I`$MRC5C&E7VD3bSL^c@??>QpyL5nV
zmz1_Cf(*HT)@aXiG3DP7Yb?G|G!6I`p%&wA7EcHbq<5`an}k^?78QMtTYVn+ygpBJ
zT}=BN=l{}qkk=z<n*<{qBDYIi<b!(7V~-2w57~d0M?ks1IErc8D`jr+6ygALosuNd
zxM>~Qz^6<DpE3<Rw>gS+C6Z~3Mz#ZSOSFhi#orxU=;M94iL|X=SKhg-u64<KU6Xf|
z-A%ihcCTYRmusHi8z*djE82L<#)YciH61@MPAnLD1s!PdK|fzE&B1SL_@<&k!|Bdj
W$K%)`8;`hl?cN}hBicX2#O4=-2qF{!

delta 4394
zcmV+_5!LRFB(x)tG#FH9bs$Q0ZXhx=ATcvKGc!6cG$1lCGBJ@+BavVQk#u2y+YkT%
zc%0-}TXPge6h6BNge2sGh=i+L1o6s6Q1C_|Hx_d<*@WP2>Fjp01G_WhOiv`Jy!hxJ
z@P|n0BUP4F`~<5kAN&VA`eb>|%;}x8!;%2TqA0uSRDb7mpIe{qp3bHh0PbG}*o^%a
zq)tfMCgyWUcdsTrwLJ5XD@+i7--~oW=;vGLTT0~Z@mP-MObkuUT06fjq;|IP{9L1N
zkLMEKgEZ>Y^p!yQz79$y*8f51r~VTDyG$H3|5*RS^Zu)^d48T{ZP>36ZU?xBldqwH
z0aWhqOU7-RSAnAzYq|t;#;G}G73M-63@f9c=+yO`>zEQ0Tt70Cs*baNl)DO{2^@Dh
zktP$SBGZ_WU{;)(b#9@I?3G~F1X0;UfriPbt^;Mf045yQsYmq^OsJa)7!5RP%$5}e
zm8j3FT7;cn7zXmu2pW79sJc^uaj&e5<GCSBD^rDe9fZhXgiBrthCJ6eZsbK_G9QX+
z8HTm%)g3pEp)fV9YibpL%o6x@ucB+#046X57fP^j0<Dw)Lp6@7p}mSj-k=!b(2%zR
zGjY@~s>*0jS5QO|x@8Sc#aHDUP=ra(3H7`c7hI!*SylHj2{C&lG)_IvWXezSDI`8|
z878!;dKH*FeMLLVRb1T3)0e!!xkX{(E<<}%Fbpm<$~EzV7EoA!f}&T$#YXcbxDXkS
zEEcPQ4y#_R0>e(|Q|>sM)*42FzzfphamUpvnDuHa8rNxv7PDSgZY3^5yaA?`mO^c~
z>dbH&T4Xk(m!ldox8m%;4`$JBxtb(ZYVb+g7-(gM)G7@iheKRdFF|jHu^h(SX~$i$
z$=g*Mb5$dG7@wGbH7YRiUWys*Q`Q%;Eak*1a-)$OGb1ZFjkgqT(P`Q@8dr0us@!GD
zriOznx={B6Z$;NN`Xqxhx)R0u7$M5SWZlvjxF&2E<5`T)T)N)LGA0$q{is8e<_d*y
z8aJ0yPL?Q{ga<JxehJ1ko;u7wKFuRHO`z;@&1l~XOk?YRq&o*Uj)6vUA+Q2o0Tr0R
zGGl}W7fXd^%IC1@7%PKkiT5KN?Rb7s2l0WT>}7E=0n9F{nUKZBRbTFOHfN7ug|43X
zWMFDzfrFdFso=?sH``d?MZUdan3?p94oi?+J}p~^zynA-1-~C@pWqK89m?pxBJgpf
zHwAwgsgU7+i-P|I>8jvs8J_I<8Givh*yw)W%zb-?&-Pm~d{>6QmEph6@ZV(kZ!`Su
z7V+7(wME=}A^ko9$Zut{JTH1FZxg+gw~M|z<Cko_Am&J0-NL4{wOPbq+a{*${Omiz
zwhpH3e45+UVtyynri`5C_OKt>DE6%_<Rt5b^nGA|8`;>#{$-=N{m^_isP!>*W#lw>
zM+^I8`}r33pF`T-!v1z1lXbMY@65<;jsqFFwYe)Jw=wL_$ZgDfgsdALd^cI%?lY3L
z3z>Wm+B4R{_H*3Zv(_zqY-J*R6X{FFt30QN`PYz+Fh0pd_!JZ2x0ndOgLInlB+?g*
zr;tj2j17{@I70Fm)7$V>#$R!Mz0A|OIl!3ugN$G0y!)7^xceEC?l#6WZfi{A+Zo%v
zlH>6mj7etih#g!H8%K`yrQe;x?||^TOZeR_{O%Ec_X@xJh2Ixj_}!QBdr<g2B>Wy0
zeqR!Pj|jg<h2LYsul=@2zb|t=jxzr`%Lu=J!TN;hJCx^f;*aw<;S+4PO^lyrIpH&6
z?%N#O8RpNjjPN-~_8H-Kd5rLTJVy9^9wYn#j}iWm#|WS2F~Wm9M)(4c5x&S{gok*H
z@Gy@N9^o;<qdZ1<jK>IH;xWPn9wYn_(iO(zNDGW7gdX*mA#J^4U8a$)G5#1yF`f~B
zanm1(i;U?VP-Z-bRAEeS0L{3Lw8WUs=`v$Fr&Y%EK5!V*-Eo~Uy%TOQ4mn;~$9Yl5
zE254IqK;QZ9X}Cu{8ZHOGf~ISMIFBobu5WGUK8t~L>(7J9m}GQ6;Vem>bNB8xGd`E
zh&o;ub+l_?>sW>VU+Wb;opt+8vENdE`|YwV$9?BBsCP~1^=0&OkiJ)`^+0;}QM2!`
zT&B13DE|5;el7_=JsCgNht(&3Ac23#H-*JyZ(*IdH$ijU$6)=qZQH1L?HlO7Ugy;f
zUT6DWdsf!j#z6K4;o;{%?jdS)4*DC>+&^#oXX*StsovlkdXKYCXV<Zl%h(}*`R{ks
zxqWvI4QAtLZ=v5Q^!u4lfB8)F*@%7A$$68#d&1tM{LXEC(BCNIYEjpKe!WfOTQAQp
zXwI`s=6NEub7BoUwn;l1xjr=4{*CiA_jzZ&Sx?@z)bhmTa!>huY#Ge1dn-F_89M{Q
z&f&kmfBvVVf1aQFwf**MKmPK6Nav*+n(I%z{GHzBw-NiH8`AUgb8hYPBJ+Hd^YWzo
z{7Ll&*U+<ZN<HGmx}JQ>{!jf|{~NI%=+3hD!;^d$Z2K?o3F$Y_=vn=a+LqOCwSPLG
zx$eZ<|JiL+{d*w25AMR+_rc?SKmTohPxD+*PHh`#uY+?Sd(-gneUpcO=Kc2%00030
z|Lj+9XdFcppZk~gtc{mT+k{fvNNucGLtFgN)<4-a<eES(J#%M_HE1S#o7|GS-F0^p
z+6dB0J_xN+zX**Kq*{zn+7u01s@{Q$X!VQuL6L$jgo1ytMEz7zXLe`2x#=c%w|CkP
z-h;=^+nwK=d2imnc`pcm0BAwH`Ol+k-fg{-1r`kpkA32Q?!I^8IY6i$v4FS^@k;W<
z6d<w}v4=3GAmR{Wd1heKqu07~Z$z5jZmf$6#q@X>U928T-_66_cg}y+j`eI`Mh_)c
z54FQBuA9NF=5TeK{a9|chnHF3)DsMT?keI3-k<pWi_b59ypSA!w(K8f@SY-g-fq9c
zd>=DEgrq4oW#K}eJT8C^;|>NFAk41+kb@uTxw2!QgUs%$nfdM`d%q3X>tlZ$L>#^_
zeD#}i(cD7lm8a%bMB5+U{Q3HAUti58Hk>&1!<NR}N4sAbKKjI^?Chtn&iuToQ7}3_
zTlcz_YoF;#9orCpznJy@HvUUU`Y`+bkFULV=FA7}tw$CI8=u*d6Xw^aUY@%*cf)t;
zz{v+&g{h6l-yfIn5t@$-o(x|LS#f+A6#B)-_=S#HzueCG!O2eQ2g*U_I{??S+|!DK
zL3SVE!hQHbVI0tX@Mprtzt4K_10Ij3YH=ST-%5!?_WuumAxWMr4d*Yw`2n0T)k<(~
z{?C~cm@c%t#oP3V;1>VvnzQ?^OijC9|I05hUB4;6mU2$2;y$loIy_z9QJLTD^|$(#
z&Wrp##oNipH@{w;?TO16m-FUv{C~vb0tgo(vVb@P%`_a*Rn@e=8<GZfXsSA(vSP}L
zl%{r#PAH;(DXYrUDX1jVlC|#`SpIxA0``1txpU7@OjpNrm}!Z~X~`Ihb|iW`_73&$
z+`e-MEbTw@6zKJUAqPK}f)fbtpr<@P28nz(;}>8a(j?nc$Si9hrlDa_|M7K))Bfr?
ze_>fpYET?+(`02*R3}T*vx?KRif+b`GaA`kF@I5iV)am5_cAy<zk$OwSazry_LI@3
zsp&(KVKVxvksqCK0Z_OPESsbD+P9*9V{t+6&HH8J%!@`eCRUaipY%Px2YX>=B#Hj3
ze)7Ff&BzK&SCl8IJ?`<P<#+zvrzE|i`?MN$L?hxApnCOqn9R5GIVkU(X}ydwDA7y-
zQ0cjURD}0Co$G(y-!u@N)%s7%sSkxBIH&yHr<yHL)6%jCM{sJcrk)g|hp$8Ub1WsF
zvU);9M%h$GwS<K6$IGlo=skt|hlp&(Bwa*R2k2D|BO)qzRy5v4=7UJ=pJ7K&Co>eu
zP|3gEo1fq|p5J=>blv@b4Y;m@>t~H^V?7IhuIu2oH!H(mI;KqfQ||G<@3`yquQ%pg
z+Yfdb`~Ii^A79M<)OPXPtXup`XCK%jHl4TDpDpwL3={bP;uzEiBr#tXqd?Rp2^3H-
z9Ffun`v%WJ$WC<<>M~SB3G^tkDZ_N9sw8ErUL<rY>X5Bq>Ab+>{>?n<_I|JR=e7QS
zyw;!B`tzmySnJPg{duiF|9|%9O`y8{`8qPc)}L3$pKC#N`*XMZcZ$okfQ?IA?jrqd
zvSq@Mm9dsYq${2PrS;2B`HjVQmsuaICi0Vr`(P&Dg(T?RhN)Ll#>^D`!nP$niYe)F
zXx}kCiZ)^HA00I$ly3J=n2bdIRm?trdgjzVEGMA}<Nc-GgBbHS00030|Ls`KZyYrg
z_imGxEu`H_MU;}moX`eARHTRx3DnSqY`4lLtL~-^r<Jqg?ikJ3BadexK`I<L^v;pL
z03q=gAT9{07gPx@TsZJ?Lr6v9gg9V(JgnE6rkRhbIq)QF=DqRnJ^MZT`FU!8Afcvi
zLmVK-r2)jmaUq9igzPoZPKx$~eo9)@e9dT*8d|RCx6js#;{U$=-dFrn@3O>SGVlu-
z_!9=cDYygb0UKxAP06$?sSIwnKEu@?GS5XZlczO*TeO?r{(#dE%u$yk205lAGK3qL
zMSPpNh;4@)2hhiSA8d?Co5qBH&%t*>5ka&?Szrh^VZ^&Z$mkRWXa-Q@2Z#Ild0_fE
z;kOWEPRplb=&v{dCOC+L7o8RnMq85==M9Hbe`Ph%Uw9pX7YeVVap%qWi*NtBT+05(
zR|?yIf9=}W-wk#O+y7&F@7H&KxOSv{umH^XEC3jDDsfj0+>(VmY1x&3{DtK8r@Ha&
zylAIE**M-;^SBH0?UPfQm)I@|W3gzrW3HYh$8$XJRm4Zip9JOdSKq*$&6tO|zT`L^
zgHPjV>;GTxqu1*hdY`iNE&+Q#0k8&&lUiTrX7)_IS@!Jxk+t8!oSud1@wI&U=CtNp
zqHT(kEjfX=V1SZ7eaO9k<n0Dxk@%?Xw7`S?l8(_F*J1e_7#K;-M~t)m&=hYkYuw$O
z{!8W3-Vjcde1H$oNO}zVv3v=;Z_jzKOK#Co$@#5=!}D<6z%>l4Nsps$EE<=h_U?du
z@oZ>bK6SiCz|dE=9{4fmc+xXb=DMLOjdw4#>9m6JRCgYq)!&ML)H|tM-N&>%-6vJ}
zyEy(;RR>=^k#9cFX#SpP9n!~S+7=0wL*u&AC75IAcL*F(UtD^~SmoNy-he~KRnh=6
zl44}tl?3(R2Ul=}#@3BVh&N)Qr0-0HCY{fFnDikNNnKpB+8!j6#KLvtiw;v{-I1ih
z>&T6j8pCEOf8P0jrtrPurS|ggzr1p>a6JF{{Lk-ud;OB@0Tdp8La&#^-~Z@1)bC~e
zmpA{N(fqP#bK6BOb?cu&(eZtF{>#qK2cMK&A8TrUUe5aAoaSYPV4pXj8}%7-vD$}9
zdK2M^$1#~~U|-CA1U3=hk?Cd6M>ALzdCxp=8}-BSb@5hz1J%_9dF(ZF{)b9y_P3;a
z+cTeuqz8GNX3`$w9(B_%2(q!KMF}(94~-mslzP~lx9lGp{j=(zb)$b%^KFJ8N3OHC
z@3@$W-0zI#*}{@U?NH`$u|1+&lm%*6tU6niJ1CNW&IE4tan#}FJZ-<&@v-aLEWa#)
zc{`GC(_jyO6Xf=&i+oVgJu2t9pN=aA`oE35LXA>pM;!V8F&#$ZaFqVn(ftxe_e&Vv
zPs9*K=|WAlD$1%)tElHL=x0^>o;CD6EH1WR?YLAio{JUF@3~_(zhz@QMfYvl^TWJ%
kH8SwK2EML&Q0w-x{BRUME^1yrd+qH(sz=hllq#74Z3ck*7XSbN

diff --git a/src/LinkShield.h b/src/LinkShield.h
index e75e02c1..2d51846d 100644
--- a/src/LinkShield.h
+++ b/src/LinkShield.h
@@ -29,14 +29,12 @@ Last update: 20.5.2020.
 
 //Defining pins used by LinkShield
 #define LINK_RPIN 0             //Potentiometer pin
-#define LINK_FLEX1_PIN 1          //1st FlexSensor pin
-#define LINK_FLEX2_PIN 2          //2nd FlexSensor pin
-#define DC_PWM_PIN 5            //DC motor PWM control pin
-#define DC_DIR_PIN 6       //DC motor direction pin
-#define SERVO_POT_PIN 5
+#define LINK_FLEX_PIN 1          //1st FlexSensor pin
+#define DC_FWD_PIN 5            //DC motor PWM control pin
+#define DC_REV_PIN 6       //DC motor direction pin
+#define SERVO_POT_PIN 2
 
 #define ENCODER 0
-
 #define ENCODER_RESOLUTION 1800
 
 #define I2C_CONNECTED 0
@@ -73,6 +71,8 @@ Last update: 20.5.2020.
 
 class LinkClass {                         //Creating class
 public:
+
+	#include "getKalmanEstimate.inl"
 	void  	begin(void);                  //initialization function
 	void  	calibrate();                  //Calibration function
 	void  	actuatorWritePwm(int);
@@ -90,12 +90,9 @@ class LinkClass {                         //Creating class
 
 	volatile long int count = 0;
 	
-	
 	float 	_voltageToPwm;
 	float 	_voltageToPwmNew;
 
-	#include "getKalmanEstimate.inl" 
-
 private:
 
 	int   	_referenceRead;
@@ -151,8 +148,8 @@ void LinkClass::begin() {
 #endif
 
 	// define DC controller pins
-	pinMode(DC_PWM_PIN, OUTPUT);
-	pinMode(DC_DIR_PIN, OUTPUT);
+	pinMode(DC_FWD_PIN, OUTPUT);
+	pinMode(DC_REV_PIN, OUTPUT);
 
 #if ENCODER == 1
 	// define interrupt pins
@@ -204,31 +201,30 @@ void LinkClass::calibrate() {
 
 float LinkClass::flexBias(int testLength) {
 
-	for (int i = 0; i < testLength; i++ ) {                 //Make N measurements
-		_flexSum  = _flexSum + analogRead(LINK_FLEX1_PIN);
+	for (int i = 0; i < testLength; i++ ) {              //Make N measurements
+		_flexSum  = _flexSum + flexRead();
 	}
-	return _flexSum / testLength;                         //Compute average
+	return _flexSum / testLength;                        //Compute average
 }
 
 //values from potentiometer in degrees , for fututre use
 float LinkClass::referenceRead() {
 	_referenceRead = analogRead(LINK_RPIN);
-	_referenceValue = AutomationShield.mapFloat((float)_referenceRead, 0.00, 1023.00, -100.00, 100.00);
+	_referenceValue = AutomationShield.mapFloat((float)_referenceRead, 0.00, 1023.00, -90.00, 90.00)*DEG_TO_RAD;
 	return _referenceValue;
 }
 
 float LinkClass::flexRead() {
-	_flexRead = analogRead(LINK_FLEX1_PIN);
-	_flexValue = AutomationShield.mapFloat((float)_flexRead, 500.00, 550.00, 0.00, 2.50)*DEG_TO_RAD;
+	_flexRead = analogRead(LINK_FLEX_PIN);
+	_flexValue = AutomationShield.mapFloat((float)_flexRead, 500.00, 550.00, 0.00, 2.50)*DEG_TO_RAD/10;
 	if(calibrated)
 	{
 		_flexValue =_flexValue - _zeroBendValue;
 	}
-
-	return -_flexValue;
+	return _flexValue;
 }
 
-#if ENCODER == 1
+#if ENCODER
 float LinkClass::encoderRead() {
 	_encoderAngle = float(LinkShield.count) / (ENCODER_RESOLUTION/360)*DEG_TO_RAD;
 	return _encoderAngle;
@@ -245,50 +241,48 @@ float LinkClass::servoPotRead() {
 void LinkClass::actuatorWritePwm(int _pwmValue) {
 	if (_pwmValue < 0)
 	{
-		digitalWrite(DC_DIR_PIN, LOW);
-		analogWrite(DC_PWM_PIN, _pwmValue);
+		digitalWrite(DC_REV_PIN, LOW);
+		analogWrite(DC_FWD_PIN, _pwmValue);
 	}
 	else
 	{
-		digitalWrite(DC_DIR_PIN, HIGH);
-		analogWrite(DC_PWM_PIN, _pwmValue);
+		digitalWrite(DC_REV_PIN, HIGH);
+		analogWrite(DC_FWD_PIN, _pwmValue);
 	}
 }
 
 void LinkClass::actuatorWritePercent(float _percentValue) {   //Turn DC to desired angle
 	if (_percentValue < 0)
 	{
-		digitalWrite(DC_DIR_PIN, LOW);
-		analogWrite(DC_PWM_PIN, AutomationShield.percToPwm(-_percentValue));
+		digitalWrite(DC_REV_PIN, LOW);
+		analogWrite(DC_FWD_PIN, AutomationShield.percToPwm(-_percentValue));
 	}
 	else
 	{
-		digitalWrite(DC_DIR_PIN, HIGH);
-		analogWrite(DC_PWM_PIN, AutomationShield.percToPwm(_percentValue));
+		digitalWrite(DC_REV_PIN, HIGH);
+		analogWrite(DC_FWD_PIN, AutomationShield.percToPwm(_percentValue));
 	}
 }
 
 void LinkClass::actuatorWriteNew(float _voltageValueNew) {
 	_voltageToPwmNew = sq(_voltageValueNew) * 255 / sq(5);
-
-
-	if (_voltageValueNew < 0)
+	
+	
+	if (_voltageValueNew > 0)
 	{
-		analogWrite(DC_DIR_PIN, (int)_voltageToPwmNew);
-		analogWrite(DC_PWM_PIN, 0);
+		analogWrite(DC_REV_PIN, (int)_voltageToPwmNew);
+		analogWrite(DC_FWD_PIN, 0);
 	}
-	else if(_voltageValueNew > 0)
+	else if(_voltageValueNew < 0)
 	{
-		analogWrite(DC_PWM_PIN, (int)_voltageToPwmNew);
-		analogWrite(DC_DIR_PIN, 0);
+		analogWrite(DC_FWD_PIN, (int)_voltageToPwmNew);
+		analogWrite(DC_REV_PIN, 0);
 	}
 	else
 	{
-		analogWrite(DC_PWM_PIN, 255);
-		analogWrite(DC_DIR_PIN, 255);
+		analogWrite(DC_FWD_PIN, 0);
+		analogWrite(DC_REV_PIN, 0);
 	}
-
-
 }
 
 
@@ -298,13 +292,13 @@ void LinkClass::actuatorWrite(float _voltageValue) {
 
 	if (_voltageValue < 0)
 	{
-		digitalWrite(DC_DIR_PIN, LOW);
-		analogWrite(DC_PWM_PIN, (int)_voltageToPwm);
+		digitalWrite(DC_REV_PIN, LOW);
+		analogWrite(DC_FWD_PIN, (int)_voltageToPwm);
 	}
 	else
 	{
-		digitalWrite(DC_DIR_PIN, HIGH);
-		analogWrite(DC_PWM_PIN, (int)_voltageToPwm);
+		digitalWrite(DC_REV_PIN, HIGH);
+		analogWrite(DC_FWD_PIN, (int)_voltageToPwm);
 	}
 }