Skip to content
Snippets Groups Projects
Commit 4e56bc5f authored by ft-Demo's avatar ft-Demo
Browse files

commit by robo pro coding

parent bb79cee2
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
omniwheels_x4_soccer_goal.blockly
+ 190
− 34
View file @ 4e56bc5f
This diff is collapsed.
omniwheels_x4_soccer_goal.py
+ 34
− 23
View file @ 4e56bc5f
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment