class Controller(object):
def __init__(self):
self.kb = KnowledgeBase()
self.location = BarcodeServer(self.location_callback)
self.flight_path = FlightPath()
self.motion = Motion(self.kb)
self.node_a = NodeAServer()
def _init_fp(self):
self.flight_path.build_full_graph()
def location_callback(self, position):
"""
Location callback.
This function is used by the threaded location module to
update the current location for the controller.
Parameters
----------
position : str
String representation of six digit number indicating current position.
"""
self.kb.current_location = position
# print "Position: ", self.kb.current_location
def run(self):
self.location.start()
self.node_a.start()
path = [100001, 100002, 100003, 100004, 100005, 100006, 100007]
for p in path:
self.motion.move_until_location(p)
if p == 100001:
tic = time.time()
else:
toc = time.time()
self.kb.duration.append(toc-tic)
tic = time.time()
print "kb.duration: ", self.kb.duration
def shutdown(self):
self.motion.shutdown()
self.node_a.join()
class Controller(object):
def __init__(self):
self.kb = KnowledgeBase()
self.location = BarcodeServer(self.location_callback)
self.flight_path = FlightPath()
self.motion = Motion(self.kb)
self.node_a = NodeAServer()
def _init_fp(self):
self.flight_path.build_full_graph()
def location_callback(self, position):
"""
Location callback.
This function is used by the threaded location module to
update the current location for the controller.
Parameters
----------
position : str
String representation of six digit number indicating current position.
"""
self.kb.current_location = position
# print "Position: ", self.kb.current_location
def run(self):
self.location.start()
self.node_a.start()
path = [100001, 100002, 100003, 100004, 100005, 100006, 100007]
for p in path:
self.motion.move_until_location(p)
if p == 100001:
tic = time.time()
else:
toc = time.time()
self.kb.duration.append(toc - tic)
tic = time.time()
print "kb.duration: ", self.kb.duration
def shutdown(self):
self.motion.shutdown()
self.node_a.join()
class ThreadedTest(object):
def __init__(self):
self.scanner = BarcodeServer(self.callback)
def callback(self, barcode):
print "\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "\n current location: ", barcode
print "\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n"
def run(self):
self.scanner.start()
while True:
print time.time()
time.sleep(1)
def shutdown(self):
self.scanner.join()
class BasicMotion(object):
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.__init_light_sensor()
self.__init_motors()
self.scanner = BarcodeServer(self.callback)
self.location_found = False
def __init_light_sensor(self):
self.light = Light(self.brick, PORT_1)
self.light.set_illuminated(True)
def __init_motors(self):
self.motor_left = Motor(self.brick, PORT_A)
self.motor_right = Motor(self.brick, PORT_B)
def __servo_rotation(self, distance, wheel_diameter = 2.221):
return (180.0 * distance) / (math.pi * wheel_diameter / 2.0)
def callback(self, barcode):
self.location_found = True
print "\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "\n current location: ", barcode
print "\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n"
def run(self):
self.scanner.start()
while True:
self.follow_line()
self.find_line()
# self.follow_line()
# for i in range (10):
# print "Light sensor reading: ", self.get_light_reading()
# time.sleep(0.1)
# self.turn_in_place(-90)
# for i in range (10):
# print "Light sensor reading: ", self.get_light_reading()
# time.sleep(0.1)
# self.shutdown()
def shutdown(self):
self.halt_motion('coast')
self.light.set_illuminated(False)
self.scanner.join()
def turn_in_place(self, yaw, power = 75, axle_length = 4.5):
arc_distance = (math.pi / 180.0) * abs(yaw) * (axle_length / 2.0)
wheel_rotation = self.__servo_rotation(arc_distance)
# print "arc_distance: ", arc_distance
# print "wheel_rotation", wheel_rotation
if (yaw > 0):
self.motor_left.weak_turn(power, int(wheel_rotation))
self.motor_right.weak_turn(-power, int(wheel_rotation))
elif (yaw < 0):
self.motor_left.weak_turn(-power, int(wheel_rotation))
self.motor_right.weak_turn(power, int(wheel_rotation))
else:
print "desired yaw angle is 0, nothing to do"
def get_light_reading(self, illumination=True):
self.light.set_illuminated(illumination)
return self.light.get_sample()
def move_forward(self, power = 15, regulated = True):
self.motor_left.run(power, regulated)
self.motor_right.run(power, regulated)
def halt_motion(self, method):
if method not in ['brake', 'coast']:
print "`method` must be one of {`brake`|`coast`}"
raise ValueError
if method == 'brake':
self.motor_left.brake()
self.motor_right.brake()
else:
self.motor_left.idle()
self.motor_right.idle()
def follow_line(self):
# print "following line"
self.move_forward()
while True:
reflected_light = self.get_light_reading()
# print "Light sensor reading: ", self.get_light_reading()
if reflected_light > 500:
self.halt_motion('brake')
# print "lost line, stopping motion"
return
def find_line(self):
# print "finding line"
sweep_angle = [10, -20, 30, -40, 60, -80, 120, -160, 240, -320]
line_found = False
is_turning = True
while not line_found:
for sweep in sweep_angle:
# print "find line: sweeping %d degrees" % sweep
self.turn_in_place(sweep)
state = self.motor_left._read_state()
run_state = state[0].run_state
# print "run_state: ", run_state
while run_state != 0:
reflected_light = self.get_light_reading()
if reflected_light < 400:
line_found = True
self.halt_motion('brake')
# print "found line, stopping search"
return
state = self.motor_left._read_state()
run_state = state[0].run_state
class BasicMotion(object):
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.__init_light_sensor()
self.__init_motors()
self.scanner = BarcodeServer(self.callback)
self.location_found = False
self.location = ''
def __init_light_sensor(self):
self.light = Light(self.brick, PORT_1)
self.light.set_illuminated(True)
def __init_motors(self):
self.motor_left = Motor(self.brick, PORT_A)
self.motor_right = Motor(self.brick, PORT_B)
def __servo_rotation(self, distance, wheel_diameter = 2.221):
return (180.0 * distance) / (math.pi * wheel_diameter / 2.0)
def callback(self, barcode):
self.location = barcode
# self.location_found = True
# print "\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
# print "\n current location: ", barcode
# print "\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n"
def run(self):
# self.go_forward_n(3, -75)
# sys.exit(1)
self.scanner.start()
while True:
self.follow_line()
# self.halt_motion('coast')
if self.location == '100004':
self.location = ''
break
self.find_line()
# self.halt_motion('coast')
print "Please enter path choice: L(eft) C(enter) R(ight)"
path_choice = raw_input("choice: ")
if path_choice == 'L' or path_choice == 'l':
angle = -45
elif path_choice == 'C' or path_choice == 'c':
angle = 0
elif path_choice == 'R' or path_choice == 'r':
angle = 45
else:
print "+++ you fail +++"
sys.exit(1)
self.acquire_path(angle)
while True:
self.follow_line()
self.find_line()
def acquire_path(self, angle):
self.go_forward_n(3, 75)
state = self.motor_left._read_state()
run_state = state[0].run_state
while run_state != 0:
state = self.motor_left._read_state()
run_state = state[0].run_state
self.turn_in_place(angle)
state = self.motor_left._read_state()
run_state = state[0].run_state
while run_state != 0:
state = self.motor_left._read_state()
run_state = state[0].run_state
self.find_line()
def shutdown(self):
self.halt_motion('coast')
self.light.set_illuminated(False)
self.scanner.join()
def turn_in_place(self, yaw, power = 75, axle_length = 4.5):
arc_distance = (math.pi / 180.0) * abs(yaw) * (axle_length / 2.0)
wheel_rotation = self.__servo_rotation(arc_distance)
# print "arc_distance: ", arc_distance
# print "wheel_rotation", wheel_rotation
if (yaw > 0):
self.motor_left.weak_turn(power, int(wheel_rotation))
self.motor_right.weak_turn(-power, int(wheel_rotation))
elif (yaw < 0):
self.motor_left.weak_turn(-power, int(wheel_rotation))
self.motor_right.weak_turn(power, int(wheel_rotation))
else:
print "desired yaw angle is 0, nothing to do"
def get_light_reading(self, illumination=True):
self.light.set_illuminated(illumination)
return self.light.get_sample()
def move_forward(self, power = 15, regulated = True):
self.motor_left.run(power, regulated)
self.motor_right.run(power, regulated)
def go_forward_n(self, n, power):
wheel_rotation = self.__servo_rotation(n)
self.motor_left.weak_turn(power, int(wheel_rotation))
self.motor_right.weak_turn(power, int(wheel_rotation))
def halt_motion(self, method):
if method not in ['brake', 'coast']:
print "`method` must be one of {`brake`|`coast`}"
raise ValueError
if method == 'brake':
self.motor_left.brake()
self.motor_right.brake()
else:
self.motor_left.idle()
self.motor_right.idle()
def follow_line(self):
print "following line"
self.move_forward()
while True:
reflected_light = self.get_light_reading()
if reflected_light > 500:
self.halt_motion('brake')
return
if self.location == '100004':
self.halt_motion('brake')
print "\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "\n arrived at desired location: ", self.location
print "\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n"
return
def find_line(self):
# sweep_angle = [5, -10, 15, -20, 30, -40, 60, -80, 120, -160, 240, -320]
sweep_angle = [10, -20, 30, -40, 60, -80, 120, -160, 240, -320]
print "finding line"
line_found = False
is_turning = True
while not line_found:
for sweep in sweep_angle:
# print "find line: sweeping %d degrees" % sweep
self.turn_in_place(sweep)
state = self.motor_left._read_state()
run_state = state[0].run_state
# print "run_state: ", run_state
while run_state != 0:
reflected_light = self.get_light_reading()
if reflected_light < 400:
line_found = True
self.halt_motion('brake')
print "found line, stopping search"
return
state = self.motor_left._read_state()
run_state = state[0].run_state