diff --git a/implementation/SCARA/06_arm/arm_model-v2.emx b/implementation/SCARA/06_arm/arm_model-v2.emx
index f6f2826..e1de3ac 100644
--- a/implementation/SCARA/06_arm/arm_model-v2.emx
+++ b/implementation/SCARA/06_arm/arm_model-v2.emx
@@ -8,7 +8,7 @@
 <IsMainModel>0</IsMainModel>
   <KeepParameterValues>False</KeepParameterValues>
     <LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\06_arm\arm_model-v2.emx</LibraryPath>
-  <TimeStamp>2020-7-23 16:01:17</TimeStamp>
+  <TimeStamp>2020-7-23 16:28:58</TimeStamp>
 </Description>
 </Information>';
  type 'Mainmodel-v2'
@@ -8538,7 +8538,7 @@ equations
      <XAxes>
       <Axis>
        <Minimum>0.0</Minimum>
-       <Maximum>6.0</Maximum>
+       <Maximum>3.2420091467026997</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>time</Label>
@@ -9248,36 +9248,36 @@ equations
       <Name>Camera Looking at Origin</Name>
       <Position>
        <X>
-        <Value>0.211790609540361</Value>
+        <Value>0.245976429808248</Value>
        </X>
        <Y>
-        <Value>-0.107655952853072</Value>
+        <Value>-0.0388302746466069</Value>
        </Y>
        <Z>
-        <Value>0.123737714793038</Value>
+        <Value>0.0780771451714441</Value>
        </Z>
       </Position>
       <Orientation>
        <ZAxis>
         <X>
-         <Value>-0.840897145443893</Value>
+         <Value>-0.976228248439824</Value>
         </X>
         <Y>
-         <Value>0.426188883453037</Value>
+         <Value>0.15372928256777</Value>
         </Y>
         <Z>
-         <Value>-0.333549136418257</Value>
+         <Value>-0.152793045094702</Value>
         </Z>
        </ZAxis>
        <YAxis>
         <X>
-         <Value>-0.297518598807634</Value>
+         <Value>-0.150933112017381</Value>
         </X>
         <Y>
-         <Value>0.150790284066671</Value>
+         <Value>0.0237678422676611</Value>
         </Y>
         <Z>
-         <Value>0.942732715882203</Value>
+         <Value>0.988258207843825</Value>
         </Z>
        </YAxis>
        <OrientationType>Direct3D</OrientationType>
@@ -9576,11 +9576,11 @@ equations
        </X>
        <Y>
         <VariableName>Body1\position[2]</VariableName>
-        <Value>-0.017071802542194314</Value>
+        <Value>-0.013216742557477136</Value>
        </Y>
        <Z>
         <VariableName>Body1\position[3]</VariableName>
-        <Value>0.0265923779880002</Value>
+        <Value>0.02929685427745184</Value>
        </Z>
       </Position>
       <Orientation>
@@ -9591,11 +9591,11 @@ equations
         </X>
         <Y>
          <VariableName>Body1\R[2,3]</VariableName>
-         <Value>-0.5021736062379976</Value>
+         <Value>-0.39150905335206887</Value>
         </Y>
         <Z>
          <VariableName>Body1\R[3,3]</VariableName>
-         <Value>0.8647668293811487</Value>
+         <Value>0.9201742558577516</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -9605,11 +9605,11 @@ equations
         </X>
         <Y>
          <VariableName>Body1\R[2,2]</VariableName>
-         <Value>0.8647668293811487</Value>
+         <Value>0.9201742558577516</Value>
         </Y>
         <Z>
          <VariableName>Body1\R[3,2]</VariableName>
-         <Value>0.5021736062379976</Value>
+         <Value>0.39150905335206887</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -9695,11 +9695,11 @@ equations
        </X>
        <Y>
         <VariableName>Body2\position[2]</VariableName>
-        <Value>-0.010639024121723569</Value>
+        <Value>-0.010731870933632882</Value>
        </Y>
        <Z>
         <VariableName>Body2\position[3]</VariableName>
-        <Value>0.006594217889494994</Value>
+        <Value>-0.006377522947254681</Value>
        </Z>
       </Position>
       <Orientation>
@@ -9710,11 +9710,11 @@ equations
         </X>
         <Y>
          <VariableName>Body2\R[2,3]</VariableName>
-         <Value>0.5249456675421719</Value>
+         <Value>-0.5128634791918395</Value>
         </Y>
         <Z>
          <VariableName>Body2\R[3,3]</VariableName>
-         <Value>0.8511357389563098</Value>
+         <Value>0.8584701810262496</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -9724,11 +9724,11 @@ equations
         </X>
         <Y>
          <VariableName>Body2\R[2,2]</VariableName>
-         <Value>0.8511357389563098</Value>
+         <Value>0.8584701810262496</Value>
         </Y>
         <Z>
          <VariableName>Body2\R[3,2]</VariableName>
-         <Value>-0.5249456675421719</Value>
+         <Value>0.5128634791918395</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -9814,11 +9814,11 @@ equations
        </X>
        <Y>
         <VariableName>Body3\position[2]</VariableName>
-        <Value>-0.03682020445746276</Value>
+        <Value>-0.03440839742237934</Value>
        </Y>
        <Z>
         <VariableName>Body3\position[3]</VariableName>
-        <Value>0.041384334395577975</Value>
+        <Value>0.016889663750438287</Value>
        </Z>
       </Position>
       <Orientation>
@@ -9829,11 +9829,11 @@ equations
         </X>
         <Y>
          <VariableName>Body3\R[2,3]</VariableName>
-         <Value>-0.48276125099189626</Value>
+         <Value>-0.4003112042408226</Value>
         </Y>
         <Z>
          <VariableName>Body3\R[3,3]</VariableName>
-         <Value>0.875752005159417</Value>
+         <Value>0.9163792554173531</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -9843,11 +9843,11 @@ equations
         </X>
         <Y>
          <VariableName>Body3\R[2,2]</VariableName>
-         <Value>0.875752005159417</Value>
+         <Value>0.9163792554173531</Value>
         </Y>
         <Z>
          <VariableName>Body3\R[3,2]</VariableName>
-         <Value>0.48276125099189626</Value>
+         <Value>0.4003112042408226</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -9933,11 +9933,11 @@ equations
        </X>
        <Y>
         <VariableName>Body4\position[2]</VariableName>
-        <Value>-0.033109209191929785</Value>
+        <Value>-0.02583103540960315</Value>
        </Y>
        <Z>
         <VariableName>Body4\position[3]</VariableName>
-        <Value>0.0542093131266403</Value>
+        <Value>0.05894321641561122</Value>
        </Z>
       </Position>
       <Orientation>
@@ -9948,11 +9948,11 @@ equations
         </X>
         <Y>
          <VariableName>Body4\R[2,3]</VariableName>
-         <Value>0.6245890404745712</Value>
+         <Value>-0.49847598509542</Value>
         </Y>
         <Z>
          <VariableName>Body4\R[3,3]</VariableName>
-         <Value>0.7809536033075554</Value>
+         <Value>0.8669035080579329</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -9962,11 +9962,11 @@ equations
         </X>
         <Y>
          <VariableName>Body4\R[2,2]</VariableName>
-         <Value>0.7809536033075554</Value>
+         <Value>0.8669035080579329</Value>
         </Y>
         <Z>
          <VariableName>Body4\R[3,2]</VariableName>
-         <Value>-0.6245890404745712</Value>
+         <Value>0.49847598509542</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -10052,11 +10052,11 @@ equations
        </X>
        <Y>
         <VariableName>Body5\position[2]</VariableName>
-        <Value>-0.004186627780447375</Value>
+        <Value>0.0064755680322661725</Value>
        </Y>
        <Z>
         <VariableName>Body5\position[3]</VariableName>
-        <Value>0.0310775428306494</Value>
+        <Value>0.07751973284114312</Value>
        </Z>
       </Position>
       <Orientation>
@@ -10067,11 +10067,11 @@ equations
         </X>
         <Y>
          <VariableName>Body5\R[2,3]</VariableName>
-         <Value>0.6245890426846639</Value>
+         <Value>-0.4984759842502531</Value>
         </Y>
         <Z>
          <VariableName>Body5\R[3,3]</VariableName>
-         <Value>0.7809536015399732</Value>
+         <Value>0.8669035085439103</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -10081,11 +10081,11 @@ equations
         </X>
         <Y>
          <VariableName>Body5\R[2,2]</VariableName>
-         <Value>0.7809536015399732</Value>
+         <Value>0.8669035085439103</Value>
         </Y>
         <Z>
          <VariableName>Body5\R[3,2]</VariableName>
-         <Value>-0.6245890426846639</Value>
+         <Value>0.4984759842502531</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -10120,11 +10120,11 @@ equations
        </X>
        <Y>
         <VariableName>Body6\position[2]</VariableName>
-        <Value>0.0054542326865783565</Value>
+        <Value>0.017244532392109463</Value>
        </Y>
        <Z>
         <VariableName>Body6\position[3]</VariableName>
-        <Value>0.02336700049128123</Value>
+        <Value>0.08371196768126417</Value>
        </Z>
       </Position>
       <Orientation>
@@ -10135,11 +10135,11 @@ equations
         </X>
         <Y>
          <VariableName>Body6\R[2,3]</VariableName>
-         <Value>0.6245890426846639</Value>
+         <Value>-0.4984759842502531</Value>
         </Y>
         <Z>
          <VariableName>Body6\R[3,3]</VariableName>
-         <Value>0.7809536015399732</Value>
+         <Value>0.8669035085439103</Value>
         </Z>
        </ZAxis>
        <YAxis>
@@ -10149,11 +10149,11 @@ equations
         </X>
         <Y>
          <VariableName>Body6\R[2,2]</VariableName>
-         <Value>0.7809536015399732</Value>
+         <Value>0.8669035085439103</Value>
         </Y>
         <Z>
          <VariableName>Body6\R[3,2]</VariableName>
-         <Value>-0.6245890426846639</Value>
+         <Value>0.4984759842502531</Value>
         </Z>
        </YAxis>
        <OrientationType>Matrix</OrientationType>
@@ -10165,17 +10165,7 @@ equations
       <LockYYaw>false</LockYYaw>
       <LockZRoll>false</LockZRoll>
       <Childs>
-       <Cylinder>
-        <CircleElements>16</CircleElements>
-        <LowerRadius>0.0073</LowerRadius>
-        <UpperRadius>0.0073</UpperRadius>
-        <Height>0.135</Height>
-        <Closed>true</Closed>
-        <LowerClosed>true</LowerClosed>
-        <UpperClosed>true</UpperClosed>
-        <CenterInOrigin>true</CenterInOrigin>
-        <ShowInnerSide>false</ShowInnerSide>
-        <ShowOuterSide>true</ShowOuterSide>
+       <Frame>
         <ScaleValue>
          <X>
      <Value>1</Value>
@@ -10187,52 +10177,8 @@ equations
      <Value>1</Value>
          </Z>
         </ScaleValue>
-        <OverrideColor>true</OverrideColor>
-        <MeshColor>
-         <X>
-     <Value>1</Value>
-         </X>
-         <Y>
-     <Value>1</Value>
-         </Y>
-         <Z>
-     <Value>1</Value>
-         </Z>
-        </MeshColor>
-        <Alpha>1.0</Alpha>
-        <MeshColorMultipliers>
-         <X>
-     <Value>1</Value>
-         </X>
-         <Y>
-     <Value>1</Value>
-         </Y>
-         <Z>
-     <Value>1</Value>
-         </Z>
-        </MeshColorMultipliers>
-        <MeshColorOffsets>
-        </MeshColorOffsets>
-        <Direct3DColorRange>true</Direct3DColorRange>
-        <SpecularColor>
-         <X>
-     <Value>0.5</Value>
-         </X>
-         <Y>
-     <Value>0.5</Value>
-         </Y>
-         <Z>
-     <Value>0.5</Value>
-         </Z>
-        </SpecularColor>
-        <EmissiveColor>
-        </EmissiveColor>
-        <ShininessPower>
-         <Value>15</Value>
-        </ShininessPower>
-        <OverrideQuality>false</OverrideQuality>
         <ShowReferenceFrame>false</ShowReferenceFrame>
-        <Name>Cylinder</Name>
+        <Name>Reference Frame</Name>
         <Position>
         </Position>
         <Orientation>
@@ -10250,8 +10196,269 @@ equations
         <LockYYaw>false</LockYYaw>
         <LockZRoll>false</LockZRoll>
         <Childs>
+         <Cylinder>
+     <CircleElements>16</CircleElements>
+     <LowerRadius>0.0073</LowerRadius>
+     <UpperRadius>0.0073</UpperRadius>
+     <Height>0.1</Height>
+     <Closed>true</Closed>
+     <LowerClosed>true</LowerClosed>
+     <UpperClosed>true</UpperClosed>
+     <CenterInOrigin>true</CenterInOrigin>
+     <ShowInnerSide>false</ShowInnerSide>
+     <ShowOuterSide>true</ShowOuterSide>
+     <ScaleValue>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </ScaleValue>
+     <OverrideColor>true</OverrideColor>
+     <MeshColor>
+      <X>
+       <Value>0.752941176470588</Value>
+      </X>
+      <Y>
+       <Value>0.752941176470588</Value>
+      </Y>
+      <Z>
+       <Value>0.752941176470588</Value>
+      </Z>
+     </MeshColor>
+     <Alpha>1.0</Alpha>
+     <MeshColorMultipliers>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </MeshColorMultipliers>
+     <MeshColorOffsets>
+     </MeshColorOffsets>
+     <Direct3DColorRange>true</Direct3DColorRange>
+     <SpecularColor>
+      <X>
+       <Value>0.5</Value>
+      </X>
+      <Y>
+       <Value>0.5</Value>
+      </Y>
+      <Z>
+       <Value>0.5</Value>
+      </Z>
+     </SpecularColor>
+     <EmissiveColor>
+     </EmissiveColor>
+     <ShininessPower>
+      <Value>15</Value>
+     </ShininessPower>
+     <OverrideQuality>false</OverrideQuality>
+     <ShowReferenceFrame>false</ShowReferenceFrame>
+     <Name>Cylinder</Name>
+     <Position>
+      <Z>
+       <Value>0.015</Value>
+      </Z>
+     </Position>
+     <Orientation>
+      <Angles>
+      </Angles>
+      <OrientationType>Bryant</OrientationType>
+     </Orientation>
+     <LockX>false</LockX>
+     <LockY>false</LockY>
+     <LockZ>false</LockZ>
+     <LockXPitch>false</LockXPitch>
+     <LockYYaw>false</LockYYaw>
+     <LockZRoll>false</LockZRoll>
+     <Childs>
+     </Childs>
+         </Cylinder>
+         <Cylinder>
+     <CircleElements>16</CircleElements>
+     <LowerRadius>0.007</LowerRadius>
+     <UpperRadius>0.007</UpperRadius>
+     <Height>0.11</Height>
+     <Closed>true</Closed>
+     <LowerClosed>true</LowerClosed>
+     <UpperClosed>true</UpperClosed>
+     <CenterInOrigin>true</CenterInOrigin>
+     <ShowInnerSide>false</ShowInnerSide>
+     <ShowOuterSide>true</ShowOuterSide>
+     <ScaleValue>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </ScaleValue>
+     <OverrideColor>true</OverrideColor>
+     <MeshColor>
+      <X>
+       <Value>0</Value>
+      </X>
+      <Y>
+       <Value>0</Value>
+      </Y>
+      <Z>
+       <Value>0.627450980392157</Value>
+      </Z>
+     </MeshColor>
+     <Alpha>1.0</Alpha>
+     <MeshColorMultipliers>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </MeshColorMultipliers>
+     <MeshColorOffsets>
+     </MeshColorOffsets>
+     <Direct3DColorRange>true</Direct3DColorRange>
+     <SpecularColor>
+      <X>
+       <Value>0.5</Value>
+      </X>
+      <Y>
+       <Value>0.5</Value>
+      </Y>
+      <Z>
+       <Value>0.5</Value>
+      </Z>
+     </SpecularColor>
+     <EmissiveColor>
+     </EmissiveColor>
+     <ShininessPower>
+      <Value>15</Value>
+     </ShininessPower>
+     <OverrideQuality>false</OverrideQuality>
+     <ShowReferenceFrame>false</ShowReferenceFrame>
+     <Name>Reference Frame</Name>
+     <Position>
+      <Z>
+       <Value>0.013</Value>
+      </Z>
+     </Position>
+     <Orientation>
+      <Angles>
+      </Angles>
+      <OrientationType>Bryant</OrientationType>
+     </Orientation>
+     <LockX>false</LockX>
+     <LockY>false</LockY>
+     <LockZ>false</LockZ>
+     <LockXPitch>false</LockXPitch>
+     <LockYYaw>false</LockYYaw>
+     <LockZRoll>false</LockZRoll>
+     <Childs>
+     </Childs>
+         </Cylinder>
+         <Cylinder>
+     <CircleElements>16</CircleElements>
+     <LowerRadius>0.001</LowerRadius>
+     <UpperRadius>0.007</UpperRadius>
+     <Height>0.02</Height>
+     <Closed>true</Closed>
+     <LowerClosed>true</LowerClosed>
+     <UpperClosed>true</UpperClosed>
+     <CenterInOrigin>true</CenterInOrigin>
+     <ShowInnerSide>false</ShowInnerSide>
+     <ShowOuterSide>true</ShowOuterSide>
+     <ScaleValue>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </ScaleValue>
+     <OverrideColor>true</OverrideColor>
+     <MeshColor>
+      <X>
+       <Value>0</Value>
+      </X>
+      <Y>
+       <Value>0</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </MeshColor>
+     <Alpha>1.0</Alpha>
+     <MeshColorMultipliers>
+      <X>
+       <Value>1</Value>
+      </X>
+      <Y>
+       <Value>1</Value>
+      </Y>
+      <Z>
+       <Value>1</Value>
+      </Z>
+     </MeshColorMultipliers>
+     <MeshColorOffsets>
+     </MeshColorOffsets>
+     <Direct3DColorRange>true</Direct3DColorRange>
+     <SpecularColor>
+      <X>
+       <Value>0.5</Value>
+      </X>
+      <Y>
+       <Value>0.5</Value>
+      </Y>
+      <Z>
+       <Value>0.5</Value>
+      </Z>
+     </SpecularColor>
+     <EmissiveColor>
+     </EmissiveColor>
+     <ShininessPower>
+      <Value>15</Value>
+     </ShininessPower>
+     <OverrideQuality>false</OverrideQuality>
+     <ShowReferenceFrame>false</ShowReferenceFrame>
+     <Name>Cylinder</Name>
+     <Position>
+      <Z>
+       <Value>-0.05</Value>
+      </Z>
+     </Position>
+     <Orientation>
+      <Angles>
+      </Angles>
+      <OrientationType>Bryant</OrientationType>
+     </Orientation>
+     <LockX>false</LockX>
+     <LockY>false</LockY>
+     <LockZ>false</LockZ>
+     <LockXPitch>false</LockXPitch>
+     <LockYYaw>false</LockYYaw>
+     <LockZRoll>false</LockZRoll>
+     <Childs>
+     </Childs>
+         </Cylinder>
         </Childs>
-       </Cylinder>
+       </Frame>
       </Childs>
      </Frame>
          </Childs>
@@ -10331,7 +10538,7 @@ equations
      <XAxes>
       <Axis>
        <Minimum>0.0</Minimum>
-       <Maximum>6.0</Maximum>
+       <Maximum>3.2420091467026997</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>time</Label>
@@ -10773,7 +10980,7 @@ equations
      <XAxes>
       <Axis>
        <Minimum>0.0</Minimum>
-       <Maximum>6.0</Maximum>
+       <Maximum>3.2420091467026997</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>time</Label>
@@ -10944,7 +11151,7 @@ equations
      <XAxes>
       <Axis>
        <Minimum>0.0</Minimum>
-       <Maximum>6.0</Maximum>
+       <Maximum>3.2420091467026997</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>time</Label>
@@ -11082,7 +11289,7 @@ equations
    </PlotPanels>
    <WindowRectangles virtualDesktopWidth="5760" virtualDesktopHeight="1080">
      <WindowRectangle monitor="1" left="-1920" top="0" right="0" bottom="1080" plotpanelid="2">
-0 0.0259259 1 1
+0.0770833 0.135185 0.827083 0.859259
      </WindowRectangle>
      <WindowRectangle monitor="1" left="-1920" top="0" right="0" bottom="1080" plotpanelid="1">
 0.075 0.0861111 0.917708 0.938889
@@ -11255,6 +11462,9 @@ equations
   </ExportData>
   <BreakPoints>
   </BreakPoints>
+  <AnimationPlayback>
+   <PlaybackSpeed>1.0</PlaybackSpeed>
+  </AnimationPlayback>
 </ExpData>
   </Experiment>
 </Experiments>