class Navigation():
def __init__(self, arduino):
self.pixy = Pixy()
self.arduino = arduino
def stop(self):
self.arduino.set_motors(0, 0)
def forward(self):
self.arduino.set_motors(constants.FORWARD_SPEED_LEFT, constants.FORWARD_SPEED_RIGHT)
def reverse(self):
self.arduino.set_motors(-1 * constants.FORWARD_SPEED_LEFT, -1 * constants.FORWARD_SPEED_RIGHT)
def spin_clockwise(self):
self.arduino.set_motors(constants.SPIN_SPEED_LEFT, -1 * constants.SPIN_SPEED_RIGHT)
def spin_counterclockwise(self):
self.arduino.set_motors(-1 * constants.SPIN_SPEED_LEFT, constants.SPIN_SPEED_RIGHT)
def spin_clockwise_fast(self):
self.arduino.set_motors(constants.SPIN_SPEED_LEFT_FAST, -1 * constants.SPIN_SPEED_RIGHT_FAST)
def spin_counterclockwise_fast(self):
self.arduino.set_motors(-1 * constants.SPIN_SPEED_LEFT_FAST, constants.SPIN_SPEED_RIGHT_FAST)
def spin_and_search_cone(self):
if (self.pixy.signature_name != "cone"):
self.pixy.set_signature_cone()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
if (block_x != None):
print("Cone Found")
self.stop()
return "CONE_FOUND"
else:
self.spin_clockwise_fast()
return None
def wander_and_search_cone(self):
if (self.pixy.signature_name != "cone"):
self.pixy.set_signature_cone()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
self.arduino.print_ir()
if (block_x != None):
print("Cone Found")
self.stop()
return "CONE_FOUND"
else:
if (self.arduino.ir_wall()):
print("Wandered into wall! Spinning...")
self.spin_clockwise_fast()
else:
self.forward()
return None
# Returns "CONE_IN_RANGE" if cone in range, "LOST_CONE" if lose cone, otherwise None
def approach_cone(self):
if (self.pixy.signature_name != "cone"):
self.pixy.set_signature_cone()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
ping = self.arduino.get_ping()
if (block_x == None):
print("Lost Cone")
self.stop()
return "LOST_CONE"
if (ping <= constants.PING_CONE_THRESHOLD and ping != 0):
print("Cone in Range")
self.stop()
return "CONE_IN_RANGE"
print("x:" + str(block_x))
if (block_x < constants.PIXY_BOUNDARY_LEFT):
print("Go left")
self.spin_counterclockwise()
return None
elif(block_x > constants.PIXY_BOUNDARY_RIGHT):
print("Go right")
self.spin_clockwise()
return None
else:
print("Go forward")
self.forward()
return None
def spin_and_search_target(self):
if (self.pixy.signature_name != "target"):
self.pixy.set_signature_target()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
if (block_x != None):
print("Target Found")
self.stop()
return "TARGET_FOUND"
else:
self.spin_clockwise()
return None
def wander_and_search_target(self):
if (self.pixy.signature_name != "target"):
self.pixy.set_signature_target()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
self.arduino.print_ir()
if (block_x != None):
print("Target Found")
self.stop()
return "TARGET_FOUND"
else:
if (self.arduino.ir_wall_target()):
print("Wandered into wall! Spinning...")
self.spin_clockwise_fast()
else:
self.forward()
return None
# Returns "TARGET_IN_RANGE" if target in range, "LOST_TARGET" if lose target, otherwise None
def approach_target(self):
if (self.pixy.signature_name != "target"):
self.pixy.set_signature_target()
block_x = self.pixy.get_pixy_block_x_average(self.arduino.get_pixy_blocks())
ping = self.arduino.get_ping()
ir_mid = self.arduino.get_ir_mid()
if (block_x == None):
print("Lost Target")
self.stop()
return "LOST_TARGET"
# if (ping <= constants.PING_TARGET_THRESHOLD and ping != 0):
if (ir_mid >= constants.IR_TARGET_THRESHOLD):
print("Target in Range")
self.stop()
return "TARGET_IN_RANGE"
print("x:" + str(block_x))
if (block_x < constants.PIXY_BOUNDARY_LEFT):
print("Go left")
self.spin_counterclockwise()
return None
elif(block_x > constants.PIXY_BOUNDARY_RIGHT):
print("Go right")
self.spin_clockwise()
return None
else:
print("Go forward")
self.forward()
return None
def __init__(self, arduino): self.pixy = Pixy() self.arduino = arduino