if data["is_ball"]:
data["ball_center"], data["ball_radius"], data[
"angle"] = determine_closest_center(image, 15)
# DEBUG
print "Radius: ", data["ball_radius"]
print "Center: ", data["ball_center"]
print "Angle: ", data["angle"]
if data["ball_radius"] > 25:
state_changed = True
curr_state = states[3]
else:
state_changed = False
print "driving forward"
car.turnAngle(data["angle"] *
3) # maybe x2 or another multiplier
car.accel(30)
else:
# TODO: Think of logic to move to marker, shouldn't be too hard
look_for_marker(image, data["curr_marker"])
# Pick up ball from here
elif curr_state == states[3]:
pickup()
car.accelT(30, 1.2)
state_changed = True
curr_state = states[4]
elif curr_state == states[4]:
data["count"] += 1
if data["count"] == 10 or data["ball_found"] == True:
state_changed = True
if not data["ball_found"]:
curr_state = states[4]
else:
curr_state = states[2]
else:
state_changed = False
elif curr_state == states[1]:
# Turn left takes in an angle
# car.turnLeft(data["angle"])
car.turnAngle(100)
car.accelT(30, 0.5)
car.stop()
car.straighten()
state_changed = True
if not data["ball_found"]:
data["num_rot"] += 1
if data["num_rot"] == 4:
curr_state = states[4]
else:
curr_state = states[0]
else:
data["num_rot"] = 0
curr_state = states[2]