diff --git a/omniwheels_x4_soccer_goal.blockly b/omniwheels_x4_soccer_goal.blockly
index 4b311a89cab40399d83609df5d0cc8680f33e814..a1d2f68426909aef43e65715804024c266658f71 100644
--- a/omniwheels_x4_soccer_goal.blockly
+++ b/omniwheels_x4_soccer_goal.blockly
@@ -7,11 +7,11 @@
<variable id=";Ki:[qYaZnowqSa`]/aW">ts_goal_left</variable>
<variable id="z^z{Ev+IMfS6[W1raY;l">ts_goal_right</variable>
<variable id="S~)Q:X5BHfQZe.ZJfdE)">factor_y</variable>
- <variable id="AnbD2s`i`niJf7bj:WR.">dist_x</variable>
<variable id="S4.mduNQC(va3iR{oSdB">posx_ball</variable>
+ <variable id="AnbD2s`i`niJf7bj:WR.">dist_x</variable>
<variable id=";S3?%3)**s[[l~Aq3*,{">speed_y</variable>
- <variable id="=2ARo+;2DS07SU~j29Fk">posx_goal</variable>
<variable id="q#H`+Gwt~43Dh@fu;G-D">posy_ball</variable>
+ <variable id="=2ARo+;2DS07SU~j29Fk">posx_goal</variable>
<variable id="Ym/+Qo0YZe25;%N1d!*f">x_min</variable>
<variable id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</variable>
<variable id="}*jl@)UA_Hjh5rc,Gt)c">y_min</variable>
@@ -157,7 +157,7 @@
</value>
<statement name="DO0">
<block type="controls_if" id="MN`F(]itK#DR^wQxXz,.">
- <mutation elseif="0" else="1"/>
+ <mutation elseif="1" else="1"/>
<value name="IF0">
<block type="logic_operation" id="QKmOZD,=Y4S|jqiAJ4.k" inline="false">
<field name="OP">AND</field>
@@ -250,6 +250,162 @@
</block>
</value>
<statement name="DO0">
+ <block type="controls_if" id="KJ#o$M|scI;Nb]boFDzc">
+ <mutation elseif="3" else="1"/>
+ <value name="IF0">
+ <block type="logic_compare" id="Hvw%^q~Hu5/N%}1^$97E">
+ <field name="OP">LT</field>
+ <value name="A">
+ <block type="variables_get" id="X1^+LJneX7N!jy6J]@;v">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id=")Y^E1(LwkP6u5Zr]E#v/">
+ <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO0">
+ <block type="procedures_callnoreturn" id="/Dyv@Uq8HyBO`zJkGf%H" inline="true">
+ <mutation name="sideleft_v">
+ <arg name="v"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="math_number" id="0~3b0e(US%D-.mrk9B9]">
+ <field name="NUM">180</field>
+ </block>
+ </value>
+ </block>
+ </statement>
+ <value name="IF1">
+ <block type="logic_compare" id="nUbsUlJ45(hnk!)6CNAr">
+ <field name="OP">GT</field>
+ <value name="A">
+ <block type="variables_get" id="$Gcp.[t#1r{E5q/q(8nE">
+ <field name="VAR" id="=2ARo+;2DS07SU~j29Fk">posx_goal</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="M9wjV+z*e*Hh*PspIXEY">
+ <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO1">
+ <block type="procedures_callnoreturn" id="$a(P8}p#iy{V.L*Qd5S-" inline="true">
+ <mutation name="sideright_v">
+ <arg name="v"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="math_number" id="D}g?6A6[y!i,A_?rI7vT">
+ <field name="NUM">180</field>
+ </block>
+ </value>
+ </block>
+ </statement>
+ <value name="IF2">
+ <block type="logic_compare" id="J%?mBh)[A7BsM@RD[LL5">
+ <field name="OP">LT</field>
+ <value name="A">
+ <block type="variables_get" id="pM-ticxUIK#qr]1woJ~P">
+ <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="wrIeSTCCcbSMCRhVhaRo">
+ <field name="VAR" id="Ym/+Qo0YZe25;%N1d!*f">x_min</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO2">
+ <block type="procedures_callnoreturn" id="dkT`%q5FBeJ{iKupE~$b" inline="true">
+ <mutation name="left">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="geK2^Cu@:u4qgd~m0Nf,">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
+ </block>
+ </value>
+ <value name="ARG1">
+ <block type="variables_get" id="ooFsLNRl;=i~B=`Qp:SK">
+ <field name="VAR" id="AnbD2s`i`niJf7bj:WR.">dist_x</field>
+ </block>
+ </value>
+ </block>
+ </statement>
+ <value name="IF3">
+ <block type="logic_compare" id="|uy*q}0w4)yxY`=hg=Qx">
+ <field name="OP">GT</field>
+ <value name="A">
+ <block type="variables_get" id="PWIj/MW+jX3VAU7:T-T)">
+ <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="variables_get" id="3KF;#Z~Ft^8{dfQ-K0-1">
+ <field name="VAR" id="2G]XEQ3Lhl$dG]=:]8Ge">x_max</field>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO3">
+ <block type="procedures_callnoreturn" id="SF5C]WGRrZa8/]-iDIyL" inline="true">
+ <mutation name="right">
+ <arg name="v"/>
+ <arg name="ticks"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="5?@ol7,I#`PPey,AEdJN">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
+ </block>
+ </value>
+ <value name="ARG1">
+ <block type="variables_get" id="Y[BTA;4/ZM,mW{P}X}rz">
+ <field name="VAR" id="AnbD2s`i`niJf7bj:WR.">dist_x</field>
+ </block>
+ </value>
+ </block>
+ </statement>
+ </block>
+ </statement>
+ <value name="IF1">
+ <block type="logic_operation" id="Bv(kvR*xIzb%RfmRxg3N" inline="false">
+ <field name="OP">AND</field>
+ <value name="A">
+ <block type="logic_compare" id="?#JnnyzAeNU`MJL^+ZOa">
+ <field name="OP">NEQ</field>
+ <value name="A">
+ <block type="variables_get" id="vft:66IZzutoOz+ig;R=">
+ <field name="VAR" id="S4.mduNQC(va3iR{oSdB">posx_ball</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="logic_null" id="f)RPYpZoWG5Mz!bJ?IKJ"/>
+ </value>
+ </block>
+ </value>
+ <value name="B">
+ <block type="logic_compare" id="muiUUW_{}49WVL]?fJ8F">
+ <field name="OP">NEQ</field>
+ <value name="A">
+ <block type="variables_get" id="{RI1+Kc:gz]:()FPOMRR">
+ <field name="VAR" id="q#H`+Gwt~43Dh@fu;G-D">posy_ball</field>
+ </block>
+ </value>
+ <value name="B">
+ <block type="logic_null" id="7%fYG^;I?g7^n6Vy{BV5"/>
+ </value>
+ </block>
+ </value>
+ </block>
+ </value>
+ <statement name="DO1">
<block type="controls_if" id="KvGY3!$zq%]{SYcSc[7m">
<mutation elseif="3" else="1"/>
<value name="IF0">
@@ -429,22 +585,22 @@
</value>
</block>
</statement>
- <statement name="ELSE">
- <block type="procedures_callnoreturn" id="~e,9WDXasUar9~[eQ#bq" inline="true">
- <mutation name="left_v">
- <arg name="v"/>
- </mutation>
- <value name="ARG0">
- <block type="variables_get" id="a6~lQO=iKTa%P$.9vwF`">
- <field name="VAR" id=";S3?%3)**s[[l~Aq3*,{">speed_y</field>
- </block>
- </value>
- </block>
- </statement>
</block>
</statement>
</block>
</statement>
+ <statement name="ELSE">
+ <block type="procedures_callnoreturn" id="~e,9WDXasUar9~[eQ#bq" inline="true">
+ <mutation name="left_v">
+ <arg name="v"/>
+ </mutation>
+ <value name="ARG0">
+ <block type="variables_get" id="a6~lQO=iKTa%P$.9vwF`">
+ <field name="VAR" id=")rK_fR,m=]b(j[{.CtBj">v_slow</field>
+ </block>
+ </value>
+ </block>
+ </statement>
</block>
</statement>
<next>
@@ -545,7 +701,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="N2EC5uK:$~0ERXAdK0!y" x="0" y="1595">
+ <block type="procedures_defnoreturn" id="N2EC5uK:$~0ERXAdK0!y" x="0" y="2039">
<field name="NAME">checkTimeouts</field>
<statement name="STACK">
<block type="controls_if" id="o.;j7*F0}TW}ju0E18(_">
@@ -691,7 +847,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="dgC*A|KHMC_R5S#ZRk}d" x="0" y="2048">
+ <block type="camera_on_ball_detected" id="dgC*A|KHMC_R5S#ZRk}d" x="0" y="2492">
<field name="instance_name">ball_detector</field>
<statement name="statement">
<block type="variables_set" id="=2/,IQfcLO{_5PNd3UK~">
@@ -865,7 +1021,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="Rg2Gt(0WT:J2|Us~[j.3" x="0" y="2609">
+ <block type="camera_on_ball_detected" id="Rg2Gt(0WT:J2|Us~[j.3" x="0" y="3053">
<field name="instance_name">ball_detector_goal_left</field>
<statement name="statement">
<block type="variables_set" id="kvK,D4e%04[?+!jq*Lch">
@@ -936,7 +1092,7 @@
</block>
</statement>
</block>
- <block type="camera_on_ball_detected" id="(?slhGsN^:d1`vP{j9#|" x="0" y="2870">
+ <block type="camera_on_ball_detected" id="(?slhGsN^:d1`vP{j9#|" x="0" y="3314">
<field name="instance_name">ball_detector_goal_right</field>
<statement name="statement">
<block type="variables_set" id="0!1?#@4|tcM8+U6E[$Tl">
@@ -1007,7 +1163,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="/pGK)WReTLfX`L_7oAMD" x="0" y="3131">
+ <block type="procedures_defnoreturn" id="/pGK)WReTLfX`L_7oAMD" x="0" y="3575">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="{M*n_!I)eN0.4..0Bzo#"/>
</mutation>
@@ -1035,7 +1191,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="qX~e_RBh^tj];fOb=cq6" x="0" y="3352">
+ <block type="procedures_defnoreturn" id="qX~e_RBh^tj];fOb=cq6" x="0" y="3796">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid="{M*n_!I)eN0.4..0Bzo#"/>
</mutation>
@@ -1063,7 +1219,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="!zD7YDZ.5n?En8cYle4R" x="0" y="3573">
+ <block type="procedures_defnoreturn" id="!zD7YDZ.5n?En8cYle4R" x="0" y="4017">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid=",q%qjDTJ~Z@[BoMwORuq"/>
</mutation>
@@ -1091,7 +1247,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="dhS+rZWE[`RZz:fU3A)e" x="0" y="3794">
+ <block type="procedures_defnoreturn" id="dhS+rZWE[`RZz:fU3A)e" x="0" y="4238">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid=",q%qjDTJ~Z@[BoMwORuq"/>
</mutation>
@@ -1119,7 +1275,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="}KXL0=RKLgCT~yQ];aw:" x="0" y="4015">
+ <block type="procedures_defnoreturn" id="}KXL0=RKLgCT~yQ];aw:" x="0" y="4459">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid=",q%qjDTJ~Z@[BoMwORuq"/>
</mutation>
@@ -1147,7 +1303,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id=")G33?J{l2sdl+nL3hpBp" x="0" y="4236">
+ <block type="procedures_defnoreturn" id=")G33?J{l2sdl+nL3hpBp" x="0" y="4680">
<mutation>
<arg name="v" varid="DfDz@$]?^P=(6r:%^xC*" argid=",q%qjDTJ~Z@[BoMwORuq"/>
</mutation>
@@ -1175,7 +1331,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="+FKt%DE[[q%Zp0T.Fn}d" x="0" y="4457">
+ <block type="procedures_defnoreturn" id="+FKt%DE[[q%Zp0T.Fn}d" x="0" y="4901">
<field name="NAME">stop</field>
<statement name="STACK">
<block type="encodermotor_stop" id="}]1#.xeDc*.k9aGX5P?4">
@@ -1187,7 +1343,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="oSa9Ly*^-c^M{$gkMZt1" x="0" y="4650">
+ <block type="procedures_defnoreturn" id="oSa9Ly*^-c^M{$gkMZt1" x="0" y="5094">
<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{"/>
@@ -1253,7 +1409,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="G@]xamrhDPW$7uQ6oPo#" x="0" y="5007">
+ <block type="procedures_defnoreturn" id="G@]xamrhDPW$7uQ6oPo#" x="0" y="5451">
<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"/>
@@ -1319,7 +1475,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="5Hv=PFtVvPQA=IZrkJfU" x="0" y="5364">
+ <block type="procedures_defnoreturn" id="5Hv=PFtVvPQA=IZrkJfU" x="0" y="5808">
<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"/>
@@ -1385,7 +1541,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="[S@[MO!Z/cNt:Jsg4dd}" x="0" y="5721">
+ <block type="procedures_defnoreturn" id="[S@[MO!Z/cNt:Jsg4dd}" x="0" y="6165">
<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#"/>
@@ -1451,7 +1607,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="UFM0X7|X:*ja#f+^7kz;" x="0" y="6078">
+ <block type="procedures_defnoreturn" id="UFM0X7|X:*ja#f+^7kz;" x="0" y="6522">
<field name="NAME">turnLeft</field>
<statement name="STACK">
<block type="procedures_callnoreturn" id="n2^-(/I;nyLq_t.8g0Hh" inline="true">
@@ -1500,7 +1656,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="$nY7HNz^i}wlpk/f(Dja" x="0" y="6263">
+ <block type="procedures_defnoreturn" id="$nY7HNz^i}wlpk/f(Dja" x="0" y="6707">
<field name="NAME">turnRight</field>
<statement name="STACK">
<block type="procedures_callnoreturn" id="%_T=@p6r8~Uq5{@hAFr2" inline="true">
@@ -1549,7 +1705,7 @@
</block>
</statement>
</block>
- <block type="input_event" id="j(TuhJ0G}{^qGSq:26FY" x="0" y="6448">
+ <block type="input_event" id="j(TuhJ0G}{^qGSq:26FY" x="0" y="6892">
<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>
@@ -1562,7 +1718,7 @@
</block>
</statement>
</block>
- <block type="procedures_defnoreturn" id="RyK*td9aN5aR;@gw4f)+" x="0" y="6557">
+ <block type="procedures_defnoreturn" id="RyK*td9aN5aR;@gw4f)+" x="0" y="7001">
<field name="NAME">shotBall</field>
<statement name="STACK">
<block type="servomotor_set_position" id="9C*rGH2+bsy8H4Xzz#gk">
diff --git a/omniwheels_x4_soccer_goal.py b/omniwheels_x4_soccer_goal.py
index 88d1b322c412a592925c88b4604978c430c07407..148307325597526105b3af86eaced78c6befff3a 100644
--- a/omniwheels_x4_soccer_goal.py
+++ b/omniwheels_x4_soccer_goal.py
@@ -14,11 +14,11 @@ ts_ball = None
ts_goal_left = None
ts_goal_right = None
factor_y = None
-dist_x = None
posx_ball = None
+dist_x = None
speed_y = None
-posx_goal = None
posy_ball = None
+posx_goal = None
x_min = None
x_max = None
y_min = None
@@ -26,7 +26,7 @@ y_max = None
v_slow = None
v_fast = None
def checkTimeouts():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if (time.time() * 1000) - ts_ball > 500:
posx_ball = None
posy_ball = None
@@ -38,7 +38,7 @@ def checkTimeouts():
def ball_callback(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
ts_ball = (time.time() * 1000)
dist_x = round(math.fabs(event.value.x) * factor_x)
speed_y = round(min(max(math.fabs(event.value.y) * factor_y + v_slow, 0), 512))
@@ -50,7 +50,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, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
ts_goal_left = (time.time() * 1000)
posx_goal[0] = event.value.x
print('goal left: posx:{} posy:{}'.format(event.value.x, event.value.y))
@@ -59,7 +59,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, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
ts_goal_right = (time.time() * 1000)
posx_goal[1] = event.value.x
print('goal right: posx:{} posy:{}'.format(event.value.x, event.value.y))
@@ -68,7 +68,7 @@ ball_detector_goal_right.add_detection_listener(ball_callback23)
def fwd_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
@@ -77,7 +77,7 @@ def fwd_v(v):
def bwd_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
@@ -86,7 +86,7 @@ def bwd_v(v):
def left_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
@@ -95,7 +95,7 @@ def left_v(v):
def sideleft_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
@@ -104,7 +104,7 @@ def sideleft_v(v):
def right_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
@@ -113,7 +113,7 @@ def right_v(v):
def sideright_v(v):
- global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
@@ -122,12 +122,12 @@ def sideright_v(v):
def stop():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_M1_encodermotor.stop_sync(TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
def fwd(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if ticks > 0:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
@@ -141,7 +141,7 @@ def fwd(v, ticks):
def bwd(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if ticks > 0:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
@@ -155,7 +155,7 @@ def bwd(v, ticks):
def left(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if ticks > 0:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
@@ -169,7 +169,7 @@ def left(v, ticks):
def right(v, ticks):
- global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if ticks > 0:
TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
@@ -183,21 +183,21 @@ def right(v, ticks):
def turnLeft():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
left(512, 100)
time.sleep(2)
right(512, 100)
def turnRight():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
right(512, 100)
time.sleep(2)
left(512, 100)
def callback(event):
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
if TXT_M_I8_photo_transistor.is_dark():
shotBall()
@@ -206,7 +206,7 @@ TXT_M_I8_photo_transistor.add_change_listener("dark", callback)
def shotBall():
- global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, dist_x, posx_ball, speed_y, posx_goal, posy_ball, x_min, x_max, y_min, y_max, v_slow, v_fast
+ global v, ticks, factor_x, ts_ball, ts_goal_left, ts_goal_right, factor_y, posx_ball, dist_x, speed_y, posy_ball, posx_goal, x_min, x_max, y_min, y_max, v_slow, v_fast
TXT_M_S1_servomotor.set_position(int(400))
TXT_M_M1_encodermotor.set_speed(int(512), Motor.CCW)
TXT_M_M2_encodermotor.set_speed(int(512), Motor.CCW)
@@ -254,6 +254,17 @@ while True:
checkTimeouts()
if display.get_attr("txt_switch_active.checked"):
if posx_ball != None and posy_ball != None and posx_goal[0] != None and posx_goal[1] != None:
+ if posx_goal < posx_ball:
+ sideleft_v(180)
+ elif posx_goal > x_max:
+ sideright_v(180)
+ elif posx_ball < x_min:
+ left(v_slow, dist_x)
+ elif posx_ball > x_max:
+ right(v_slow, dist_x)
+ else:
+ pass
+ elif posx_ball != None and posy_ball != None:
if posx_goal < x_min:
sideleft_v(180)
elif posx_goal > x_max:
@@ -268,9 +279,9 @@ while True:
elif posy_ball < y_max:
fwd_v(speed_y)
else:
- left_v(speed_y)
+ pass
else:
- pass
+ left_v(v_slow)
display.set_attr("txt_label_ball.text", str('x:{}, y:{}'.format(posx_ball, posy_ball)))
display.set_attr("txt_label_goal.text", str('x:{}'.format(posx_goal)))
time.sleep(0.1)