lbessard@293: <?xml version="1.0" encoding="UTF-8"?> lbessard@293: <project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" lbessard@293: xmlns="http://www.plcopen.org/xml/tc6.xsd" lbessard@293: xmlns:xhtml="http://www.w3.org/1999/xhtml" lbessard@293: xsi:schemaLocation="http://www.plcopen.org/xml/tc6.xsd"> lbessard@293: <fileHeader companyName="Lolitech" lbessard@293: productName="GenTraj" lbessard@293: productVersion="1.0" lbessard@293: creationDateTime="2009-01-13T12:15:00"/> lbessard@293: <contentHeader name="GenTraj" lbessard@293: modificationDateTime="2009-01-14T20:10:13"> lbessard@293: <coordinateInfo> lbessard@293: <fbd> lbessard@293: <scaling x="0" y="0"/> lbessard@293: </fbd> lbessard@293: <ld> lbessard@293: <scaling x="0" y="0"/> lbessard@293: </ld> lbessard@293: <sfc> lbessard@293: <scaling x="0" y="0"/> lbessard@293: </sfc> lbessard@293: </coordinateInfo> lbessard@293: </contentHeader> lbessard@293: <types> lbessard@293: <dataTypes/> lbessard@293: <pous> lbessard@293: <pou name="GenTraj" pouType="functionBlock"> lbessard@293: <interface> lbessard@293: <inputVars> lbessard@293: <variable name="Period"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Gmvt"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Vmvt"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="P0"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="V0"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Pf"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Vf"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: </inputVars> lbessard@293: <outputVars> lbessard@293: <variable name="Pn"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Vn"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Gn"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: </outputVars> lbessard@293: <localVars> lbessard@293: <variable name="P"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="F"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Ps2"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Fs3"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="delta"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Va"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Vb"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="Vaut"> lbessard@293: <type> lbessard@293: <REAL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: </localVars> lbessard@293: </interface> lbessard@293: <body> lbessard@293: <ST> lbessard@293: <![CDATA[IF Period <> P THEN lbessard@293: P := Period; lbessard@293: F := 1.0 / P; lbessard@293: Ps2 := P / 2.0; lbessard@293: Fs3 := F / 3.0; lbessard@293: END_IF; lbessard@293: lbessard@293: IF Pf <> Pn THEN lbessard@293: delta := Pf - P0 - V0 * P; lbessard@293: lbessard@293: Va := SQRT(ABS(2.0 * Gmvt * delta) + Vf * Vf); lbessard@293: Vb := Vf + delta * Fs3; lbessard@293: lbessard@293: IF Va > ABS(Vb) THEN lbessard@293: Vaut := MIN(ABS(Vb), Vmvt); lbessard@293: IF Vb < 0.0 THEN lbessard@293: Vaut := -Vaut; lbessard@293: END_IF; lbessard@293: ELSE lbessard@293: Vaut := MIN(Va, Vmvt); lbessard@293: IF Pf < P0 THEN lbessard@293: Vaut := -Vaut; lbessard@293: END_IF; lbessard@293: END_IF; lbessard@293: lbessard@293: Gn := MIN(ABS((Vaut - V0) * F), Gmvt); lbessard@293: IF Vaut < V0 THEN lbessard@293: Gn := -Gn; lbessard@293: END_IF; lbessard@293: lbessard@293: END_IF; lbessard@293: lbessard@293: Vn := V0 + Gn * P; lbessard@293: lbessard@293: Pn := P0 + (Vn + V0) * Ps2; lbessard@293: lbessard@293: IF (P0 >= Pf) <> (Pn >= Pf) AND Vf <> 0.0 THEN lbessard@293: Pn := Pf; lbessard@293: END_IF; lbessard@293: lbessard@293: ]]> lbessard@293: </ST> lbessard@293: </body> lbessard@293: </pou> lbessard@293: <pou name="TestMotion" pouType="program"> lbessard@293: <interface> lbessard@293: <localVars> lbessard@293: <variable name="GO"> lbessard@293: <type> lbessard@293: <BOOL/> lbessard@293: </type> lbessard@293: <initialValue> lbessard@293: <simpleValue value="TRUE"/> lbessard@293: </initialValue> lbessard@293: </variable> lbessard@293: <variable name="TRIG"> lbessard@293: <type> lbessard@293: <BOOL/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="GenTraj1"> lbessard@293: <type> lbessard@293: <derived name="GenTraj"/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="py_eval"> lbessard@293: <type> lbessard@293: <derived name="python_eval"/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: <variable name="RS1"> lbessard@293: <type> lbessard@293: <derived name="RS"/> lbessard@293: </type> lbessard@293: </variable> lbessard@293: </localVars> lbessard@293: </interface> lbessard@293: <body> lbessard@293: <FBD> lbessard@293: <inVariable localId="2" height="25" width="63"> lbessard@293: <position x="203" y="208"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="63" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>GO</expression> lbessard@293: </inVariable> lbessard@293: <inVariable localId="4" height="25" width="100"> lbessard@293: <position x="295" y="312"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="100" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>REAL#0.1</expression> lbessard@293: </inVariable> lbessard@293: <inVariable localId="6" height="25" width="100"> lbessard@293: <position x="294" y="345"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="100" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>REAL#1.0</expression> lbessard@293: </inVariable> lbessard@293: <inVariable localId="7" height="25" width="99"> lbessard@293: <position x="294" y="378"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="99" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>REAL#1.0</expression> lbessard@293: </inVariable> lbessard@293: <inVariable localId="8" height="25" width="100"> lbessard@293: <position x="292" y="477"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="100" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>REAL#10.0</expression> lbessard@293: </inVariable> lbessard@293: <block localId="9" width="178" height="290" typeName="GenTraj" instanceName="GenTraj1"> lbessard@293: <position x="488" y="255"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="EN"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="36"/> lbessard@293: <connection refLocalId="13" formalParameter="Q1"> lbessard@293: <position x="488" y="291"/> lbessard@293: <position x="442" y="291"/> lbessard@293: <position x="442" y="259"/> lbessard@293: <position x="396" y="259"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="Period"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="69"/> lbessard@293: <connection refLocalId="4"> lbessard@293: <position x="488" y="324"/> lbessard@293: <position x="395" y="324"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="Gmvt"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="102"/> lbessard@293: <connection refLocalId="6"> lbessard@293: <position x="488" y="357"/> lbessard@293: <position x="394" y="357"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="Vmvt"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="135"/> lbessard@293: <connection refLocalId="7"> lbessard@293: <position x="488" y="390"/> lbessard@293: <position x="393" y="390"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="P0"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="168"/> lbessard@293: <connection refLocalId="9" formalParameter="Pn"> lbessard@293: <position x="488" y="423"/> lbessard@293: <position x="430" y="423"/> lbessard@293: <position x="430" y="607"/> lbessard@293: <position x="724" y="607"/> lbessard@293: <position x="724" y="324"/> lbessard@293: <position x="666" y="324"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="V0"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="201"/> lbessard@293: <connection refLocalId="9" formalParameter="Vn"> lbessard@293: <position x="488" y="456"/> lbessard@293: <position x="453" y="456"/> lbessard@293: <position x="453" y="577"/> lbessard@293: <position x="696" y="577"/> lbessard@293: <position x="696" y="357"/> lbessard@293: <position x="666" y="357"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="Pf"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="234"/> lbessard@293: <connection refLocalId="8"> lbessard@293: <position x="488" y="489"/> lbessard@293: <position x="392" y="489"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="Vf"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="267"/> lbessard@293: <connection refLocalId="10"> lbessard@293: <position x="488" y="522"/> lbessard@293: <position x="392" y="522"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="ENO"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="178" y="36"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: <variable formalParameter="Pn"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="178" y="69"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: <variable formalParameter="Vn"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="178" y="102"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: <variable formalParameter="Gn"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="178" y="135"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <inVariable localId="10" height="25" width="99"> lbessard@293: <position x="293" y="510"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="99" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>REAL#0.0</expression> lbessard@293: </inVariable> lbessard@293: <block localId="11" width="75" height="70" typeName="EQ"> lbessard@293: <position x="197" y="247"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="IN1"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="32"/> lbessard@293: <connection refLocalId="9" formalParameter="Pn"> lbessard@293: <position x="197" y="279"/> lbessard@293: <position x="145" y="279"/> lbessard@293: <position x="145" y="629"/> lbessard@293: <position x="724" y="629"/> lbessard@293: <position x="724" y="324"/> lbessard@293: <position x="666" y="324"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="IN2"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="57"/> lbessard@293: <connection refLocalId="8"> lbessard@293: <position x="197" y="304"/> lbessard@293: <position x="176" y="304"/> lbessard@293: <position x="176" y="433"/> lbessard@293: <position x="409" y="433"/> lbessard@293: <position x="409" y="489"/> lbessard@293: <position x="392" y="489"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="OUT"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="75" y="32"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <block localId="12" width="120" height="87" typeName="python_eval" instanceName="py_eval"> lbessard@293: <position x="318" y="73"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="TRIG"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="36"/> lbessard@293: <connection refLocalId="15" formalParameter="OUT"> lbessard@293: <position x="318" y="109"/> lbessard@293: <position x="225" y="109"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="CODE"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="69"/> lbessard@293: <connection refLocalId="14"> lbessard@293: <position x="318" y="142"/> lbessard@293: <position x="280" y="142"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="ACK"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="120" y="36"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: <variable formalParameter="RESULT"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="120" y="69"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <block localId="13" width="55" height="60" typeName="RS" instanceName="RS1"> lbessard@293: <position x="341" y="229"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="S"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="30"/> lbessard@293: <connection refLocalId="2"> lbessard@293: <position x="341" y="259"/> lbessard@293: <position x="303" y="259"/> lbessard@293: <position x="303" y="220"/> lbessard@293: <position x="266" y="220"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: <variable formalParameter="R1"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="50"/> lbessard@293: <connection refLocalId="11" formalParameter="OUT"> lbessard@293: <position x="341" y="279"/> lbessard@293: <position x="272" y="279"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="Q1"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="55" y="30"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <inVariable localId="14" height="25" width="50"> lbessard@293: <position x="230" y="130"/> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="50" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>'GO'</expression> lbessard@293: </inVariable> lbessard@293: <block localId="15" width="65" height="40" typeName="NOT"> lbessard@293: <position x="160" y="79"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="IN"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="30"/> lbessard@293: <connection refLocalId="16"> lbessard@293: <position x="160" y="109"/> lbessard@293: <position x="118" y="109"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="OUT"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="65" y="30"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <inOutVariable localId="16" height="25" width="50"> lbessard@293: <position x="68" y="97"/> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="12"/> lbessard@293: <connection refLocalId="15" formalParameter="OUT"> lbessard@293: <position x="68" y="109"/> lbessard@293: <position x="42" y="109"/> lbessard@293: <position x="42" y="50"/> lbessard@293: <position x="262" y="50"/> lbessard@293: <position x="262" y="109"/> lbessard@293: <position x="225" y="109"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="50" y="12"/> lbessard@293: </connectionPointOut> lbessard@293: <expression>TRIG</expression> lbessard@293: </inOutVariable> lbessard@293: <block localId="17" width="140" height="40" typeName="STRING_TO_INT"> lbessard@293: <position x="486" y="112"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="IN"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="30"/> lbessard@293: <connection refLocalId="12" formalParameter="RESULT"> lbessard@293: <position x="486" y="142"/> lbessard@293: <position x="438" y="142"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="OUT"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="140" y="30"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <block localId="18" width="120" height="40" typeName="INT_TO_BOOL"> lbessard@293: <position x="671" y="112"/> lbessard@293: <inputVariables> lbessard@293: <variable formalParameter="IN"> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="30"/> lbessard@293: <connection refLocalId="17" formalParameter="OUT"> lbessard@293: <position x="671" y="142"/> lbessard@293: <position x="626" y="142"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: </variable> lbessard@293: </inputVariables> lbessard@293: <inOutVariables/> lbessard@293: <outputVariables> lbessard@293: <variable formalParameter="OUT"> lbessard@293: <connectionPointOut> lbessard@293: <relPosition x="120" y="30"/> lbessard@293: </connectionPointOut> lbessard@293: </variable> lbessard@293: </outputVariables> lbessard@293: </block> lbessard@293: <outVariable localId="19" height="25" width="63"> lbessard@293: <position x="826" y="130"/> lbessard@293: <connectionPointIn> lbessard@293: <relPosition x="0" y="12"/> lbessard@293: <connection refLocalId="18" formalParameter="OUT"> lbessard@293: <position x="826" y="142"/> lbessard@293: <position x="791" y="142"/> lbessard@293: </connection> lbessard@293: </connectionPointIn> lbessard@293: <expression>GO</expression> lbessard@293: </outVariable> lbessard@293: </FBD> lbessard@293: </body> lbessard@293: </pou> lbessard@293: </pous> lbessard@293: </types> lbessard@293: <instances> lbessard@293: <configurations> lbessard@293: <configuration name="config"> lbessard@293: <resource name="ress"> lbessard@293: <task name="MAIN_TASK" interval="00:00:00.100000" priority="0"> lbessard@293: <pouInstance name="MAIN_INSTANCE" type="TestMotion"/> lbessard@293: </task> lbessard@293: </resource> lbessard@293: </configuration> lbessard@293: </configurations> lbessard@293: </instances> lbessard@293: </project>