diff --git a/omniwheels_x4_soccer_goal.blockly b/omniwheels_x4_soccer_goal.blockly
index e8624ed7951563239095537dfc35fc7c889a6fc1..48bd351401a0309102cb5c9342ec1d0ce1f4bf95 100644
--- a/omniwheels_x4_soccer_goal.blockly
+++ b/omniwheels_x4_soccer_goal.blockly
@@ -9,11 +9,13 @@
<variable id="fPH`.@]riH+=v{,A)bP5">cmd</variable>
<variable id="vH;^OzJY7dOdepMScU1s">state</variable>
<variable id="S~)Q:X5BHfQZe.ZJfdE)">factor_y</variable>
+ <variable id="v0BvP7[91TGaMpQG$r0f">goal_width</variable>
<variable id="S4.mduNQC(va3iR{oSdB">posx_ball</variable>
<variable id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</variable>
<variable id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</variable>
<variable id="5@U(%o_ghQ1;jiB#odH8">counters</variable>
<variable id="Ym/+Qo0YZe25;%N1d!*f">x_min</variable>
+ <variable id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</variable>
<variable id="q#H`+Gwt~43Dh@fu;G-D">posy_ball</variable>
<variable id="a8;eFhj+gj?)7exUyU;p">posy_goal_left</variable>
<variable id="+j/X+RCIMajR28(u#7DG">posy_goal_right</variable>
@@ -25,8 +27,6 @@
<variable id="1idj3sw*{G$j`2:II{^5">v_fast</variable>
<variable id="xjf5O*w9;IZf$n55ytVF">y_ball_touch</variable>
<variable id="09n]XF2CKURQs)nHOfam">y_ball_near</variable>
- <variable id="v0BvP7[91TGaMpQG$r0f">goal_width</variable>
- <variable id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</variable>
<variable id="2Lkg^#Z.uZy(%Nr[26)6">search_dir_left</variable>
</variables>
<block type="start_block" id="1n38UCidqR*}g6g]e[!h" deletable="false" x="0" y="0">
@@ -442,18 +442,18 @@
</block>
</value>
<statement name="DO1">
- <block type="procedures_callnoreturn" id="R590b9iY$aQ~B7!m8f2l">
- <mutation name="turnSideRight">
+ <block type="procedures_callnoreturn" id="@n*^wewk`x?NX#m2}EQE">
+ <mutation name="turnSideLeft">
<arg name="v"/>
<arg name="ticks"/>
</mutation>
<value name="ARG0">
- <block type="variables_get" id="vk%)2O)R9Ft/FMn)2yr7">
+ <block type="variables_get" id="+m}1a:ZW]JoRW2QP.n06">
<field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
<value name="ARG1">
- <block type="math_number" id="N.3ER1#5:PMk;ocu-#q(">
+ <block type="math_number" id="_;2gj%#h1g[wnD|7oXuo">
<field name="NUM">20</field>
</block>
</value>
@@ -475,18 +475,18 @@
</block>
</value>
<statement name="DO2">
- <block type="procedures_callnoreturn" id="@n*^wewk`x?NX#m2}EQE">
- <mutation name="turnSideLeft">
+ <block type="procedures_callnoreturn" id="R590b9iY$aQ~B7!m8f2l">
+ <mutation name="turnSideRight">
<arg name="v"/>
<arg name="ticks"/>
</mutation>
<value name="ARG0">
- <block type="variables_get" id="+m}1a:ZW]JoRW2QP.n06">
+ <block type="variables_get" id="vk%)2O)R9Ft/FMn)2yr7">
<field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
<value name="ARG1">
- <block type="math_number" id="_;2gj%#h1g[wnD|7oXuo">
+ <block type="math_number" id="N.3ER1#5:PMk;ocu-#q(">
<field name="NUM">20</field>
</block>
</value>
@@ -538,18 +538,18 @@
</block>
</value>
<next>
- <block type="procedures_callnoreturn" id="+j-FQusX0T9FJ1P}u:0|">
- <mutation name="turnSideRight">
+ <block type="procedures_callnoreturn" id="k0ElL*hZU:LP$5|:#LB?">
+ <mutation name="turnSideLeft">
<arg name="v"/>
<arg name="ticks"/>
</mutation>
<value name="ARG0">
- <block type="variables_get" id="Jx~`*{sDX:}UuM`gsZp/">
+ <block type="variables_get" id="OPBK1GzGPPmCYw|3a:2R">
<field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
<value name="ARG1">
- <block type="math_number" id=")k]SdR3F1bII`Mo!4Q2T">
+ <block type="math_number" id="gcH1Ps(J*{a)=3pi9diY">
<field name="NUM">20</field>
</block>
</value>
@@ -597,18 +597,18 @@
</block>
</value>
<next>
- <block type="procedures_callnoreturn" id="k0ElL*hZU:LP$5|:#LB?">
- <mutation name="turnSideLeft">
+ <block type="procedures_callnoreturn" id="+j-FQusX0T9FJ1P}u:0|">
+ <mutation name="turnSideRight">
<arg name="v"/>
<arg name="ticks"/>
</mutation>
<value name="ARG0">
- <block type="variables_get" id="OPBK1GzGPPmCYw|3a:2R">
+ <block type="variables_get" id="Jx~`*{sDX:}UuM`gsZp/">
<field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
<value name="ARG1">
- <block type="math_number" id="gcH1Ps(J*{a)=3pi9diY">
+ <block type="math_number" id=")k]SdR3F1bII`Mo!4Q2T">
<field name="NUM">20</field>
</block>
</value>
@@ -676,262 +676,180 @@
</block>
</value>
<next>
- <block type="variables_set" id="[N80,Pt{V6fUM/*V.[V,">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- <value name="VALUE">
- <block type="math_arithmetic" id="stzng{bs`Stm,oBHUX4!">
- <field name="OP">MINUS</field>
+ <block type="controls_if" id="ip*`sP8:b(5pR4)~R_an">
+ <mutation elseif="2" else="1"/>
+ <value name="IF0">
+ <block type="logic_operation" id="fn=DRPVeg$aV~zkMY*tl" inline="false">
+ <field name="OP">AND</field>
<value name="A">
- <block type="variables_get" id="((%]wA!T|%@cv3,wHB|j">
- <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
+ <block type="logic_compare" id="2Q5e3vZ!5/=nainF0od+">
+ <field name="OP">GTE</field>
+ <value name="A">
+ <block type="variables_get" id="CDb^~4feXbvngdh004${">
+ <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="cR*DIW$3$NL#u11S8#v,">
+ <field name="VAR" id="Ym/+Qo0YZe25;%N1d!*f">x_min</field>
+ </block>
+ </value>
</block>
</value>
<value name="B">
- <block type="variables_get" id="0%WOX|exw*bIEaQ3.`9q">
- <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
+ <block type="logic_compare" id="c(LuogB[ECxBtM-6/r4b">
+ <field name="OP">LTE</field>
+ <value name="A">
+ <block type="variables_get" id="z~zx!V^G#Ywd}HPCLcCn">
+ <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id=")0O+HGV1!`X_dJGq4=yZ">
+ <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
+ </block>
+ </value>
</block>
</value>
</block>
</value>
- <next>
- <block type="variables_set" id="VutK`C):{nbAi#Axhu!x">
- <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
- <value name="VALUE">
- <block type="math_arithmetic" id="(KM3-N3=G5B(T:(!4_lq">
- <field name="OP">ADD</field>
+ <statement name="DO0">
+ <block type="controls_if" id="#R16p?0!Rt1}82Q_5zO2">
+ <mutation elseif="1" else="1"/>
+ <value name="IF0">
+ <block type="logic_compare" id="UzXI%rAMZA[}UE*cbXUd">
+ <field name="OP">LT</field>
<value name="A">
- <block type="variables_get" id="bS|/)kwXja$*IpF01zL/">
- <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
+ <block type="variables_get" id="@5%T+t9W1)BZT@oXqnww">
+ <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
</block>
</value>
<value name="B">
- <block type="math_arithmetic" id="X;-9(dOX5-El:hAx$.=B">
- <field name="OP">DIVIDE</field>
- <value name="A">
- <block type="variables_get" id="Z4=3XhJOodeLD~q6l?{4">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="+%Vyps+4_*q%/:(:2j+E">
- <field name="NUM">2</field>
- </block>
- </value>
+ <block type="math_number" id="Wb8s[K5@WIquU`@,7c@w">
+ <field name="NUM">130</field>
</block>
</value>
</block>
</value>
- <next>
- <block type="controls_if" id="ip*`sP8:b(5pR4)~R_an">
- <mutation elseif="2" else="1"/>
- <value name="IF0">
- <block type="logic_operation" id="fn=DRPVeg$aV~zkMY*tl" inline="false">
- <field name="OP">AND</field>
- <value name="A">
- <block type="logic_compare" id="2Q5e3vZ!5/=nainF0od+">
- <field name="OP">GTE</field>
- <value name="A">
- <block type="variables_get" id="CDb^~4feXbvngdh004${">
- <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
- </block>
- </value>
- <value name="B">
- <block type="variables_get" id="cR*DIW$3$NL#u11S8#v,">
- <field name="VAR" id="Ym/+Qo0YZe25;%N1d!*f">x_min</field>
- </block>
- </value>
- </block>
- </value>
- <value name="B">
- <block type="logic_compare" id="c(LuogB[ECxBtM-6/r4b">
- <field name="OP">LTE</field>
- <value name="A">
- <block type="variables_get" id="z~zx!V^G#Ywd}HPCLcCn">
- <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
- </block>
- </value>
- <value name="B">
- <block type="variables_get" id=")0O+HGV1!`X_dJGq4=yZ">
- <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
- </block>
- </value>
- </block>
- </value>
+ <statement name="DO0">
+ <block type="procedures_callnoreturn" id="pu3s6{_T@bDFusTBxc)c">
+ <mutation name="fwd">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="]RO~:%x7+T~!YzQ5FD:C">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
- <statement name="DO0">
- <block type="controls_if" id="#R16p?0!Rt1}82Q_5zO2">
- <mutation elseif="1" else="1"/>
- <value name="IF0">
- <block type="logic_compare" id="UzXI%rAMZA[}UE*cbXUd">
- <field name="OP">LT</field>
- <value name="A">
- <block type="variables_get" id="@5%T+t9W1)BZT@oXqnww">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="Wb8s[K5@WIquU`@,7c@w">
- <field name="NUM">130</field>
- </block>
- </value>
- </block>
- </value>
- <statement name="DO0">
- <block type="procedures_callnoreturn" id="pu3s6{_T@bDFusTBxc)c">
- <mutation name="fwd">
- <arg name="v"/>
- <arg name="ticks"/>
- </mutation>
- <value name="ARG0">
- <block type="variables_get" id="]RO~:%x7+T~!YzQ5FD:C">
- <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
- </block>
- </value>
- <value name="ARG1">
- <block type="math_number" id="v0c-z!Z^U6/IWhKEPnE*">
- <field name="NUM">10</field>
- </block>
- </value>
- </block>
- </statement>
- <value name="IF1">
- <block type="logic_compare" id="XoLsJ}.)nZb?`6tpIqv-">
- <field name="OP">GT</field>
- <value name="A">
- <block type="variables_get" id="9NAs7T{MBx/=YP5BpW43">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="|2@A]z`Rs0UlH}Ovv9`T">
- <field name="NUM">40</field>
- </block>
- </value>
- </block>
- </value>
- <statement name="DO1">
- <block type="procedures_callnoreturn" id="Y6^M7Oqd:JSsRteXXtVD">
- <mutation name="bwd">
- <arg name="v"/>
- <arg name="ticks"/>
- </mutation>
- <value name="ARG0">
- <block type="variables_get" id="8F$+J#P35!A!gFI[:AHU">
- <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
- </block>
- </value>
- <value name="ARG1">
- <block type="math_number" id="dFB2rRWf.tj`$C?![{FH">
- <field name="NUM">10</field>
- </block>
- </value>
- </block>
- </statement>
+ <value name="ARG1">
+ <block type="math_number" id="v0c-z!Z^U6/IWhKEPnE*">
+ <field name="NUM">10</field>
</block>
- </statement>
- <value name="IF1">
- <block type="logic_compare" id="(H+T:yEGNo[y%GO7P;Dz">
- <field name="OP">LT</field>
- <value name="A">
- <block type="variables_get" id="D`d-ScN5}NDq#So6U%2M">
- <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
- </block>
- </value>
- <value name="B">
- <block type="variables_get" id="kw^gNGg!Q00uE[+DZwGg">
- <field name="VAR" id="Ym/+Qo0YZe25;%N1d!*f">x_min</field>
- </block>
- </value>
+ </value>
+ </block>
+ </statement>
+ <value name="IF1">
+ <block type="logic_compare" id="XoLsJ}.)nZb?`6tpIqv-">
+ <field name="OP">GT</field>
+ <value name="A">
+ <block type="variables_get" id="9NAs7T{MBx/=YP5BpW43">
+ <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
</block>
</value>
- <statement name="DO1">
- <block type="procedures_callnoreturn" id="mG-?FU_hknyuqCH{oWJT">
- <mutation name="left">
- <arg name="v"/>
- <arg name="ticks"/>
- </mutation>
- <value name="ARG0">
- <block type="variables_get" id="R%Oe)=kR9%E%`E3L[QVN">
- <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
- </block>
- </value>
- <value name="ARG1">
- <block type="math_round" id="6n}aM10-I0P:)!Gr(]i5">
- <field name="OP">ROUND</field>
- <field name="DECIMALS">0</field>
- <value name="NUM">
- <block type="math_arithmetic" id="9e@Xpj!L)E*5)/BWxD;,">
- <field name="OP">MULTIPLY</field>
- <value name="A">
- <block type="variables_get" id="|5*]zYtMm]955K1])q46">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="=Vugu-k,xt8LKJmb5/PQ">
- <field name="NUM">0.01</field>
- </block>
- </value>
- </block>
- </value>
- </block>
- </value>
+ <value name="B">
+ <block type="math_number" id="|2@A]z`Rs0UlH}Ovv9`T">
+ <field name="NUM">40</field>
</block>
- </statement>
- <value name="IF2">
- <block type="logic_compare" id="VxyB%0*)9[A{wCi_b~$0">
- <field name="OP">GT</field>
- <value name="A">
- <block type="variables_get" id="-;*b3g{dTtQ{%@$gw6P)">
- <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
- </block>
- </value>
- <value name="B">
- <block type="variables_get" id="?q##-)hY2%F2{Lx*2U0U">
- <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
- </block>
- </value>
+ </value>
+ </block>
+ </value>
+ <statement name="DO1">
+ <block type="procedures_callnoreturn" id="Y6^M7Oqd:JSsRteXXtVD">
+ <mutation name="bwd">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="8F$+J#P35!A!gFI[:AHU">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
</block>
</value>
- <statement name="DO2">
- <block type="procedures_callnoreturn" id="oGweCRo;|ZbP%1KR7B]a">
- <mutation name="right">
- <arg name="v"/>
- <arg name="ticks"/>
- </mutation>
- <value name="ARG0">
- <block type="variables_get" id="Vc7Fp*A$WDu^X7t.%1`9">
- <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
- </block>
- </value>
- <value name="ARG1">
- <block type="math_round" id="5^^7yxNg{mw+_^(*rmC#">
- <field name="OP">ROUND</field>
- <field name="DECIMALS">0</field>
- <value name="NUM">
- <block type="math_arithmetic" id="eW}|s]3dmm#iEvK%uj/1">
- <field name="OP">MULTIPLY</field>
- <value name="A">
- <block type="variables_get" id="B}iVX#7NZ$fvR1!YH6@S">
- <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="(n)Uz^8S1N!C^7@](sJ)">
- <field name="NUM">0.01</field>
- </block>
- </value>
- </block>
- </value>
- </block>
- </value>
+ <value name="ARG1">
+ <block type="math_number" id="dFB2rRWf.tj`$C?![{FH">
+ <field name="NUM">10</field>
</block>
- </statement>
+ </value>
</block>
- </next>
+ </statement>
</block>
- </next>
+ </statement>
+ <value name="IF1">
+ <block type="logic_compare" id="(H+T:yEGNo[y%GO7P;Dz">
+ <field name="OP">LT</field>
+ <value name="A">
+ <block type="variables_get" id="D`d-ScN5}NDq#So6U%2M">
+ <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="kw^gNGg!Q00uE[+DZwGg">
+ <field name="VAR" id="Ym/+Qo0YZe25;%N1d!*f">x_min</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO1">
+ <block type="procedures_callnoreturn" id="mG-?FU_hknyuqCH{oWJT">
+ <mutation name="left">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="R%Oe)=kR9%E%`E3L[QVN">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
+ </block>
+ </value>
+ <value name="ARG1">
+ <block type="math_number" id="xRlPd!,I^P3`5.OAFO-L">
+ <field name="NUM">10</field>
+ </block>
+ </value>
+ </block>
+ </statement>
+ <value name="IF2">
+ <block type="logic_compare" id="VxyB%0*)9[A{wCi_b~$0">
+ <field name="OP">GT</field>
+ <value name="A">
+ <block type="variables_get" id="-;*b3g{dTtQ{%@$gw6P)">
+ <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="?q##-)hY2%F2{Lx*2U0U">
+ <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO2">
+ <block type="procedures_callnoreturn" id="oGweCRo;|ZbP%1KR7B]a">
+ <mutation name="right">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="Vc7Fp*A$WDu^X7t.%1`9">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
+ </block>
+ </value>
+ <value name="ARG1">
+ <block type="math_number" id="E-tWBnGP,8}Za*a95_6n">
+ <field name="NUM">10</field>
+ </block>
+ </value>
+ </block>
+ </statement>
</block>
</next>
</block>
@@ -987,29 +905,8 @@
</block>
</value>
<value name="ARG1">
- <block type="math_round" id=")kbJ+|FY0:`OyFD|5o1n">
- <field name="OP">ROUND</field>
- <field name="DECIMALS">0</field>
- <value name="NUM">
- <block type="math_single" id="w#OguMAoO|^wrS9~m7QM">
- <field name="OP">ABS</field>
- <value name="NUM">
- <block type="math_arithmetic" id="A9MgtS+IiS%Ns7f$JnJ`">
- <field name="OP">MULTIPLY</field>
- <value name="A">
- <block type="variables_get" id="^$$6dKK$Lr7ThLi|d_3y">
- <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="=+CXXJ[n-?V=f6zroOtS">
- <field name="NUM">0.1</field>
- </block>
- </value>
- </block>
- </value>
- </block>
- </value>
+ <block type="math_number" id="_v5Xu{_@Lx/yhwcczyYw">
+ <field name="NUM">10</field>
</block>
</value>
</block>
@@ -1067,29 +964,8 @@
</block>
</value>
<value name="ARG1">
- <block type="math_round" id="!9XkF[+`Qfl(y.B94[cc">
- <field name="OP">ROUND</field>
- <field name="DECIMALS">0</field>
- <value name="NUM">
- <block type="math_single" id="gOM#UD(tV-_1Cyz:a;/^">
- <field name="OP">ABS</field>
- <value name="NUM">
- <block type="math_arithmetic" id="nqa/Wid0hzlziX,(MK#4">
- <field name="OP">MULTIPLY</field>
- <value name="A">
- <block type="variables_get" id="1sebS=sup~V3d9+*a:d=">
- <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
- </block>
- </value>
- <value name="B">
- <block type="math_number" id="qoM$8go,0,2xd@dr`hjO">
- <field name="NUM">0.1</field>
- </block>
- </value>
- </block>
- </value>
- </block>
- </value>
+ <block type="math_number" id="xovp]RW^z_.J8r09vIYb">
+ <field name="NUM">10</field>
</block>
</value>
</block>
@@ -1104,6 +980,16 @@
<field name="TEXT">NONE</field>
</block>
</value>
+ <next>
+ <block type="variables_set" id="[XyQyt*v|[_TPm5C]YnP">
+ <field name="VAR" id="fPH`.@]riH+=v{,A)bP5">cmd</field>
+ <value name="VALUE">
+ <block type="text" id="IaVZ-uIe[M@`VD!(z-d1">
+ <field name="TEXT">NONE</field>
+ </block>
+ </value>
+ </block>
+ </next>
</block>
</statement>
</block>
@@ -1280,7 +1166,7 @@
</value>
<next>
<block type="procedures_callnoreturn" id="O6U,(~H4n-^0fP^x)_,{">
- <mutation name="checkTimeouts"/>
+ <mutation name="checkTimeoutsSetGoalBoth"/>
<next>
<block type="util_wait_for" id="U4XL;U0gG@tD~]*kp|L{">
<field name="format">ms</field>
@@ -1360,7 +1246,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="BJV|hZc!#JXq:io7MmM`" x="0" y="4281">
+ <block type="procedures_defnoreturn" id="BJV|hZc!#JXq:io7MmM`" x="0" y="4183">
<field name="NAME">stepBall</field>
<statement name="STACK">
<block type="variables_set" id=".tHpZ}-+@nb7zjqP7C^#">
@@ -1698,121 +1584,222 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="N2EC5uK:$~0ERXAdK0!y" x="0" y="5376">
- <field name="NAME">checkTimeouts</field>
+ <block type="procedures_defnoreturn" id="N2EC5uK:$~0ERXAdK0!y" x="0" y="5278">
+ <field name="NAME">checkTimeoutsSetGoalBoth</field>
<statement name="STACK">
- <block type="controls_if" id="o.;j7*F0}TW}ju0E18(_">
+ <block type="controls_if" id="63FfwOqK(iF!2~DVc~`p">
<value name="IF0">
- <block type="logic_compare" id="yaTkeEV~1|XTU#NgT)j7" inline="false">
- <field name="OP">GT</field>
+ <block type="logic_operation" id="L/E/`yRU.kw4/D+HP-F[" inline="false">
+ <field name="OP">AND</field>
<value name="A">
- <block type="math_arithmetic" id="zgksfhhp2%*/nv0kv]6k">
- <field name="OP">MINUS</field>
+ <block type="logic_compare" id="I9uC`Uf0Y=_8gpCN?Ov_">
+ <field name="OP">NEQ</field>
<value name="A">
- <block type="util_runtime_measurement" id="j-xyUfRHV,b}kCz08o+m">
- <field name="TIME">MILLISECOND</field>
+ <block type="variables_get" id="3RRwxyvfyhi.+L{8{c4N">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
</block>
</value>
<value name="B">
- <block type="variables_get" id="w|9iU`@W=6$!Dl%9u-Y@">
- <field name="VAR" id="v3eHF0atAL+ouaa{W$=m">ts_ball</field>
- </block>
+ <block type="logic_null" id="aVy}X6fIheDVU7?QmIga"/>
</value>
</block>
</value>
<value name="B">
- <block type="math_number" id="5_ryZ94QfcJv2_:O]/?g">
- <field name="NUM">500</field>
+ <block type="logic_compare" id="OVUGpM,e=EFQNaqa,3lI">
+ <field name="OP">NEQ</field>
+ <value name="A">
+ <block type="variables_get" id="p3!)j~MgxcC.una)S?7l">
+ <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="logic_null" id="3QE)6f#5:5QB$%^pd4]N"/>
+ </value>
</block>
</value>
</block>
</value>
<statement name="DO0">
- <block type="variables_set" id="Z:~u5a?M/hcKIufGbk]L">
- <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
+ <block type="variables_set" id="[N80,Pt{V6fUM/*V.[V,">
+ <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
<value name="VALUE">
- <block type="logic_null" id="6nvUE+P$gKwIm:+6*sP!"/>
+ <block type="math_arithmetic" id="stzng{bs`Stm,oBHUX4!">
+ <field name="OP">MINUS</field>
+ <value name="A">
+ <block type="variables_get" id="((%]wA!T|%@cv3,wHB|j">
+ <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="0%WOX|exw*bIEaQ3.`9q">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
+ </block>
+ </value>
+ </block>
</value>
<next>
- <block type="variables_set" id="$`MFfO6i!mT/klG+$:H]">
- <field name="VAR" id="q#H`+Gwt~43Dh@fu;G-D">posy_ball</field>
+ <block type="variables_set" id="VutK`C):{nbAi#Axhu!x">
+ <field name="VAR" id="{]]L-TSC^#XtGnVinxf[">posx_goal_both</field>
<value name="VALUE">
- <block type="logic_null" id="FHu):3h?Msu%QttHEtj]"/>
+ <block type="math_arithmetic" id="(KM3-N3=G5B(T:(!4_lq">
+ <field name="OP">ADD</field>
+ <value name="A">
+ <block type="variables_get" id="bS|/)kwXja$*IpF01zL/">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="math_arithmetic" id="X;-9(dOX5-El:hAx$.=B">
+ <field name="OP">DIVIDE</field>
+ <value name="A">
+ <block type="variables_get" id="Z4=3XhJOodeLD~q6l?{4">
+ <field name="VAR" id="v0BvP7[91TGaMpQG$r0f">goal_width</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="math_number" id="+%Vyps+4_*q%/:(:2j+E">
+ <field name="NUM">2</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ </block>
</value>
</block>
</next>
</block>
</statement>
<next>
- <block type="controls_if" id="TB*rcFM~i@*D%Dwtx/?U">
+ <block type="controls_if" id="o.;j7*F0}TW}ju0E18(_">
<value name="IF0">
- <block type="logic_compare" id="gPw1GMDTD1_[][5luwe;" inline="false">
+ <block type="logic_compare" id="yaTkeEV~1|XTU#NgT)j7" inline="false">
<field name="OP">GT</field>
<value name="A">
- <block type="math_arithmetic" id="THgFqXlrn6V_SnFEL,qY">
+ <block type="math_arithmetic" id="zgksfhhp2%*/nv0kv]6k">
<field name="OP">MINUS</field>
<value name="A">
- <block type="util_runtime_measurement" id="uw^6{2#8iqZO7RG;YaY7">
+ <block type="util_runtime_measurement" id="j-xyUfRHV,b}kCz08o+m">
<field name="TIME">MILLISECOND</field>
</block>
</value>
<value name="B">
- <block type="variables_get" id="HKOfg8b$oR@vts;7DObj">
- <field name="VAR" id=";Ki:[qYaZnowqSa`]/aW">ts_goal_left</field>
+ <block type="variables_get" id="w|9iU`@W=6$!Dl%9u-Y@">
+ <field name="VAR" id="v3eHF0atAL+ouaa{W$=m">ts_ball</field>
</block>
</value>
</block>
</value>
<value name="B">
- <block type="math_number" id=".b=S3huhD!|a4Hsf^rGL">
+ <block type="math_number" id="5_ryZ94QfcJv2_:O]/?g">
<field name="NUM">500</field>
</block>
</value>
</block>
</value>
<statement name="DO0">
- <block type="variables_set" id="9zHV?OMe7TGQwr6uc)dD">
- <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
+ <block type="variables_set" id="Z:~u5a?M/hcKIufGbk]L">
+ <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
<value name="VALUE">
- <block type="logic_null" id="cM8^dckuY:nW*9,QoDS%"/>
+ <block type="logic_null" id="6nvUE+P$gKwIm:+6*sP!"/>
</value>
+ <next>
+ <block type="variables_set" id="$`MFfO6i!mT/klG+$:H]">
+ <field name="VAR" id="q#H`+Gwt~43Dh@fu;G-D">posy_ball</field>
+ <value name="VALUE">
+ <block type="logic_null" id="FHu):3h?Msu%QttHEtj]"/>
+ </value>
+ </block>
+ </next>
</block>
</statement>
<next>
- <block type="controls_if" id="3O$%2WU}~L29)rJb:dRd">
+ <block type="controls_if" id="TB*rcFM~i@*D%Dwtx/?U">
<value name="IF0">
- <block type="logic_compare" id="KdN#iJT(!{.3f(:7.mAV" inline="false">
+ <block type="logic_compare" id="gPw1GMDTD1_[][5luwe;" inline="false">
<field name="OP">GT</field>
<value name="A">
- <block type="math_arithmetic" id="o?sdoD{y^2G_me0TF$8d">
+ <block type="math_arithmetic" id="THgFqXlrn6V_SnFEL,qY">
<field name="OP">MINUS</field>
<value name="A">
- <block type="util_runtime_measurement" id="N%$;|Spy3LX-7f1.cFC$">
+ <block type="util_runtime_measurement" id="uw^6{2#8iqZO7RG;YaY7">
<field name="TIME">MILLISECOND</field>
</block>
</value>
<value name="B">
- <block type="variables_get" id="bpKR0|hoi*mdX]v*_:W7">
- <field name="VAR" id="z^z{Ev+IMfS6[W1raY;l">ts_goal_right</field>
+ <block type="variables_get" id="HKOfg8b$oR@vts;7DObj">
+ <field name="VAR" id=";Ki:[qYaZnowqSa`]/aW">ts_goal_left</field>
</block>
</value>
</block>
</value>
<value name="B">
- <block type="math_number" id=".+UfY)W[?!t8Vr%!Xs/v">
+ <block type="math_number" id=".b=S3huhD!|a4Hsf^rGL">
<field name="NUM">500</field>
</block>
</value>
</block>
</value>
<statement name="DO0">
- <block type="variables_set" id="5!z7rY5T3ynPTu}d$.l[">
- <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
+ <block type="variables_set" id="9zHV?OMe7TGQwr6uc)dD">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal_left</field>
<value name="VALUE">
- <block type="logic_null" id="oiV/Q5_]2ENQr*Jd5%nz"/>
+ <block type="logic_null" id="cM8^dckuY:nW*9,QoDS%"/>
</value>
+ <next>
+ <block type="variables_set" id="[=A8t9eZ::W@6c=_h``$">
+ <field name="VAR" id="a8;eFhj+gj?)7exUyU;p">posy_goal_left</field>
+ <value name="VALUE">
+ <block type="logic_null" id="BJJL`4eo!GG.c,6KeN?z"/>
+ </value>
+ </block>
+ </next>
</block>
</statement>
+ <next>
+ <block type="controls_if" id="3O$%2WU}~L29)rJb:dRd">
+ <value name="IF0">
+ <block type="logic_compare" id="KdN#iJT(!{.3f(:7.mAV" inline="false">
+ <field name="OP">GT</field>
+ <value name="A">
+ <block type="math_arithmetic" id="o?sdoD{y^2G_me0TF$8d">
+ <field name="OP">MINUS</field>
+ <value name="A">
+ <block type="util_runtime_measurement" id="N%$;|Spy3LX-7f1.cFC$">
+ <field name="TIME">MILLISECOND</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="bpKR0|hoi*mdX]v*_:W7">
+ <field name="VAR" id="z^z{Ev+IMfS6[W1raY;l">ts_goal_right</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <value name="B">
+ <block type="math_number" id=".+UfY)W[?!t8Vr%!Xs/v">
+ <field name="NUM">500</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO0">
+ <block type="variables_set" id="5!z7rY5T3ynPTu}d$.l[">
+ <field name="VAR" id="7Tp~7_.qfmJN0SR]5p}F">posx_goal_right</field>
+ <value name="VALUE">
+ <block type="logic_null" id="oiV/Q5_]2ENQr*Jd5%nz"/>
+ </value>
+ <next>
+ <block type="variables_set" id="sb-+ahXY(pTQ|#ee-*8!">
+ <field name="VAR" id="+j/X+RCIMajR28(u#7DG">posy_goal_right</field>
+ <value name="VALUE">
+ <block type="logic_null" id="IsslYqR(7@VVytL2;=8@"/>
+ </value>
+ </block>
+ </next>
+ </block>
+ </statement>
+ </block>
+ </next>
</block>
</next>
</block>
@@ -1820,7 +1807,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="dgC*A|KHMC_R5S#ZRk}d" x="0" y="5809">
+ <block type="camera_on_ball_detected" id="dgC*A|KHMC_R5S#ZRk}d" x="0" y="5943">
<field name="instance_name">ball_detector</field>
<statement name="statement">
<block type="variables_set" id="=2/,IQfcLO{_5PNd3UK~">
@@ -2021,7 +2008,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="Rg2Gt(0WT:J2|Us~[j.3" x="0" y="6476">
+ <block type="camera_on_ball_detected" id="Rg2Gt(0WT:J2|Us~[j.3" x="0" y="6610">
<field name="instance_name">ball_detector_goal_left</field>
<statement name="statement">
<block type="variables_set" id="kvK,D4e%04[?+!jq*Lch">
@@ -2102,7 +2089,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="(?slhGsN^:d1`vP{j9#|" x="0" y="6807">
+ <block type="camera_on_ball_detected" id="(?slhGsN^:d1`vP{j9#|" x="0" y="6941">
<field name="instance_name">ball_detector_goal_right</field>
<statement name="statement">
<block type="variables_set" id="0!1?#@4|tcM8+U6E[$Tl">
@@ -2183,7 +2170,7 @@
</block>
</statement>
</block>
- <block type="counter_event" id="T^aHOKaV|bU#ZeZfn[GJ" x="0" y="7138">
+ <block type="counter_event" id="T^aHOKaV|bU#ZeZfn[GJ" x="0" y="7272">
<value name="condition">
<shadow type="counter_if_value" id="9`P*eh,|:Wnytp?G[|;{">
<field name="instance_name">TXT_M_C1_motor_step_counter</field>
@@ -2231,7 +2218,7 @@
</block>
</statement>
</block>
- <block type="counter_event" id="7wA~J+BkZ5~nV6aI|fLZ" x="0" y="7285">
+ <block type="counter_event" id="7wA~J+BkZ5~nV6aI|fLZ" x="0" y="7419">
<value name="condition">
<shadow type="counter_if_value" id="RDF|2PoXWvG^_k{FB37U">
<field name="instance_name">TXT_M_C2_motor_step_counter</field>
@@ -2279,7 +2266,7 @@
</block>
</statement>
</block>
- <block type="counter_event" id="Nv;e.HwUkTK/=(qEs~82" x="0" y="7432">
+ <block type="counter_event" id="Nv;e.HwUkTK/=(qEs~82" x="0" y="7566">
<value name="condition">
<shadow type="counter_if_value" id="05ihu;VYh*tmqA78pJQL">
<field name="instance_name">TXT_M_C3_motor_step_counter</field>
@@ -2327,7 +2314,7 @@
</block>
</statement>
</block>
- <block type="counter_event" id="j|Pz^uHewb7]si9}3az3" x="0" y="7579">
+ <block type="counter_event" id="j|Pz^uHewb7]si9}3az3" x="0" y="7713">
<value name="condition">
<shadow type="counter_if_value" id="N9]b*Kn_Nxk}[I9e!^Z.">
<field name="instance_name">TXT_M_C4_motor_step_counter</field>
@@ -2375,7 +2362,7 @@
</block>
</statement>
</block>
- <block type="input_event" id="j(TuhJ0G}{^qGSq:26FY" x="0" y="7726">
+ <block type="input_event" id="j(TuhJ0G}{^qGSq:26FY" x="0" y="7860">
<value name="condition">
<block type="input_photo_transistor_if_state" id="dX]-jf`=2/tNnEnHfKvx">
<field name="instance_name">TXT_M_I8_photo_transistor</field>
@@ -2398,7 +2385,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="RyK*td9aN5aR;@gw4f)+" x="0" y="7863">
+ <block type="procedures_defnoreturn" id="RyK*td9aN5aR;@gw4f)+" x="0" y="7997">
<field name="NAME">shotBall</field>
<statement name="STACK">
<block type="variables_set" id="l,2u2WF|]I#%kDS#5O7-">
@@ -2510,7 +2497,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="na=DVwj{P1Npv6z`ZSY{" x="0" y="8420">
+ <block type="procedures_defnoreturn" id="na=DVwj{P1Npv6z`ZSY{" x="0" y="8554">
<field name="NAME">soundGoal</field>
<statement name="STACK">
<block type="variables_set" id="o^c1yss2pIv%8YmX`=6X">
@@ -2551,7 +2538,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="UFM0X7|X:*ja#f+^7kz;" x="0" y="8614">
+ <block type="procedures_defnoreturn" id="UFM0X7|X:*ja#f+^7kz;" x="0" y="8748">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="-fFb#6a:h}yA(|L+pf*l"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="h?@dWX*~x|BDzo:O{is["/>
@@ -2648,7 +2635,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="*i[s2YTWD*g9HS4dxkPq" x="0" y="9027">
+ <block type="procedures_defnoreturn" id="*i[s2YTWD*g9HS4dxkPq" x="0" y="9161">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="k6At@e}Qga_x|SmVlf6C"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="~VyZflQ+9HCPP`ynQ/=Y"/>
@@ -2745,7 +2732,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="oSa9Ly*^-c^M{$gkMZt1" x="0" y="9440">
+ <block type="procedures_defnoreturn" id="oSa9Ly*^-c^M{$gkMZt1" x="0" y="9574">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="K)Z,8|d5@gck+Q]pnW01"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="(Id`t]O(jxfZJay}20X{"/>
@@ -2821,7 +2808,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="G@]xamrhDPW$7uQ6oPo#" x="0" y="9825">
+ <block type="procedures_defnoreturn" id="G@]xamrhDPW$7uQ6oPo#" x="0" y="9959">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="_mFFsmoZwL{(llRvf-eW"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="aNHXtqe[T`=Y[mqp`ZDN"/>
@@ -2897,7 +2884,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="5Hv=PFtVvPQA=IZrkJfU" x="0" y="10210">
+ <block type="procedures_defnoreturn" id="5Hv=PFtVvPQA=IZrkJfU" x="0" y="10344">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="~d8Z+AHZ!wz|G]JHW0lQ"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="jRMx@L,Ya|zd@/bBb#1T"/>
@@ -2988,7 +2975,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="[S@[MO!Z/cNt:Jsg4dd}" x="0" y="10651">
+ <block type="procedures_defnoreturn" id="[S@[MO!Z/cNt:Jsg4dd}" x="0" y="10785">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="8Z2n_ZVA.)85]V7j%3;7"/>
<arg name="ticks" varid="l9lR0!)Laya}3RYVW(ZQ" argid="J(gJ~*9ZJJa,[V1)[xn#"/>
diff --git a/omniwheels_x4_soccer_goal.py b/omniwheels_x4_soccer_goal.py
index 335485f645d498290a2b4c3b13b78451b0b8a97c..2a153047a2d3b246f5983b7299532968036bd3c8 100644
--- a/omniwheels_x4_soccer_goal.py
+++ b/omniwheels_x4_soccer_goal.py
@@ -16,11 +16,13 @@ ts_goal_right = None
cmd = None
state = None
factor_y = None
+goal_width = None
posx_ball = None
posx_goal_left = None
posx_goal_right = None
counters = None
x_min = None
+posx_goal_both = None
posy_ball = None
posy_goal_left = None
posy_goal_right = None
@@ -32,23 +34,26 @@ v_var_ball_ypos = None
v_fast = None
y_ball_touch = None
y_ball_near = None
-goal_width = None
-posx_goal_both = None
search_dir_left = None
-def checkTimeouts():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+def checkTimeoutsSetGoalBoth():
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
+ if posx_goal_left != None and posx_goal_right != None:
+ goal_width = posx_goal_right - posx_goal_left
+ posx_goal_both = posx_goal_left + goal_width / 2
if (time.time() * 1000) - ts_ball > 500:
posx_ball = None
posy_ball = None
if (time.time() * 1000) - ts_goal_left > 500:
posx_goal_left = None
+ posy_goal_left = None
if (time.time() * 1000) - ts_goal_right > 500:
posx_goal_right = None
+ posy_goal_right = None
def stepBall():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
state = 'stepBall'
if posx_ball < x_min:
left(v_slow, round(math.fabs(posx_ball) * 0.07))
@@ -68,7 +73,7 @@ def stepBall():
def ball_callback(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
ts_ball = (time.time() * 1000)
posx_ball = event.value.x
posy_ball = event.value.y
@@ -81,7 +86,7 @@ ball_detector.add_detection_listener(ball_callback)
def ball_callback2(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
ts_goal_left = (time.time() * 1000)
posx_goal_left = event.value.x
posy_goal_left = event.value.y
@@ -92,7 +97,7 @@ ball_detector_goal_left.add_detection_listener(ball_callback2)
def ball_callback23(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
ts_goal_right = (time.time() * 1000)
posx_goal_right = event.value.x
posy_goal_right = event.value.y
@@ -103,7 +108,7 @@ ball_detector_goal_right.add_detection_listener(ball_callback23)
def callback(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
if TXT_M_C1_motor_step_counter.get_count() >= 0:
counters[0] = TXT_M_C1_motor_step_counter.get_count()
display.set_attr("txt_label_counters.text", str(counters))
@@ -113,7 +118,7 @@ TXT_M_C1_motor_step_counter.add_change_listener("count", callback)
def callback2(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
if TXT_M_C2_motor_step_counter.get_count() >= 0:
counters[1] = TXT_M_C2_motor_step_counter.get_count()
display.set_attr("txt_label_counters.text", str(counters))
@@ -123,7 +128,7 @@ TXT_M_C2_motor_step_counter.add_change_listener("count", callback2)
def callback3(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
if TXT_M_C3_motor_step_counter.get_count() >= 0:
counters[2] = TXT_M_C3_motor_step_counter.get_count()
display.set_attr("txt_label_counters.text", str(counters))
@@ -133,7 +138,7 @@ TXT_M_C3_motor_step_counter.add_change_listener("count", callback3)
def callback4(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
if TXT_M_C4_motor_step_counter.get_count() >= 0:
counters[3] = TXT_M_C4_motor_step_counter.get_count()
display.set_attr("txt_label_counters.text", str(counters))
@@ -143,7 +148,7 @@ TXT_M_C4_motor_step_counter.add_change_listener("count", callback4)
def callback5(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
if TXT_M_I8_photo_transistor.is_dark():
shotBall()
time.sleep(5)
@@ -153,7 +158,7 @@ TXT_M_I8_photo_transistor.add_change_listener("dark", callback5)
def shotBall():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'shotBall'
TXT_M_S1_servomotor.set_position(int(400))
TXT_M_M1_encodermotor.set_speed(int(512), Motor.CCW)
@@ -180,7 +185,7 @@ def shotBall():
def soundGoal():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'soundGOAL'
TXT_M.get_loudspeaker().play("26_Augenzwinkern.wav", False)
display.set_attr("txt_switch_active.checked", str(False).lower())
@@ -191,7 +196,7 @@ def soundGoal():
def turnSideLeft(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'turnSideLeft'
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CCW)
@@ -207,7 +212,7 @@ def turnSideLeft(v, ticks):
def turnSideRight(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'turnSideRight'
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CW)
@@ -223,7 +228,7 @@ def turnSideRight(v, ticks):
def fwd(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'fwd'
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
@@ -238,7 +243,7 @@ def fwd(v, ticks):
def bwd(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'bwd'
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
@@ -253,7 +258,7 @@ def bwd(v, ticks):
def left(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'left ({}, {})'.format(v, ticks)
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
@@ -268,7 +273,7 @@ def left(v, ticks):
def right(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, goal_width, posx_goal_both, search_dir_left
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, cmd, state, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, counters, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, v_slow, y_ball_far, v_var_ball_ypos, v_fast, y_ball_touch, y_ball_near, search_dir_left
cmd = 'right ({}, {})'.format(v, ticks)
if ticks >= 1:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
@@ -320,23 +325,21 @@ while True:
if posx_ball > posx_goal_left and posx_ball < posx_goal_right:
stepBall()
elif posx_ball < posx_goal_both:
- turnSideRight(v_slow, 20)
- elif posx_ball > posx_goal_both:
turnSideLeft(v_slow, 20)
+ elif posx_ball > posx_goal_both:
+ turnSideRight(v_slow, 20)
elif posx_goal_left != None and posx_goal_right == None:
state = 'BALL_GOAL_LEFT'
- turnSideRight(v_slow, 20)
+ turnSideLeft(v_slow, 20)
elif posx_goal_left == None and posx_goal_right != None:
state = 'BALL_GOAL_RIGHT'
- turnSideLeft(v_slow, 20)
+ turnSideRight(v_slow, 20)
else:
state = 'BALL_NO_GOAL'
stepBall()
else:
if posx_goal_left != None and posx_goal_right != None:
state = 'NO_BALL_GOAL_BOTH'
- goal_width = posx_goal_right - posx_goal_left
- posx_goal_both = posx_goal_left + goal_width / 2
if posx_goal_both >= x_min and posx_goal_both <= x_max:
if goal_width < 130:
fwd(v_slow, 10)
@@ -345,24 +348,25 @@ while True:
else:
pass
elif posx_goal_both < x_min:
- left(v_slow, round(goal_width * 0.01))
+ left(v_slow, 10)
elif posx_goal_both > x_max:
- right(v_slow, round(goal_width * 0.01))
+ right(v_slow, 10)
else:
pass
elif posx_goal_left != None and posx_goal_right == None:
state = 'NO_BALL_GOAL_LEFT'
- right(v_slow, round(math.fabs(posx_goal_left * 0.1)))
+ right(v_slow, 10)
elif posx_goal_left == None and posx_goal_right != None:
state = 'NO_BALL_GOAL_RIGHT'
- left(v_slow, round(math.fabs(posx_goal_right * 0.1)))
+ left(v_slow, 10)
else:
state = 'NONE'
+ cmd = 'NONE'
display.set_attr("txt_label_ball.text", str('x:{}, y:{}'.format(posx_ball, posy_ball)))
display.set_attr("txt_label_goal.text", str('left:{} {}, right:{} {}, p:{}, w:{}'.format(posx_goal_left, posy_goal_left, posx_goal_right, posy_goal_right, posx_goal_both, goal_width)))
display.set_attr("txt_label_counters.text", str('{}'.format(counters)))
print('{}:{} GOAL left: {} {}, right:{} {}, p:{}, w:{} BALL_pos:{},{} v_x:{} v_y:{}'.format(state, cmd, posx_goal_left, posy_goal_left, posx_goal_right, posy_goal_right, posx_goal_both, goal_width, posx_ball, posy_ball, v_var_ball_xpos, v_var_ball_ypos))
- checkTimeouts()
+ checkTimeoutsSetGoalBoth()
time.sleep(0.08)