def __init__(self, _zumi=None):
if _zumi is None:
self.zumi = Zumi()
else:
self.zumi = _zumi
self.current_x = 0
self.current_y = 0
self.zumi.reset_gyro()
def __init__(self, _zumi=None):
if _zumi is None:
self.z = Zumi()
else:
self.z = _zumi
self.z.mpu.calibrate_MPU()
self.NORTH = 0
self.WEST = 90
self.EAST = -90
self.SOUTH = 180
self.heading = self.NORTH
self.current_x = 0
self.current_y = 0
self.motor_speed = 10
self.ir_threshold = 125
def __init__(self):
# OELD 제어를 위한 설정
self.disp = Adafruit_SSD1306.SSD1306_128_64(rst=24)
self.disp.begin()
self.image = Image.new('1', (self.disp.width, self.disp.height))
self.draw = ImageDraw.Draw(self.image)
# 포인터의 크기
self.size = 3
# 자이로 X,Y 각도 값
self.oldCharX = 255
self.oldCharY = 255
# 펜의 좌표 X,Y 값
self.penX = 0
self.penY = 0
# 자이로 센서
self.zumi = Zumi()
self.zumi.reset_gyro()
def __init__(self, zumi = None):
self.start_node = Point(0, 0)
self.bigben = Point(5, 20)
self.seattle = Point(25, 0)
self.paris = Point(15, 10)
self.NY = Point(10, 15)
self.china = Point(25, 20)
self.node1 = Point(10, 0)
self.node2 = Point(20, 0)
self.node3 = Point(30, 0)
self.node4 = Point(0, 10)
self.node5 = Point(10, 10)
self.node6 = Point(20, 10)
self.node7 = Point(30, 10)
self.node8 = Point(0, 20)
self.node9 = Point(10, 20)
self.node10 = Point(20, 20)
self.node11 = Point(30, 20)
self.node12 = Point(0, 30)
self.node13 = Point(10, 30)
self.node14 = Point(30, 30)
self.G = nx.Graph()
self.generate_map()
self.NORTH = 0
self.WEST = 90
self.EAST = -90
self.SOUTH = 180
self.heading = self.NORTH
if zumi is None:
# pass
self.zumi = Zumi()
self.motor_speed = 10
self.ir_threshold = 70
self.reverse = False
def run():
try:
zumi = Zumi()
camera = Camera()
screen = Screen()
personality = Personality(zumi, screen)
faceDetector = FaceDetector()
camera.start_camera()
while True:
image = camera.capture()
face = faceDetector.detect_face(image)
if face:
face_roi = faceDetector.get_face_roi()
smile = faceDetector.detect_smile(face_roi)
if smile:
personality.happy()
else:
personality.angry()
else:
screen.close_eyes()
finally:
screen.draw_text("")
camera.close()
class Route_new:
def __init__(self, zumi = None):
self.start_node = Point(0, 0)
self.bigben = Point(5, 20)
self.seattle = Point(25, 0)
self.paris = Point(15, 10)
self.NY = Point(10, 15)
self.china = Point(25, 20)
self.node1 = Point(10, 0)
self.node2 = Point(20, 0)
self.node3 = Point(30, 0)
self.node4 = Point(0, 10)
self.node5 = Point(10, 10)
self.node6 = Point(20, 10)
self.node7 = Point(30, 10)
self.node8 = Point(0, 20)
self.node9 = Point(10, 20)
self.node10 = Point(20, 20)
self.node11 = Point(30, 20)
self.node12 = Point(0, 30)
self.node13 = Point(10, 30)
self.node14 = Point(30, 30)
self.G = nx.Graph()
self.generate_map()
self.NORTH = 0
self.WEST = 90
self.EAST = -90
self.SOUTH = 180
self.heading = self.NORTH
if zumi is None:
# pass
self.zumi = Zumi()
self.motor_speed = 10
self.ir_threshold = 70
self.reverse = False
def generate_map(self):
# Directed Graph
# G 그래프 만들기 (node 간의 edge가 존재하면 add_node 하고 add_edge 안해도 됨
self.G.add_edge(self.start_node, self.node1, distance=10)
self.G.add_edge(self.start_node, self.node4, distance=10)
self.G.add_edge(self.node1, self.node2, distance=10)
self.G.add_edge(self.node1, self.node5, distance=10)
self.G.add_edge(self.node2, self.seattle, distance=5)
self.G.add_edge(self.node2, self.node6, distance=10)
self.G.add_edge(self.seattle, self.node3, distance=5)
self.G.add_edge(self.node3, self.node7, distance=10)
self.G.add_edge(self.node4, self.node5, distance=10)
self.G.add_edge(self.node4, self.node8, distance=10)
self.G.add_edge(self.node5, self.NY, distance=5)
self.G.add_edge(self.node5, self.paris, distance=5)
self.G.add_edge(self.NY, self.node9, distance=5)
self.G.add_edge(self.paris, self.node6, distance=5)
self.G.add_edge(self.node6, self.node7, distance=10)
self.G.add_edge(self.node6, self.node10, distance=10)
self.G.add_edge(self.node7, self.node11, distance=10)
self.G.add_edge(self.node8, self.bigben, distance=5)
self.G.add_edge(self.node8, self.node12, distance=10)
self.G.add_edge(self.bigben, self.node9, distance=5)
self.G.add_edge(self.node9, self.node10, distance=10)
self.G.add_edge(self.node9, self.node13, distance=10)
self.G.add_edge(self.node10, self.china, distance=5)
self.G.add_edge(self.china, self.node11, distance=5)
self.G.add_edge(self.node11, self.node14, distance=10)
self.G.add_edge(self.node12, self.node13, distance=10)
self.G.add_edge(self.node13, self.node14, distance=20)
def find_path(self, start, destination):
start_point = start
destination = destination
# 연결 안 된 노드가 있을 경우를 방지
if nx.has_path(self.G, start_point, destination):
path = nx.shortest_path(self.G, source=start_point, target=destination, weight='distance')
else:
print("No path!!")
return
if path is not None:
print(path)
return path
def driving(self, start, destination):
shortest_path = self.find_path(start, destination)[1:]
current_node = start
while len(shortest_path):
print(shortest_path)
next_node = shortest_path.pop(0)
# found_obstacle = False
found_obstacle = self.drive_to_nextnode(current_node,next_node)
if found_obstacle:
print("find obstacle : {}".format(found_obstacle))
self.go_back_to_node(found_obstacle)
self.disconnect_route(current_node, next_node)
self.driving(current_node, destination)
return
current_node = next_node
self.zumi.stop()
return
def driving_without_reroute(self, start, destination):
shortest_path = self.find_path(start, destination)[1:]
current_node = start
while len(shortest_path):
print(shortest_path)
next_node = shortest_path.pop(0)
x = self.drive_to_nextnode(current_node,next_node)
current_node = next_node
self.zumi.stop()
return
def drive_to_nextnode(self, current, next):
dx = next.x - current.x
dy = next.y - current.y
print("[{},{}]".format(dx, dy))
self.decide_turn_or_pass_intersection(dx, dy, current)
# if max(abs(dx),abs(dy))%10:
# result = self.drive_n_block(abs(dx + dy)+1)
# else:
result = self.drive_n_block(abs(dx+dy))
return result
def decide_turn_or_pass_intersection(self, dx, dy, current):
print("check turn or not")
temp = current.x + current.y
if self.reverse:
self.reverse = False
elif temp and temp % 10 == 0:
self.cross_intersection()
if dx > 0:
new_heading = self.EAST
elif dx < 0:
new_heading = self.WEST
elif dy > 0:
new_heading = self.NORTH
else:
new_heading = self.SOUTH
if not new_heading - 20 < self.heading < new_heading + 20:
print("change heading")
self.heading = new_heading
def drive_n_block(self, n):
print("drive n block")
ir_readings = self.zumi.get_all_IR_data()
left_on_white = False if ir_readings[3] > self.ir_threshold else True
right_on_white = False if ir_readings[1] > self.ir_threshold else True
right_switch = 0
left_switch = 0
while left_on_white or right_on_white:
ir_readings = self.zumi.get_all_IR_data()
if ir_readings[3] < self.ir_threshold:
if not left_on_white:
left_on_white = True
else:
left_on_white = False
if ir_readings[1] < self.ir_threshold:
if not right_on_white:
right_on_white = True
else:
right_on_white = False
self.adjust_driving(left_on_white, right_on_white)
if ir_readings[0] < 70 or ir_readings[5] < 70:
return max(right_switch, left_switch)
self.zumi.go_straight(self.motor_speed, self.heading)
while right_switch != n and left_switch != n:
print("{},{}".format(left_switch, right_switch))
ir_readings = self.zumi.get_all_IR_data()
if ir_readings[3] < self.ir_threshold:
if not left_on_white:
left_switch += 1
left_on_white = True
else:
left_on_white = False
if ir_readings[1] < self.ir_threshold:
if not right_on_white:
right_switch += 1
right_on_white = True
else:
right_on_white = False
self.adjust_driving(left_on_white, right_on_white)
# detect obstacle
if ir_readings[0] < 70 or ir_readings[5] < 70:
return max(right_switch, left_switch)
self.zumi.go_straight(self.motor_speed, self.heading)
return False
def adjust_driving(self, left_on_white, right_on_white, reverse=1):
if right_on_white and not left_on_white:
correction = -1
elif left_on_white and not right_on_white:
correction = 1
else:
return
self.heading += correction*reverse
def cross_intersection(self):
print("cross road")
start = time.time()
end = 0
while end < 0.45:
end = time.time()-start
self.zumi.go_straight(10, self.heading)
def go_back_to_node(self, n):
self.reverse = True
left_on_white = False
right_on_white = False
right_switch = 0
left_switch = 0
while right_switch != n and left_switch != n:
print( "{},{}".format(left_switch, right_switch))
ir_readings = self.zumi.get_all_IR_data()
if ir_readings[3] < self.ir_threshold:
if not left_on_white:
left_switch += 1
left_on_white = True
else:
left_on_white = False
if ir_readings[1] < self.ir_threshold:
if not right_on_white:
right_switch += 1
right_on_white = True
else:
right_on_white = False
self.adjust_driving(left_on_white, right_on_white, reverse=-1)
self.zumi.go_reverse(self.motor_speed, self.heading)
# while right_on_white or left_on_white:
# ir_readings = self.zumi.get_all_IR_data()
#
# if ir_readings[3] < self.ir_threshold:
# if not left_on_white:
# left_switch += 1
# left_on_white = True
# else:
# left_on_white = False
#
# if ir_readings[1] < self.ir_threshold:
# if not right_on_white:
# right_switch += 1
# right_on_white = True
# else:
# right_on_white = False
#
# self.adjust_driving(left_on_white, right_on_white, reverse=-1)
#
# # detect obstacle
# if ir_readings[0] < 70 or ir_readings[5] < 70:
# return max(right_switch, left_switch)
#
# self.zumi.go_reverse(self.motor_speed, self.heading)
time.sleep(0.4)
print("done")
def disconnect_route(self, current_node, next_node):
# self.G.add_edge(current_node, next_node, distance=1000)
self.G.remove_edge(current_node, next_node)
def reset_map(self):
self.G = nx.Graph()
self.generate_map()
def turn(self, angle=91, speed=30, step=4, direction=-1, delay=0.01):
direction = self.zumi.clamp(direction,-1,1)
init_ang_z = self.zumi.read_z_angle()
for i in range(0, angle, step):
self.zumi.go_straight(speed, init_ang_z+direction*i)
time.sleep(delay)
def park_left(self):
self.turn(direction=1)
self.zumi.forward(duration=0.5)
def park_right(self):
self.turn()
self.zumi.forward(duration=0.5)
def predict(model):
c = Crop()
eye = Screen()
zumi = Zumi()
camera = Camera(64, 64)
personality = Personality(zumi, eye)
cnt = 0
prev_label = -1
cnt_none_crop = 0
try:
while True:
zumi.reset_drive()
img = camera.run()
crop_img = c.crop(img)
if crop_img is None:
if cnt_none_crop == 0:
personality.look_around_open01()
elif cnt_none_crop == 1:
personality.look_around_open02()
elif cnt_none_crop == 2:
personality.look_around_open03()
elif cnt_none_crop == 3:
personality.look_around_open04()
prev_label = -1
print(cnt, prev_label, cnt_none_crop)
cnt_none_crop += 1
cnt_none_crop %= 4
continue
else:
cnt_none_crop = 0
x = Image.fromarray(crop_img)
x = np.expand_dims(x, axis=0)
preds = model.predict_classes(x)
print(landmark[preds[0]])
if prev_label == preds[0]:
cnt += 1
if cnt > 2:
print(eye.EYE_IMAGE_FOLDER_PATH + "sad1.ppm")
print("reaction!!!!")
eye.draw_image(
eye.path_to_image(LAND_PATH + landmark[preds[0]] +
".jpg"))
time.sleep(2)
if landmark[preds[0]] == 'eiffel':
zumi.turn_right(90)
elif landmark[preds[0]] == 'nyc':
zumi.turn_right(45)
elif landmark[preds[0]] == 'seattle':
zumi.turn_left(45)
elif landmark[preds[0]] == 'china':
zumi.turn_left(90)
time.sleep(.5)
zumi.forward(10, duration)
time.sleep(.5)
personality.celebrate()
time.sleep(.5)
zumi.reverse(10, duration)
time.sleep(.5)
if landmark[preds[0]] == 'eiffel':
zumi.turn_left(90)
elif landmark[preds[0]] == 'nyc':
zumi.turn_left(45)
elif landmark[preds[0]] == 'seattle':
zumi.turn_right(45)
elif landmark[preds[0]] == 'china':
zumi.turn_right(90)
cnt = 0
else:
personality.reading(
eye.path_to_image(READ_PATH + "reading_" + str(cnt) +
".PPM"))
else:
cnt = 0
prev_label = preds[0]
except KeyboardInterrupt:
camera.shutdown()
eye.draw_text("")
zumi.stop()
print("\nExiting...")
except:
camera.shutdown()
eye.draw_text("")
zumi.stop()
print("\nExiting...")
from zumi.zumi import Zumi
import time
from zumi.protocol import IR
zumi = Zumi()
def crossing_line(numLines, motor_speed=10, ir_threshold=125):
# 직진을 위한 자이로 센서 보정
#zumi.mpu.calibrate_MPU(100)
zumi.reset_gyro()
heading = 0
left_on_white = True
right_on_white = True
right_switch = 0
left_switch = 0
time_switch = False
lineCount = 1
# 흰색에서 출발
print("start")
while True:
bottom_left_ir = zumi.get_IR_data(IR.bottom_left)
bottom_right_ir = zumi.get_IR_data(IR.bottom_right)
# white check
class Drive:
def __init__(self, _zumi=None):
if _zumi is None:
self.z = Zumi()
else:
self.z = _zumi
self.z.mpu.calibrate_MPU()
self.NORTH = 0
self.WEST = 90
self.EAST = -90
self.SOUTH = 180
self.heading = self.NORTH
self.current_x = 0
self.current_y = 0
self.motor_speed = 10
self.ir_threshold = 125
# ----------------FUNCTIONS-----------------
def turn(self, angle=91, speed=30, step=4, direction=-1, delay=0.01):
direction = self.z.clamp(direction, -1, 1)
init_ang_z = self.z.read_z_angle()
for i in range(0, angle, step):
self.z.go_straight(speed, init_ang_z + direction * i)
time.sleep(delay)
def cross_intersection(self):
start = time.time()
end = 0
while end < 0.4:
end = time.time() - start
self.z.go_straight(10, self.heading)
def move_to_coordinate(self, desired_x, desired_y):
dx = desired_x - self.current_x # Find out the difference in x
dy = desired_y - self.current_y # Find out the difference in y
if dx % 10 == 0 and dx != 5:
if dx > 0: # If x is positive (going East)
if not self.current_x == 0:
self.cross_intersection()
self.heading = self.EAST
self.drive_block(dx)
elif dx < 0: # If x is negative (going West)
self.cross_intersection()
self.heading = self.WEST
self.drive_block(abs(dx))
self.current_x = desired_x
if dy > 0: # If y is also positive (going North)
self.cross_intersection()
self.heading = self.NORTH
self.drive_block(dy)
elif dy < 0: # If y is negative (going South)
self.cross_intersection()
self.heading = self.SOUTH
self.drive_block(abs(dy))
self.current_y = desired_y
else:
if dy > 0: # If y is also positive (going North)
self.cross_intersection()
self.heading = self.NORTH
self.drive_block(dy)
elif dy < 0: # If y is negative (going South)
self.cross_intersection()
self.heading = self.SOUTH
self.drive_block(abs(dy))
self.current_y = desired_y
if dx > 0: # If x is positive (going East)
self.cross_intersection()
self.heading = self.EAST
self.drive_block(dx)
elif dx < 0: # If x is negative (going West)
if self.self.current_x != 0:
self.cross_intersection()
self.heading = self.WEST
self.drive_block(abs(dx))
self.current_x = desired_x
def park_left(self):
self.turn(direction=1)
self.z.forward(duration=0.5)
def park_right(self):
self.turn()
self.z.forward(duration=0.5)
def drive_block(self, x):
left_on_white = False
right_on_white = False
right_switch = 0
left_switch = 0
while True:
ir_readings = self.z.get_all_IR_data()
bottom_right_ir = ir_readings[1]
bottom_left_ir = ir_readings[3]
front_left_ir = ir_readings[0]
front_right_ir = ir_readings[5]
if bottom_left_ir < self.ir_threshold:
if not left_on_white:
left_switch += 1
left_on_white = True
else:
left_on_white = False
if bottom_right_ir < self.ir_threshold:
if not right_on_white:
right_switch += 1
right_on_white = True
else:
right_on_white = False
if right_on_white and not left_on_white:
self.heading -= 1
if left_on_white and not right_on_white:
self.heading += 1
if right_switch == x or left_switch == x:
break
if front_left_ir < 70 or front_right_ir < 70:
self.z.stop(0)
continue
#clear_output(wait=True)
self.z.go_straight(self.motor_speed, self.heading)
def run_demo(self, location):
try:
if location == "a":
self.move_to_coordinate(10, 5)
self.park_left()
if location == "b":
self.move_to_coordinate(15, 10)
self.park_right()
if location == "c":
self.move_to_coordinate(25, 0)
self.park_left()
if location == "d":
self.move_to_coordinate(5, 10)
self.park_left()
if location == "e":
self.move_to_coordinate(15, 20)
self.park_right()
if location == "f":
self.move_to_coordinate(20, 16)
self.park_right()
if location == "g":
self.move_to_coordinate(0, 26)
self.park_right()
if location == "h":
self.move_to_coordinate(10, 25)
self.park_right()
if location == "i":
self.move_to_coordinate(26, 30)
self.park_right()
finally:
self.z.stop()
from zumi.zumi import Zumi
zumi = Zumi()
zumi.mpu.calibrate_MPU()
# CONSTANTS
DIV_CONST = 6.0
POWER = 40
def getTimeForTravel(dist):
time = (dist + 1) / DIV_CONST
return time
#make zumi go left by 10 inches from point S to point A
distanceInInches = 10
timeToTravel = getTimeForTravel(distanceInInches)
zumi.turn_left(90)
zumi.forward(POWER, timeToTravel)
#write the code to get Zumi from point S to point X
#write the code to get Zumi from point S to point B
#goal is to type in X instead of 10 from point S
# travel('s', 'x')
# travel('x', 'a')
from zumi.zumi import Zumi
import numpy as np
import time
zumi = Zumi()
idx = 0
print("curr idx : " + str(idx))
while idx < 30:
accX = round(zumi.update_angles()[3], 3)
print("acc X : " + str(accX))
time.sleep(0.1)
idx = idx + 1
from zumi.zumi import Zumi zumi = Zumi() zumi.forward(speed=5, duration=1) zumi.reverse(speed=5, duration=1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = .2
turn = 1
def vels(speed, turn):
return "currently:\tspeed %s\tturn %s " % (speed, turn)
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
zumi = Zumi()
x = 0
count = 0
try:
while (1):
key = getKey()
print("key : " + str(key))
if key in moveBindings:
if (key == "i"):
zumi.forward(speed=5, duration=1)
elif (key == "k"):
zumi.reverse(speed=5, duration=1)
elif (key == "j"):
zumi.turn_left(desired_angle=30, duration=0.1)
elif (key == "l"):
from zumi.zumi import Zumi
zumi = Zumi()
# CONSTANTS
DIV_CONST = 6.0
POWER = 40
# globals
heading = 180
route = [ (90, 10) ] # first part is angle, second is distance
# angle = 90 degrees , distance = 10 inches
def getTimeForTravel(distanceInInches):
time = (distanceInInches + 1) / DIV_CONST
return time
# ex 1:
# currHeading = 270 (down)
# desiredHeading = 90 (up)
# X = 180
# we want to turn 180 degrees in the pos -> turning left by 180
# answer = change_heading_to_desired_heading(currHeading, desiredHeading )
# answer = 90
def change_heading_to_desired_heading( currHeading, desiredHeading):
# step1: calc the magnitude of angle change , call it X
X = desiredHeading - currHeading
print("X", X)
zumi.turn(X)
return desiredHeading
class Drive:
def __init__(self, _zumi=None):
if _zumi is None:
self.zumi = Zumi()
else:
self.zumi = _zumi
self.current_x = 0
self.current_y = 0
self.zumi.reset_gyro()
def move_inches(self, distance, angle):
y_intercept = 1
slope = 6
duration = (distance + y_intercept) / slope
# make sure if there is no distance only turn
if (distance < 0.2):
self.zumi.turn(angle)
# if there is a distance go at speed 40 at that angle
else:
self.zumi.forward(40, duration, angle)
def move_to_coordinate(self, desired_x, desired_y):
dx = desired_x - self.current_x
dy = desired_y - self.current_y
# find the angle
angle = math.degrees(math.atan2(dy, dx))
# find the distance to the coordinate
distance = math.hypot(dx, dy)
# update the coordinates
self.current_x = desired_x
self.current_y = desired_y
print(" ang = ", angle, " dist = ", distance)
self.zumi.turn(angle)
self.move_inches(distance, angle)
# this is for uber challenge/robo world demo map location hard coding
def move_to_a(self):
self.move_to_coordinate(centimeter_to_inch(50) - 4, 0)
self.move_to_coordinate(
centimeter_to_inch(50) - 4,
centimeter_to_inch(25) - 3)
self.move_to_coordinate(
centimeter_to_inch(40) - 4,
centimeter_to_inch(25) - 3)
def return_from_a(self):
self.move_to_coordinate(
centimeter_to_inch(50) - 4,
centimeter_to_inch(25) - 3)
self.move_to_coordinate(centimeter_to_inch(50) - 4, 0)
self.move_to_coordinate(0, 0)
def move_to_b(self):
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(75) - 4,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(75) - 4,
centimeter_to_inch(40) - 4)
def return_from_b(self):
self.move_to_coordinate(
centimeter_to_inch(90) - 4,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(0, 0)
def move_to_c(self):
self.move_to_coordinate(centimeter_to_inch(105) - 4, 0)
self.move_to_coordinate(centimeter_to_inch(105) - 4, 4)
def return_from_c(self):
self.move_to_coordinate(centimeter_to_inch(105) - 4, 0)
self.move_to_coordinate(0, 0)
def move_to_d(self):
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(25) - 3,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(25) - 3, centimeter_to_inch(50))
def return_from_d(self):
self.move_to_coordinate(
centimeter_to_inch(25) - 3,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(0, 0)
def move_to_e(self):
self.move_to_coordinate(20, 0)
self.move_to_coordinate(20, 15)
self.move_to_coordinate(18, 15)
def return_from_e(self):
self.move_to_coordinate(20, 0)
self.move_to_coordinate(20, 15)
self.move_to_coordinate(18, 15)
self.move_to_coordinate(0, 0)
def move_to_f(self):
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(100) - 4,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(100) - 4,
centimeter_to_inch(90) - 4)
self.move_to_coordinate(
centimeter_to_inch(120) - 4,
centimeter_to_inch(90) - 4)
def return_from_f(self):
self.move_to_coordinate(0, centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(100) - 4,
centimeter_to_inch(50) - 4)
self.move_to_coordinate(
centimeter_to_inch(100) - 4,
centimeter_to_inch(90) - 4)
self.move_to_coordinate(
centimeter_to_inch(120) - 4,
centimeter_to_inch(90) - 4)
self.move_to_coordinate(0, 0)
def move_to_g(self):
self.move_to_coordinate(0, 28)
self.move_to_coordinate(3, 28)
def return_from_g(self):
self.move_to_coordinate(0, 28)
self.move_to_coordinate(3, 28)
self.move_to_coordinate(0, 0)
def move_to_h(self):
self.move_to_coordinate(0, 20)
self.move_to_coordinate(10, 20)
self.move_to_coordinate(10, 25)
self.move_to_coordinate(13, 25)
def return_from_h(self):
self.move_to_coordinate(0, 20)
self.move_to_coordinate(10, 20)
self.move_to_coordinate(10, 25)
self.move_to_coordinate(13, 25)
self.move_to_coordinate(0, 0)
def move_to_i(self):
self.move_to_coordinate(0, 30)
self.move_to_coordinate(28, 30)
self.move_to_coordinate(28, 28)
def return_from_i(self):
self.move_to_coordinate(0, 30)
self.move_to_coordinate(28, 30)
self.move_to_coordinate(28, 28)
self.move_to_coordinate(0, 0)
def __init__(self):
self.myConnection = ''
self.user = ''
#init zumi instance
self.zumi = Zumi()
class ZumiService(rpyc.Service):
""" RPC server class
# Note
only methods that are _exposed_ are callable via rpc
"""
def __init__(self):
self.myConnection = ''
self.user = ''
#init zumi instance
self.zumi = Zumi()
def on_connect(self, conn):
""" method is automatically run at new rpc connections
"""
self.myConnection = conn
print ('new connection')
#print (conn._config['credentials']['subject'])
#self.user = conn._config['credentials']['subject'][5][0][1]
#if DEBUG:
#print("new connection: ",
# conn._config['endpoints'], "from user ", self.user)
pass
def on_disconnect(self, conn):
"""method is automatically run when rpc connection is terminated
"""
if DEBUG:
print ("conn ended", conn._config['connid'])
pass
def exposed_ping(self):
"""simple ping for rpc connection testing
"""
if DEBUG:
print ("ping")
return 'pong'
#-----------------------------------------------------------------------------
# reproducing ZUMI API - see http://docs.robolink.com/zumi-library for details
#-----------------------------------------------------------------------------
# DRIVING
def exposed_right_circle(self, speed=30, step=2):
self.zumi.right_circle(speed, step)
def exposed_hard_brake(self):
self.zumi.hard_brake()
def exposed_parallel_park(self,speed=15, step=1, delay=0.01):
self.zumi.parallel_park(speed, step, delay)
def exposed_circle(self,speed=30, step=2, direction=1, delay=0.02):
self.zumi.circle(speed, step, direction, delay)
def exposed_triangle(self,speed=40, seconds=1.5, direction=1):
self.zumi.triangle(speed, seconds, direction)
def exposed_go_straight(self,speed, desired_angle, max_speed=127):
self.zumi.go_straight(speed, desired_angle, max_speed)
def exposed_right_u_turn(self,speed=30, step=4, delay=0.02):
self.zumi.right_u_turn(speed, step, delay)
def exposed_figure_8(self,speed=30, step=3, delay=0.02):
self.zumi.figure_8(speed, step, delay)
def exposed_rectangle(self,speed=40, seconds=1.0, direction=1, ratio=2):
self.zumi.rectangle(speed, seconds, direction, ratio)
def exposed_reverse(self,speed=40, duration=1.0, desired_angle=123456):
self.zumi.reverse(speed, duration, desired_angle)
def exposed_left_u_turn(self,speed=30, step=4, delay=0.02):
self.zumi.left_u_turn(speed, step, delay)
def exposed_square(self,speed=40, seconds=1, direction=1):
self.zumi.square(speed, seconds, direction)
def exposed_square_left(self,speed=40, seconds=1.0):
self.zumi.square_left(speed, seconds)
def exposed_turn_left(self,desired_angle=90, duration=1.0):
self.zumi.turn_left(desired_angle, duration)
def exposed_j_turn(self,speed=80, step=4, delay=0.005):
self.zumi.j_turn(speed, step, delay)
def exposed_left_circle(self,speed=30, step=2):
self.zumi.left_circle(speed, step)
def exposed_forward(self,speed=40, duration=1.0, desired_angle=123456):
self.zumi.forward(speed, duration, desired_angle)
def exposed_turn_right(self,desired_angle=-90,duration=1.0):
self.zumi.turn_right(desired_angle,duration)
def exposed_square_right(self,speed=40, seconds=1.0):
self.zumi.square_right(speed, seconds)
def exposed_go_reverse(self, speed, desired_angle, max_speed=127):
self.zumi.go_reverse(speed, desired_angle, max_speed)
def exposed_line_follower(self, duration, left_thresh=100, right_thresh=100):
self.zumi.line_follower(duration, left_thresh, right_thresh)
#SENSORS
def exposed_get_all_IR_data(self):
return self.zumi.get_all_IR_data()
def exposed_get_battery_voltage(self):
return self.zumi.get_battery_voltage()
#this is undocumented in the Zumi API
def exposed_get_battery_percent(self):
return self.zumi.get_battery_percent()
def exposed_get_IR_data(self,ir_sensor_index):
return self.zumi.get_IR_data(ir_sensor_index)
# CAM - here we use picamera directly, avoiding the poor zumi interface
def exposed_get_picture(self, resolution=(1024,768)):
with picamera.PiCamera() as camera:
camera.resolution = resolution
camera.rotation = 180
output = np.empty((resolution[1],resolution[0],3),dtype=np.uint8)
camera.capture(output , 'rgb')
return output
from zumi.zumi import Zumi zumi = Zumi() zumi.forward(speed=10, duration=1)
from zumi.zumi import Zumi
zumi = Zumi()
acc = zumi.get_acc()
acc_x = acc[0]
acc_y = acc[1]
acc_z = acc[2]
msg = "acc x : " + str(acc_x) + ", acc y : " + str(acc_y) + ", acc z : " + str(
acc_z)
print(msg)
#!/usr/bin/env python
# coding: utf-8
# In[ ]:
from zumi.zumi import Zumi
import time
import socket
from zumi.util.screen import Screen
zumi = Zumi()
screen = Screen()
screen.happy()
while True:
full_msg = ''
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
host = 'XXX.XXX.XX.XX' # ADD HOST IP HERE
s.connect((host, 5560))
full_msg = str(full_msg)
while True:
msg = s.recv(40)
full_msg += msg.decode("utf-8")
if full_msg == "Wearing":
screen.draw_text("Thank you for wearing you mask")
zumi.play_note(26, 150)
zumi.play_note(26, 250)
zumi.play_note(27, 500)
zumi.play_note(28, 150)
from zumi.zumi import Zumi zumi = Zumi() zumi.reverse(speed=5, duration=2)
from zumi.zumi import Zumi
import numpy as np
import time
zumi = Zumi()
zumi.reset_drive()
while True:
angle_list = zumi.update_angles()
x = round(angle_list[0], 3)
y = round(angle_list[1], 3)
z = round(angle_list[2], 3)
print("x : " + str(x) + ", y : " + str(y) + ", z : " + str(z))
time.sleep(0.1)
while (duration > time_elapsed):
# update the time that has passed
time_elapsed = time.time() - time_start
# take a step in that direction going forward
zumi.go_straight(speed, angle)
# once done stop zumi
zumi.hard_brake()
def route():
pass
#create the Zumi object
zumi = Zumi()
#Recablibrate Zumi just in case
zumi.mpu.calibrate_MPU()
#reset the gyro list
zumi.reset_gyro()
#initiate our starting location to 0,0
current_x = 0
current_y = 0
#you must point Zumi at the X axis. this will be 0 degrees
try:
route()
finally:
from zumi.zumi import Zumi import numpy as np zumi = Zumi() zumi.reset_coordinate() zumi.reset_drive() zumi.move_to_coordinate(desired_x=5, desired_y=0) zumi.move_to_coordinate(desired_x=0, desired_y=0)
from socket import *
from select import *
import sys
from time import ctime
import json
from zumi.zumi import Zumi
HOST = '192.168.0.5'
PORT = 10000
BUFSIZE = 1024
ADDR = (HOST,PORT)
clientSocket = socket(AF_INET, SOCK_STREAM)# ^d^| ^d ^w^p ^q ^f^m ^u^x ^|^d ^u^| ^f^l ^s ^}^d ^c^} ^d $
clientSocket.connect(ADDR)# ^d^| ^d ^w^p ^q ^f^m ^}^d ^k^| ^o^d ^u^| ^k .
zumi = Zumi()
while True :
data = clientSocket.recv(BUFSIZE)
print(data.decode())
tmp_str2 = data.decode().split(",")
if len(tmp_str2) != 2:
print("this is no data")
continue
l_speed = int(tmp_str2[0])
r_speed = int(tmp_str2[1])
print('l_speed {0} r_speed {1}'.format(l_speed,r_speed))
#-126 and 127 range
zumi.control_motors(l_speed, r_speed)
import Adafruit_SSD1306
import time
import os
from PIL import Image, ImageFont, ImageDraw
import smbus
import math
from zumi.zumi import Zumi
zumi = Zumi()
zumi.reset_gyro()
def reading_gyro(pos=''):
updateAngles = zumi.update_angles()
if (pos == 'x'):
return int(updateAngles[3])
elif (pos == 'y'):
return int(updateAngles[4])
elif (pos == 'z'):
return int(updateAngles[5])
else:
return 0
class GyroDraw():
# GyroDraw 클래스 초기화합니다.
def __init__(self):
from zumi.zumi import Zumi
import time
zumi = Zumi()
for i in range(0, 50):
vals = zumi.update_angles()
gyroX = " gyroX : " + str(round(vals[0], 2))
gyroY = ", gyroY : " + str(round(vals[1], 2))
gyroZ = ", gyroZ : " + str(round(vals[2], 2))
accX = ", accX : " + str(round(vals[3], 2))
accY = ", accY : " + str(round(vals[4], 2))
msg = gyroX + gyroY + gyroZ + accX + accY
print(msg)
time.sleep(0.1)
print("Done")
class GyroDraw():
# GyroDraw 클래스 초기화합니다.
def __init__(self):
# OELD 제어를 위한 설정
self.disp = Adafruit_SSD1306.SSD1306_128_64(rst=24)
self.disp.begin()
self.image = Image.new('1', (self.disp.width, self.disp.height))
self.draw = ImageDraw.Draw(self.image)
# 포인터의 크기
self.size = 3
# 자이로 X,Y 각도 값
self.oldCharX = 255
self.oldCharY = 255
# 펜의 좌표 X,Y 값
self.penX = 0
self.penY = 0
# 자이로 센서
self.zumi = Zumi()
self.zumi.reset_gyro()
# OLED 화면에 작은 사각형태의 점을 표시합니다.(포인터)
def draw_xy(self, posX=0, posY=0):
# 사각형으로 포인터를 그리며, 위치를 구분하기 위해 현재 위치는 원으로 표시합니다.
self.draw.rectangle(
(self.oldCharX - 1, self.oldCharY, self.oldCharX + self.size,
self.oldCharY + self.size),
outline=1,
fill=1)
self.draw.ellipse(
(posX, posY, posX + self.size + 2, posY + self.size + 2),
outline=1,
fill=0)
self.disp.image(self.image)
self.disp.display()
self.oldCharX = posX
self.oldCharY = posY
# OLED 화면에 십자선을 그립니다.
def draw_crossline(self):
self.draw.line((0, 32, 128, 32), fill=255)
self.draw.line((64, 0, 64, 64), fill=255)
self.disp.image(self.image)
self.disp.display()
# 센서에서 읽은 데이터를 계산하여 자이로 각도 X,Y 값을 반환합니다.
def get_angle(self):
updateAngles = self.zumi.update_angles()
angles = [0, 0]
angles[0] = -1 * int(updateAngles[3])
angles[1] = -1 * int(updateAngles[4])
return angles
# 자이로 X,Y 각도 값의 이전 상태와 현재 상태를 비교하여 펜의 좌표를 반환합니다.
def pen_xy(self):
# 자이로 각도 값 가져오기
angle = self.get_angle()
penSpeed = 3
# X ,Y축의 자이로 기울기에 따른 펜의 좌표 값
if (angle[0] > 10):
self.penX = self.penX + penSpeed
elif (angle[0] < -10):
self.penX = self.penX - penSpeed
if (angle[1] > 10):
self.penY = self.penY - penSpeed
elif (angle[1] < -10):
self.penY = self.penY + penSpeed
# 펜이 화면 밖으로 나가지 않도록 좌표 값 크기 제어
if (self.penX > 59):
self.penX = 59
elif (self.penX < -59):
self.penX = -59
if (self.penY > 27):
self.penY = 27
elif (self.penY < -27):
self.penY = -27
angle[0] = self.penX
angle[1] = self.penY
return angle
# 입력된 값을 원하는 범위의 스케일로 변환
def scale_change(self, x, in_min, in_max, out_min, out_max):
return int((x - in_min) * (out_max - out_min) / (in_max - in_min) +
out_min)
# 화면 필셀 제거
def clear_screen(self):
self.draw.rectangle((0, 0, self.disp.width, self.disp.height),
outline=0,
fill=0)
self.disp.image(self.image)
self.disp.display()
def acc_text(string='', line=25, count=100, font_size=16):
import Adafruit_SSD1306
import time
import os
from PIL import Image, ImageFont, ImageDraw
length = len(string)
zumi = Zumi()
zumi.reset_gyro()
state = 0
TEXT_FILE_PATH = os.path.dirname(
os.path.abspath('__file__')) + "/module/futura.ttf"
disp = Adafruit_SSD1306.SSD1306_128_64(rst=24)
#print(length)
disp.begin()
width = disp.width
height = disp.height
screen_image = None
image = Image.new('1', (width, height))
font = ImageFont.truetype(TEXT_FILE_PATH, font_size)
draw = ImageDraw.Draw(image)
disp.clear()
disp.display()
size = draw.textsize(string, font=font)
if (line >= 0 and line <= 45):
if (size[0] > 128):
print("The string entered is too long.")
else:
draw.text((50, line), string, font=font, fill=255)
disp.image(image)
disp.display()
disp.command(0x2E) #SSD1306_DEACTIVATE_SCROLL
for i in range(0, count):
angles = zumi.update_angles()
time.sleep(0.1)
if (angles[3] > 5):
if (state != 1):
state = 1
#loop_text('L', string)
disp.command(0x27) #SSD1306_LEFT_HORIZONTAL_SCROLL
disp.command(0x00) #dummy
disp.command(0x00) #start
disp.command(0x00)
disp.command(0x0f) #stop
disp.command(0x00)
disp.command(0xff)
disp.command(0x2f) #activate scroll
elif (angles[3] < -5):
if (state != 2):
state = 2
#loop_text('R', string)
disp.command(0x26) #SSD1306_RIGHT_HORIZONTAL_SCROLL
disp.command(0x00) #dummy
disp.command(0x00) #start
disp.command(0x00)
disp.command(0x0f) #stop
disp.command(0x00)
disp.command(0xff)
disp.command(0x2f) #activate scroll
else:
if (state != 0):
state = 0
disp.command(0x2E) #SSD1306_DEACTIVATE_SCROLL
disp.command(0x2E) #SSD1306_DEACTIVATE_SCROLL
else:
print('Lines can be entered from 0 to 45.')
def predict(model,Reroute=False):
c = Crop()
eye = Screen()
zumi = Zumi()
camera = Camera(64, 64)
personality = Personality(zumi, eye)
cnt = 0
prev_label = -1
cnt_none_crop = 0
try:
while True:
img = camera.run()
crop_img = c.crop(img)
if crop_img is None:
if cnt_none_crop == 0:
personality.look_around_open01()
elif cnt_none_crop == 1:
personality.look_around_open02()
elif cnt_none_crop == 2:
personality.look_around_open03()
elif cnt_none_crop == 3:
personality.look_around_open04()
prev_label = -1
print(cnt, prev_label, cnt_none_crop)
cnt_none_crop += 1
cnt_none_crop %= 4
continue
else:
cnt_none_crop = 0
x = Image.fromarray(crop_img)
x = np.expand_dims(x, axis=0)
preds = model.predict_classes(x)
print(landmark[preds[0]])
if prev_label == preds[0]:
cnt += 1
if cnt > 2:
print(eye.EYE_IMAGE_FOLDER_PATH + "sad1.ppm")
print("reaction!!!!")
eye.draw_image(eye.path_to_image(LAND_PATH + landmark[preds[0]] + ".jpg"))
time.sleep(2)
route = Route_new()
if Reroute:
print("go with rerouting")
if landmark[preds[0]] == 'eiffel':
route.driving(route.start_node, route.paris)
elif landmark[preds[0]] == 'nyc':
route.driving(route.start_node, route.NY)
elif landmark[preds[0]] == 'seattle':
route.driving(route.start_node, route.seattle)
elif landmark[preds[0]] == 'china':
route.driving(route.start_node, route.china)
else:
route.driving(route.start_node, route.bigben)
zumi.stop()
personality.celebrate()
time.sleep(.5)
else:
print("no reroute")
if landmark[preds[0]] == 'eiffel':
route.driving_without_reroute(route.start_node, route.paris)
route.park_right()
elif landmark[preds[0]] == 'nyc':
route.driving_without_reroute(route.start_node, route.NY)
route.park_right()
elif landmark[preds[0]] == 'seattle':
route.driving_without_reroute(route.start_node, route.seattle)
route.park_left()
elif landmark[preds[0]] == 'china':
route.driving_without_reroute(route.start_node, route.china)
route.park_left()
else:
route.driving_without_reroute(route.start_node, route.bigben)
route.park_right()
zumi.stop()
personality.celebrate()
time.sleep(.5)
cnt = 0
else:
personality.reading(eye.path_to_image(READ_PATH + "reading_" + str(cnt) + ".PPM"))
else:
cnt = 0
prev_label = preds[0]
except KeyboardInterrupt:
camera.shutdown()
eye.draw_text("")
zumi.stop()
print("\nExiting...")
except:
camera.shutdown()
eye.draw_text("")
zumi.stop()
print("\nExiting...")