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]:
print "Done doing things"
break
# show the image to our screen
# cv2.imshow("image", image)
# key = cv2.waitKey(1) & 0xFF
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
# cleanup the camera and close any open windows
# DEBUG print "Radius: ", radius print "Center: ", ball_center if radius > 25: state_changed = True curr_state = states[3] data["found"] = False data["visited"] = [False for i in xrange(num_markers)] else: state_changed = False print "Driving forward to ball" car.turnAngle(int(data["angle"] * 3)) # maybe x2 or another multiplier car.accelT(35, 0.75) else: # TODO: Think of logic to move to marker, shouldn't be too hard data["angle"], _, distance = look_for_marker(image, data["curr_marker"]) print "Distance of marker is: ", distance if distance < 2: state_changed = True curr_state = states[0] data["found"] = False data["visited"][data["curr_marker"]] = True data["curr_marker"] = -1 else: state_changed = False print "Driving forward to marker"
def pickup(): print "PICKING UP BALL" car.accelT(28, 0.8)
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]
elif curr_state == states[2]:
