def run(self): frameCounter = 0 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles({ 'x': 0.75, 'y': 0 }, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise self.left_motor.setVelocity(1) self.right_motor.setVelocity(-1)
def run(self): while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise if direction == 0: left_speed = 5 right_speed = 5 else: left_speed = direction * 4 right_speed = direction * -4 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed)
def run(self): if self.name[0] == "B": goalDir = -1 else: goalDir = 1 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] goToPos = utils.calculateGBRLine(ball_pos, goalDir) # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(goToPos, robot_pos) # If the robot has the ball right in front of it, go forward, # rotate otherwise if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 8 right_speed = direction * -8 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed)
def moveTo(x, y): robot_angle_2 = robot_pos['orientation'] angle = math.atan2( y - robot_pos['y'], x - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if robot_angle_2 < 0: robot_angle_2 = 2 * math.pi + robot_angle_2 angle2 = math.degrees(angle + robot_angle_2) angle2 -= 90 if angle2 > 360: angle2 -= 360 d2 = utils.get_direction(angle2) return d2
def run(self): while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise #if direction == 0: # left_speed = -5 # right_speed = -5 #else: # left_speed = direction * 4 # right_speed = direction * -4 real_robot_angle = robot_angle*57 robot_x = robot_pos["x"]*100 robot_y = robot_pos["y"]*100 ball_x = ball_pos["x"]*100 ball_y = ball_pos["y"]*100 brana_x = -75 def get_angles(pos1_x, pos1_y): if polovica == 1: pos2_x = -75 else: pos2_x = 75 pos2_y = 0 if pos1_y < 0: pos1_y = pos1_y * (-1) if pos1_x < 0: pos1_x = pos1_x * (-1) a = pos2_x - pos1_x b = pos2_y - pos1_y c = math.sqrt(a**2 + b**2) global alfa alfa = math.degrees(math.asin(a / c)) beta = math.degrees(math.asin(b / c)) gama = 90 return beta def zisti_ci_mas_loptu(): if polovica == 1: if robot_x < ball_x and robot_x + 5 > ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y: return 1 else: return 0 else: if robot_x > ball_x and robot_x - 5 < ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y: return 1 else: return 0 def navigacia(): if robot_y > 0: cielovy_uhol = 90 + get_angles(robot_x, robot_y) else: cielovy_uhol = 90 - get_angles(robot_x, robot_y) global vr, vl if real_robot_angle + 4 >= cielovy_uhol and real_robot_angle - 4 <= cielovy_uhol: vr = 10 vl = 10 else: if real_robot_angle < cielovy_uhol: vr = -5 vl = 5 else: vr = 5 vl = -5 global a if a == 0: if robot_x < 0: polovica = -1 else: polovica = 1 a = 1 if polovica == 1: if robot_x < 20: if zisti_ci_mas_loptu() == 0: if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 10 right_speed = direction * -10 else: print("mamy2") if ball_y > -10 and ball_y < 10: navigacia() left_speed = vl right_speed = vr else: if ball_y < 0: left_speed = -5 right_speed = -10 else: left_speed = -10 right_speed = -5 else: if real_robot_angle <= 274: if real_robot_angle >= 264: left_speed = -10 right_speed = -10 else: left_speed = 3 right_speed = -3 else: left_speed = -3 right_speed = 3 else: if robot_x > -25: if zisti_ci_mas_loptu() == 0: if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 10 right_speed = direction * -10 else: if ball_y > -10 and ball_y < 10: navigacia() left_speed = vl right_speed = vr else: if ball_y < 0: left_speed = -5 right_speed = -10 else: left_speed = -10 right_speed = -5 else: if real_robot_angle <= 274: if real_robot_angle >= 264: left_speed = 10 right_speed = 10 else: left_speed = 3 right_speed = -3 else: left_speed = -3 right_speed = 3 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed)
def evanMethod(self): moving = True shooting = False ball_moving = False ball_pos_last = [0, 0] waiting_for_ball = False team = -1 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.name[0] == 'B': team = 1 if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] if not (ball_pos['x'] == ball_pos_last[0]) and not ( ball_pos['y'] == ball_pos_last[0]): ball_moving = True ball_change_x = ball_pos['x'] - ball_pos_last[0] ball_change_y = ball_pos['y'] - ball_pos_last[1] # print(ball_change_x, ball_change_y) robot_angle_2 = robot_pos['orientation'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise rx = robot_pos['x'] ry = robot_pos['y'] bx = ball_pos['x'] by = ball_pos['y'] if team == 1: ball_x_dist_from_goal = abs(bx + 0.75) else: ball_x_dist_from_goal = bx - 0.75 ball_y_dist_from_goal = by if team == 1: if (rx - 0.05 < bx): moving = True shooting = False elif (rx + 0.05 > bx): moving = True shooting = False # print("byd"+str(ball_y_dist_from_goal)) # shotx = bx + 0.15/math.sqrt(ball_x_dist_from_goal**2+ball_y_dist_from_goal**2)*ball_x_dist_from_goal + 20*ball_change_x shotx = bx + 0.2 / math.sqrt( ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2 ) * ball_x_dist_from_goal + 10 * ball_change_x shoty = by + 0.2 / math.sqrt( ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2 ) * ball_y_dist_from_goal + 23 * ball_change_y if shoty > 0.65: shoty = shoty = by + 0.2 / math.sqrt( ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2 ) * ball_y_dist_from_goal + 23 * ball_change_y if shoty > 0.65: shoty = 0.65 if shoty < -0.65: shoty = -0.65 if shotx > 0.65: shotx = 0.60 if shotx < -0.65: shotx = -0.60 # print(ball_x_dist_from_goal) # print(shotx, shoty) #if not on wall or not behind robot and compensate for movement, improve shooting mechanism (focusing on center of goal instead of ball) xtarget = shotx ytarget = shoty if team == 1: if xtarget > 0.2: xtarget = 0.2 ytarget = 0 else: if xtarget < -0.2: xtarget = -0.2 ytarget = 0 ball_pos = data['ball'] xb = ball_pos['x'] yb = ball_pos['y'] b1 = data[self.name] bX = b1['x'] bY = b1['y'] bdist = math.sqrt((bX - xb)**2 + (bY - yb)**2) # if bdist < 0.1: # xtarget = 0.2 # ytarget = 0 # TO KEEP THE ATTACKER ON OFFENSE # if (xtarget > 0.2 and bdist > 0.1) or xtarget > 0.4: # xtarget = 0.12 # ytarget = 0 # def moveTo(x, y): robot_angle_2 = robot_pos['orientation'] angle = math.atan2( y - robot_pos['y'], x - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if robot_angle_2 < 0: robot_angle_2 = 2 * math.pi + robot_angle_2 angle2 = math.degrees(angle + robot_angle_2) angle2 -= 90 if angle2 > 360: angle2 -= 360 d2 = utils.get_direction(angle2) return d2 if moving: sp = moveTo(xtarget, ytarget) if abs(xtarget - rx) < 0.02 and abs(ytarget - ry) < 0.02: moving = False # print("DONE") shooting = True direction = sp if shooting: if team == 1: angle = math.atan2( -robot_pos['y'], -0.75 - robot_pos['x'], ) else: angle = math.atan2( -robot_pos['y'], 0.75 - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if robot_angle_2 < 0: robot_angle_2 = 2 * math.pi + robot_angle_2 angle2 = math.degrees(angle + robot_angle_2) angle2 -= 90 if angle2 > 360: angle2 -= 360 direction = utils.get_direction(angle2) if direction == 0: left_speed = -10 right_speed = -10 else: right_speed = direction * -10 left_speed = direction * 10 self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed) ball_pos_last = [ball_pos['x'], ball_pos['y']]
def run(self): while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise #if direction == 0: # left_speed = -5 # right_speed = -5 #else: # left_speed = direction * 4 # right_speed = direction * -4 real_robot_angle = robot_angle * 57 robot_x = robot_pos["x"] * 100 robot_y = robot_pos["y"] * 100 ball_x = ball_pos["x"] * 100 ball_y = ball_pos["y"] * 100 #superova brana je x 70 #a y je 15 az -15 def buduce_suradnice_lopty(): global buduceX, buduceY, loptaX, loptaY, koeficient if len(loptaX) == 2: loptaX.pop(0) loptaY.pop(0) loptaX.append(ball_x) loptaY.append(ball_y) buduceX = loptaX[1] + ( (loptaX[1] - loptaX[0]) * koeficient) buduceY = loptaY[1] + ( (loptaY[1] - loptaY[0]) * koeficient) else: loptaX.append(ball_x) loptaY.append(ball_y) def get_angles(pos1_x, pos1_y): if polovica == 1: pos2_x = -75 else: pos2_x = 75 pos2_y = 0 if pos1_y < 0: pos1_y = pos1_y * (-1) if pos1_x < 0: pos1_x = pos1_x * (-1) a = pos2_x - pos1_x b = pos2_y - pos1_y c = math.sqrt(a**2 + b**2) global alfa alfa = math.degrees(math.asin(a / c)) beta = math.degrees(math.asin(b / c)) gama = 90 return beta def zisti_ci_mas_loptu(): if polovica == -1: if robot_x < ball_x and robot_x + 5 > ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y: return 1 else: return 0 else: if robot_x > ball_x and robot_x - 5 < ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y: return 1 else: return 0 def navigacia(): if robot_y > 0: cielovy_uhol = 90 + get_angles(robot_x, robot_y) else: cielovy_uhol = 90 - get_angles(robot_x, robot_y) global vr, vl if real_robot_angle + 4 >= cielovy_uhol and real_robot_angle - 4 <= cielovy_uhol: vr = 10 vl = 10 else: if real_robot_angle < cielovy_uhol: vr = -3 vl = 3 else: vr = 3 vl = -3 global a if a == 0: if robot_x < 0: polovica = -1 else: polovica = 1 start_time = time.time() a = 1 if ball_x > 75 or ball_x < -75: a = 0 if (time.time() - start_time) < 4: if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 5 right_speed = direction * -5 else: if polovica == 1: if robot_x < 20: if zisti_ci_mas_loptu() == 0: if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 10 right_speed = direction * -10 else: print("mamb3") if ball_y > -10 and ball_y < 10: navigacia() left_speed = vl right_speed = vr else: if ball_y < 0: left_speed = -5 right_speed = -10 else: left_speed = -10 right_speed = -5 else: if real_robot_angle <= 274: if real_robot_angle >= 264: left_speed = -10 right_speed = -10 else: left_speed = 3 right_speed = -3 else: left_speed = -3 right_speed = 3 else: if robot_x > -25: if zisti_ci_mas_loptu() == 0: if direction == 0: left_speed = -10 right_speed = -10 else: left_speed = direction * 10 right_speed = direction * -10 else: if ball_y > -10 and ball_y < 10: navigacia() left_speed = vl right_speed = vr else: if ball_y < 0: left_speed = -5 right_speed = -10 else: left_speed = -10 right_speed = -5 else: if real_robot_angle <= 274: if real_robot_angle >= 264: left_speed = 10 right_speed = 10 else: left_speed = 3 right_speed = -3 else: left_speed = -3 right_speed = 3 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed)
def run(self): frameCounter = 0 if self.name[0] == 'Y': t_m = 1 else: t_m = -1 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # # Compute the speed for motors direction = utils.get_direction(ball_angle) # # # If the robot has the ball right in front of it, go forward, # # rotate otherwise # # 0 forward, -1 right, 1 left # if direction == 0: # left_speed = -5 # right_speed = -5 # else: # left_speed = direction * 4 # right_speed = direction * -4 status = "" target_x = -0.07 idle_speed = 4 if robot_pos['y'] > -0.03 and robot_pos['y'] < 0.03: if ball_pos['y'] > -0.05 and ball_pos[ 'y'] < 0.05 and robot_pos['x'] * t_m < ball_pos[ 'x'] * t_m: if direction == 0: left_speed = -10 right_speed = -10 status = "charging" else: left_speed = direction * 4 right_speed = direction * -4 status = "aiming" else: if ball_pos['y'] < 0.3 and ball_pos[ 'y'] > -0.3 and robot_pos[ 'x'] * t_m < ball_pos['x'] * t_m: target_x = ball_pos['x'] - 0.08 idle_speed = 10 * t_m if robot_angle < ((math.pi / 2) - 0.05): left_speed = 2 right_speed = -2 status = "turning" elif robot_angle > ((math.pi / 2) + 0.05): left_speed = -2 right_speed = 2 status = "turning" else: if robot_pos['x'] * t_m < target_x: left_speed = -idle_speed * t_m right_speed = -idle_speed * t_m else: left_speed = idle_speed * t_m right_speed = idle_speed * t_m status = "idling" else: if robot_angle < (math.pi - 1): left_speed = 0 right_speed = 0 status = "turning back" elif robot_angle > (math.pi + 1): left_speed = 0 right_speed = 0 status = "turning back" else: left_speed = -5 right_speed = -5 status = "returning" # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed)
def evanMethod(self, ball_pos_last): moving = True shooting = False ball_moving = False waiting_for_ball = False xbOLD = 0 GETOUT = False team = -1 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.name[0] == 'B': team = 1 if self.is_new_data(): data = self.get_new_data() # Get the position of the ball ball_pos = data['ball'] xb = ball_pos['x'] if team == 1: if xb > 0.3: GETOUT = False else: GETOUT = True else: if xb < -0.3: GETOUT = False else: GETOUT = True xbOLD = xb # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] if not (ball_pos['x'] == ball_pos_last[0]) and not ( ball_pos['y'] == ball_pos_last[0]): ball_moving = True print("bl" + str(ball_pos_last[1])) ball_change_x = ball_pos['x'] - ball_pos_last[0] ball_change_y = ball_pos['y'] - ball_pos_last[1] # print(ball_change_x, ball_change_y) print("ball_change_y: " + str(ball_change_y)) # for x in range(10): robot_angle_2 = robot_pos['orientation'] # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise rx = robot_pos['x'] ry = robot_pos['y'] bx = ball_pos['x'] by = ball_pos['y'] if team == 1: ball_x_dist_from_goal = abs(bx + 0.75) else: ball_x_dist_from_goal = bx - 0.75 ball_y_dist_from_goal = by if team == 1: if (rx - 0.05 < bx): moving = True shooting = False elif (rx + 0.05 > bx): moving = True shooting = False # print("byd"+str(ball_y_dist_from_goal)) # shotx = bx + 0.15/math.sqrt(ball_x_dist_from_goal**2+ball_y_dist_from_goal**2)*ball_x_dist_from_goal + 20*ball_change_x shotx = bx shoty = by if shoty > -0.25 or shoty < 0.25 and shotx > 0.70 and shotx < -0.70: shoty = shoty = by + 0.2 / math.sqrt( ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2 ) * ball_y_dist_from_goal + 23 * ball_change_y if shoty > 0.33: shoty = 0.33 if shoty < -0.33: shoty = -0.33 if shotx > 0.70: shotx = 0.75 if shotx < -0.70: shotx = -0.75 print("shoty: " + str(shoty)) print("by" + str(by)) print("ry" + str(ry)) # print(ball_x_dist_from_goal) # print(shotx, shoty) #if not on wall or not behind robot and compensate for movement, improve shooting mechanism (focusing on center of goal instead of ball) xtarget = shotx ytarget = shoty def moveTo(x, y): robot_angle_2 = robot_pos['orientation'] angle = math.atan2( y - robot_pos['y'], x - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if robot_angle_2 < 0: robot_angle_2 = 2 * math.pi + robot_angle_2 angle2 = math.degrees(angle + robot_angle_2) angle2 -= 90 if angle2 > 360: angle2 -= 360 d2 = utils.get_direction(angle2) return d2 if moving: sp = moveTo(xtarget, ytarget) direction = sp if direction == 0: left_speed = -10 right_speed = -10 else: right_speed = direction * -10 left_speed = direction * 10 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed) ball_pos_last = [ball_pos['x'], ball_pos['y']] if GETOUT: break
def run(self): BLOCK = False SPOTONE = True SPOTTWO = False ROAMCLOSE = False xbOLD = 0 ybOLD = 0 ball_pos_last = [0, 0] #worth it to take the penalty spotY = 0.02 while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() if self.name[0] == 'B': team = 1 else: team = -1 if team == 1: spotX = 0.55 else: spotX = -0.55 # Get the position of our robot robot_pos = data[self.name] orientation = robot_pos['orientation'] xr = robot_pos['x'] yr = robot_pos['y'] # Get the position of the ball ball_pos = data['ball'] xb = ball_pos['x'] yb = ball_pos['y'] b1 = data['B1'] b3 = data['B3'] if abs(xr - spotX) <= 0.05 and abs(yr - spotY) <= 0.05: SPOTONE = False if abs(xr - spotX) <= 0.05 and abs(yr + spotY) <= 0.05: SPOTONE = True if team == 1: if xb > 0.3: BLOCK = True else: BLOCK = False else: if xb < -0.3: BLOCK = True else: BLOCK = False if BLOCK: self.evanMethod(ball_pos_last) elif SPOTONE: robotPointAngle, robot_angle = utils.getPointAngle( orientation, xr, yr, spotX, spotY) direction = utils.get_direction(robotPointAngle) # If the robot has the ball right in front of it, go forward, # rotate otherwise if direction == 0: left_speed = -10 right_speed = -10 elif direction == -1: left_speed = direction * 10 right_speed = direction * -10 else: left_speed = direction * 10 right_speed = direction * -10 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed) else: robotPointAngle, robot_angle = utils.getPointAngle( orientation, xr, yr, spotX, spotY * -1) direction = utils.get_direction(robotPointAngle) # If the robot has the ball right in front of it, go forward, # rotate otherwise if direction == 0: left_speed = -10 right_speed = -10 elif direction == -1: left_speed = direction * 10 right_speed = direction * -10 else: left_speed = direction * 10 right_speed = direction * -10 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed) xbOLD = xb yBOLD = yb ball_pos_last = [ball_pos['x'], ball_pos['y']]
def run(self): frameCounter = 0 past_position = {"x": 0, "y": 0} x_diff = 1 y_diff = 1 slope = 0 if self.name[0] == 'Y': t_m = 1 else: t_m = -1 status = "idling" past_intercept = {"x": 0, "y": 0} while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1: if self.is_new_data(): data = self.get_new_data() # Get the position of our robot robot_pos = data[self.name] # Get the position of the ball ball_pos = data['ball'] x_diff = ball_pos['x'] - past_position['x'] y_diff = ball_pos['y'] - past_position['y'] # if ball_pos['x'] > -0.75 and ball_pos['x'] < -0.59 and ball_pos['y'] > -0.35 and ball_pos['y'] < 0.35: if frameCounter % 15 == 0: if x_diff != 0: slope = y_diff / x_diff to_go = self.get_next_position(slope, ball_pos, robot_pos, t_m) if to_go == {}: status = "idling" if robot_pos['y'] < 0.1: to_go = {'x': -0.56 * t_m, 'y': 0.11} elif robot_pos['y'] > 0.1: to_go = {'x': -0.56 * t_m, 'y': -0.11} elif ball_pos['x'] * t_m < -0.35 and robot_pos[ 'x'] > to_go['x'] - 0.02 and robot_pos[ 'x'] < to_go['x'] + 0.02 and robot_pos[ 'y'] > to_go['y'] - 0.02 and robot_pos[ 'y'] < to_go['y'] + 0.02 and abs( to_go['y'] != 0.11): to_go = ball_pos status = "charging" # print("charging") elif status == "charging": if past_intercept['y'] > to_go[ 'y'] - 0.01 and past_intercept['y'] < to_go[ 'y'] + 0.01 and past_intercept[ 'x'] > to_go[ 'x'] - 0.01 and past_intercept[ 'x'] < to_go['x'] + 0.01: to_go = ball_pos status = "charging" else: status = "idling" else: to_go = self.track_ball(ball_pos, t_m) status = "idling" if to_go == {}: if robot_pos['y'] < 0.1: to_go = {'x': -0.56 * t_m, 'y': 0.11} elif robot_pos['y'] > 0.1: to_go = {'x': -0.56 * t_m, 'y': -0.11} past_position = ball_pos past_intercept = to_go # Get angle between the robot and the ball # and between the robot and the north ball_angle, robot_angle = self.get_angles(to_go, robot_pos) # Compute the speed for motors direction = utils.get_direction(ball_angle) # If the robot has the ball right in front of it, go forward, # rotate otherwise # normal forward speed is 5, but i turned it up to 10 - stanley if direction == 0: left_speed = -10 right_speed = -10 elif direction == 2: left_speed = 10 right_speed = 10 else: left_speed = direction * 4 right_speed = direction * -4 # Set the speed to motors self.left_motor.setVelocity(left_speed) self.right_motor.setVelocity(right_speed) frameCounter += 1