commit by robo pro coding

omniwheels_x4_soccer_goal.py

@@ -26,8 +26,8 @@ posx_goal_both = None
 posy_ball = None
 posy_goal_left = None
 posy_goal_right = None
-x_max = None
 v_slow = None
+x_max = None
 v_var_ball_xpos = None
 y_ball_far = None
 v_fast = None
@@ -35,8 +35,9 @@ y_ball_touch = None
 v_var_ball_ypos = None
 y_ball_near = None
 search_dir_left = None
+ts_deltas = None
 def checkTimeoutsSetGoalBoth():
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     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
@@ -55,8 +56,25 @@ def checkTimeoutsSetGoalBoth():
 
 
 
+def stepOnlyGoal():
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
+    if posx_goal_both >= x_min and posx_goal_both <= x_max:
+        if goal_width < 130:
+            fwd(v_slow, 10)
+        elif goal_width > 40:
+            bwd(v_slow, 10)
+        else:
+            pass
+    elif posx_goal_both < x_min:
+        left(v_slow, 10)
+    elif posx_goal_both > x_max:
+        right(v_slow, 10)
+    else:
+        pass
+
+
 def stepBall(shot):
-    global v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     state = 'stepBall'
     if posx_ball < x_min:
         left(v_slow, round(math.fabs(posx_ball) * 0.07))
@@ -78,7 +96,7 @@ def stepBall(shot):
 
 
 def ball_callback(event):
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     ts_ball = (time.time() * 1000)
     posx_ball = event.value.x
     posy_ball = event.value.y
@@ -91,7 +109,7 @@ ball_detector.add_detection_listener(ball_callback)
 
 
 def ball_callback2(event):
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     ts_goal_left = (time.time() * 1000)
     posx_goal_left = event.value.x
     posy_goal_left = event.value.y
@@ -102,7 +120,7 @@ ball_detector_goal_left.add_detection_listener(ball_callback2)
 
 
 def ball_callback23(event):
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     ts_goal_right = (time.time() * 1000)
     posx_goal_right = event.value.x
     posy_goal_right = event.value.y
@@ -113,7 +131,7 @@ ball_detector_goal_right.add_detection_listener(ball_callback23)
 
 
 def callback(event):
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     if TXT_M_I8_photo_transistor.is_dark():
         shotBall()
         time.sleep(5)
@@ -123,7 +141,7 @@ TXT_M_I8_photo_transistor.add_change_listener("dark", callback)
 
 
 def shotBall():
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'shotBall'
     TXT_M_S1_servomotor.set_position(int(400))
     TXT_M_M1_encodermotor.set_speed(int(512), Motor.CCW)
@@ -150,7 +168,7 @@ def shotBall():
 
 
 def soundGoal():
-    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'soundGOAL'
     TXT_M.get_loudspeaker().play("26_Augenzwinkern.wav", False)
     display.set_attr("txt_switch_ball.checked", str(False).lower())
@@ -161,7 +179,7 @@ def soundGoal():
 
 
 def turnSideLeft(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'turnSideLeft'
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CCW)
@@ -177,7 +195,7 @@ def turnSideLeft(v, ticks):
 
 
 def turnSideRight(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'turnSideRight'
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CW)
@@ -193,7 +211,7 @@ def turnSideRight(v, ticks):
 
 
 def fwd(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'fwd'
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
@@ -208,7 +226,7 @@ def fwd(v, ticks):
 
 
 def bwd(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'bwd'
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
@@ -223,7 +241,7 @@ def bwd(v, ticks):
 
 
 def left(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'left ({}, {})'.format(v, ticks)
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
@@ -238,7 +256,7 @@ def left(v, ticks):
 
 
 def right(v, ticks):
-    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_slow, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left
+    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, v_slow, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, search_dir_left, ts_deltas
     cmd = 'right ({}, {})'.format(v, ticks)
     if ticks >= 1:
         TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
@@ -280,6 +298,9 @@ cmd = 'NONE'
 time.sleep(2)
 while True:
     if display.get_attr("txt_switch_ball.checked"):
+        display.set_attr("txt_switch_goal.checked", str(False).lower())
+        state = 'NONE'
+        cmd = 'NONE'
         if posx_ball != None and posy_ball != None:
             if posx_goal_left != None and posx_goal_right != None:
                 state = 'BALL_GOAL_BOTH'
@@ -294,9 +315,11 @@ while True:
                         turnSideRight(v_slow, 20)
             elif posx_goal_left != None and posx_goal_right == None:
                 state = 'BALL_GOAL_LEFT'
+                right(v_slow, 10)
                 turnSideLeft(v_slow, 10)
             elif posx_goal_left == None and posx_goal_right != None:
                 state = 'BALL_GOAL_RIGHT'
+                left(v_slow, 10)
                 turnSideRight(v_slow, 10)
             else:
                 state = 'BALL_NO_GOAL'
@@ -304,19 +327,7 @@ while True:
         else:
             if posx_goal_left != None and posx_goal_right != None:
                 state = 'NO_BALL_GOAL_BOTH'
-                if posx_goal_both >= x_min and posx_goal_both <= x_max:
-                    if goal_width < 130:
-                        fwd(v_slow, 10)
-                    elif goal_width > 40:
-                        bwd(v_slow, 10)
-                    else:
-                        pass
-                elif posx_goal_both < x_min:
-                    left(v_slow, 10)
-                elif posx_goal_both > x_max:
-                    right(v_slow, 10)
-                else:
-                    pass
+                left(v_slow, 10)
             elif posx_goal_left != None and posx_goal_right == None:
                 state = 'NO_BALL_GOAL_LEFT'
                 right(v_slow, 10)
@@ -325,7 +336,10 @@ while True:
                 left(v_slow, 10)
             else:
                 state = 'NONE'
-                cmd = 'NONE'
+                left(v_slow, 10)
+    elif display.get_attr("txt_switch_goal.checked"):
+        display.set_attr("txt_switch_ball.checked", str(False).lower())
+        stepOnlyGoal()
     display.set_attr("txt_status_indicator_ball.active", str(posx_ball != None).lower())
     display.set_attr("txt_status_indicator_goal_left.active", str(posx_goal_left != None).lower())
     display.set_attr("txt_status_indicator_goal_right.active", str(posx_goal_right != None).lower())
@@ -333,8 +347,9 @@ while True:
     display.set_attr("txt_label_goal_left.text", str('{} {}'.format(posx_goal_left, posy_goal_left)))
     display.set_attr("txt_label_goal_right.text", str('{} {}'.format(posx_goal_right, posy_goal_right)))
     display.set_attr("txt_label_goal_both.text", str('p:{}, w:{}'.format(posx_goal_both, goal_width)))
-    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))
+    ts_deltas = [(time.time() * 1000) - ts_ball, (time.time() * 1000) - ts_goal_left, (time.time() * 1000) - ts_goal_right]
+    print('{:.1f} {:.1f} {:.1f} {}:{}  GOAL left: {} {}, right:{} {}, p:{}, w:{} BALL_pos:{},{} v_x:{} v_y:{}'.format(ts_deltas[0], ts_deltas[1], ts_deltas[2], 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))
     checkTimeoutsSetGoalBoth()
-    time.sleep(0.08)
+    time.sleep(min(ts_deltas) / 1000)