![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
#author("2024-08-12T20:58:09+09:00","default:TESLA202407","TESLA202407") #author("2024-08-12T21:00:16+09:00","default:TESLA202407","TESLA202407") [[Real2Virtual202111]] * main_controller_01.py [#a780d24c] - GUI -- &ref(main_controller_01.py/main_controller_01_gui.png,50%); #code(Python){{ import math from tkinter import * import tkinter.font as tkFont import matrix ## License: Apache 2.0. See LICENSE file in root directory. ## Copyright(c) 2017 Intel Corporation. All Rights Reserved. #20240710 ##################################################### ## Align Depth to Color ## ##################################################### # First import the library import os,sys #import pyrealsense2 as rs # Import Numpy for easy array manipulation #import numpy as np # Import OpenCV for easy image rendering #import cv2 import copy import threading import re import socket import threading import sys from tkinter import * import tkinter as tk from tkinter import scrolledtext #from PIL import Image, ImageTk, ImageOps import tkinter.ttk as ttk from tkinter import messagebox from tkinter import filedialog import time import readchar from Cube import Cube class Space: ''' Coordiante-system: Right-handed, Matrices are column-major ones ''' WIDTH = 1280.0 HEIGHT = 960.0 SPEED = 0.05 EPSILON = 1.0e-8 ZERO = EPSILON import threading PORT = 9998 cxyz=[ [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]] ] pcx=3 pcy=3 pcz=3 class Com: def __init__(self,host,window): self.host=host self.connected=False self.soc=None self.handle_thread=None self.message_window=window def set_message_window(self,window): print("set_message_window",end='') print(self.window) self.message_window=window def is_connected(sekf): return self.connected def connect_address(self,address): self.host=address self.connect() def connect(self): print("re connect to host "+self.host) if self.soc !=None: print("close the previous host, "+self.soc.gethostname()) try: self.soc.close() self.soc=None time.sleep(2.0) except: print("close failed") try: #socket.socket ... socket を作成し、socに代入 self.soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #IPアドレスがhostのserver socketにsocを接続 self.soc.connect((self.host,PORT)) except Exception as e: print("connect error.") if self.message_window: self.message_window.write_message("error when connect to "+self.host+":"+str(PORT)) self.connected=False #受信担当の関数handlerを物threadでうごかすhandle_threadを生成。 self.handle_thread=threading.Thread(target=self.handler) #handle_threadをstart self.handle_thread.start() self.connected=True if self.message_window: self.message_window.write_message("connected to "+self.host+":"+str(PORT)) def send_command(self,command): if self.soc!=None: try: self.soc.send(bytes(command,"utf-8")) except KeyboardInterrupt: if self.message_window: self.message_window.write_message("error when sends "+command + " to "+self.host+":"+str(PORT)) self.soc.close() exit() else: if self.host is not None: self.message_window.write_message("no socket when sends "+command + " to "+self.host+":"+str(PORT)) else: self.message_window.write_message("no host assigned when sends "+command) #受信の処理。送信threadとは別に、並行して処理を行う。 def handler(self): print("at client, connected, start handler") while self.soc is not None: try: data = self.soc.recv(1024) line="[receive]- {}".format(data.decode("utf-8")) print(line) #print(self.message_window) if self.message_window: self.message_window.write_message("from "+self.host+":"+str(PORT)+", "+line) if self.message_window.recv_queue!=None: self.message_window.recv_queue.append(line) except Exception as e: if self.message_window: self.message_window.write_message(self.host+":"+str(PORT)+" closed") if self.soc is not None: self.soc.close() self.soc=None tb=sys.exec_info()[2] print("message:{0}".format(e.with_traceback(tb))) break def close(self): self.soc.close() self.soc=None pjxy=[[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]] pjyz=[[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]] pjzx=[[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]] class Coms: def __init__(self): self.coms=[None,None,None,None,None,None,None,None,None] #self.coms=[None,None, ...., None] #... number of sub controllers def connect_all(self): self.coms[2]=Com("192.168.14.10" ,self.message_window) # 2: x self.coms[0]=Com("192.168.14.11", self.message_window) # 0: y-l self.coms[1]=Com("192.168.14.12",self.message_window) # 1: y-r self.coms[3]=Com("192.168.14.13",self.message_window) # 3:b-0 self.coms[4]=Com("192.168.14.14",self.message_window) # 4:m-0 self.coms[5]=Com("192.168.14.15",self.message_window) # 5:m-1 self.coms[6]=Com("192.168.14.16",self.message_window) # 5:m-2 self.coms[7]=Com("192.168.14.17",self.message_window) # 5:m-3 self.coms[8]=Com("192.168.14.18",self.message_window) # 8:a-0 # #self.coms[0]=Com("192.168.1.34",self.message_window) # 0:y-l #self.coms[0]=Com("192.168.13.18",self.message_window) #self.coms[1]=Com("192.168.1.8",self.message_window) # 1:y-r #self.coms[2]=Com("192.168.1.10",self.message_window) # 2:x #self.coms[3]=Com("192.168.13.131",self.message_window) #self.coms[4]=Com("192.168.13.132") #self.coms[5]=Com("192.168.13.133") #self.coms[6]=Com("192.168.13.134") #self.coms[7]=Com("192.168.13.16") #self.coms[7]=Com("192.168.13.135") #self.coms[7]=Com("192.168.1.22") cwxyz=[ [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]], [[0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0], [0,0,0,0,0,0,0]] ] def clear_cxyz(self): for i in range(7): for j in range(7): for k in range(7): self.cxyz[i][j][k]=0 def make_pjxy(self): for i in range(7): for j in range(7): self.pjxy[i][j]=0 for k in range(7): self.pjxy[i][j]+=self.cxyz[i][j][k] print("pjxy") for i in range(7): print(self.pjxy[i]) def make_pjyz(self): for i in range(7): for j in range(7): self.pjyz[i][j]=0 for k in range(7): self.pjyz[i][j]+=self.cxyz[k][i][j] print("pjyz") for i in range(7): print(self.pjyz[i]) def make_pjzx(self): for i in range(7): for j in range(7): self.pjzx[i][j]=0 for k in range(7): self.pjzx[i][j]+=self.cxyz[j][k][i] print("pjzx") for i in range(7): print(self.pjzx[i]) def make_cxyz(self): self.clear_cxyz() for cx in self.polygons_3D: ccp=cx.get_cube_center() self.cxyz[round(ccp.x)+self.pcx][round(ccp.y)+self.pcy][round(ccp.z)+self.pcz]=1 print("cxyz") for i in range(7): print(self.cxyz[i]) def rotate_z_cw_90(self): #for k in range(7): # for i in range(7): # for j in range(7): # self.cwxyz[j][6-i][k]=self.cxyz[i][j][k] #for i in range(7): # for j in range(7): # for k in range(7): # self.cxyz[i][j][k]=self.cwxyz[i][j][k] for cx in self.polygons_3D: ccp=cx.get_cube_center() cx.rotate(0.0,0.0,90.0) cx.moveTo(ccp.y,-ccp.x,ccp.z) self.update(self.polygons_3D) def rotate_x_cw_90(self): #for k in range(7): # for i in range(7): # for j in range(7): # self.cwxyz[k][j][6-i]=self.cxyz[k][i][j] #for i in range(7): # for j in range(7): # for k in range(7): # self.cxyz[i][j][k]=self.cwxyz[i][j][k] for cx in self.polygons_3D: ccp=cx.get_cube_center() cx.rotate(90.0,0.0,0.0) cx.moveTo(ccp.x,ccp.z,-ccp.y) self.update(self.polygons_3D) def rotate_y_cw_90(self): #for k in range(7): # for i in range(7): # for j in range(7): # self.cwxyz[6-i][k][j]=self.cxyz[j][k][i] #for i in range(7): # for j in range(7): # for k in range(7): # self.cxyz[i][j][k]=self.cwxyz[i][j][k] for cx in self.polygons_3D: ccp=cx.get_cube_center() cx.rotate(0.0,90.0,0.0) cx.moveTo(-ccp.z,ccp.y,ccp.x) self.update(self.polygons_3D) def max_width(self): max_width=-10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.x>max_width: max_width=ccp.x return max_width def min_width(self): min_width=10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.x<min_width: min_width=ccp.x return min_width def width(self): return self.max_width()-self.min_width() +1.0 def max_height(self): max_height=-10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.y>max_height: max_height=ccp.y return max_height def min_height(self): min_height=10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.y<min_height: min_height=ccp.y return min_height def height(self): return self.max_height()-self.min_height() +1.0 def max_depth(self): max_depth=-10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.z>max_depth: max_depth=ccp.z return max_depth def min_depth(self): min_depth=10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.z<min_depth: min_depth=ccp.z return min_depth def depth(self): return self.max_depth()-self.min_depth() +1.0 def move_and_rotate_for_factory(self): wx=self.width() if wx<0.0: print("no tesla dice.") for i in range(len(self.coms)): if self.coms[i]!=None: self.coms[i].connect() def connect_com_to_address(self,i,address): print("connect_com_to_address("+str(i)+","+address+")") if i<0 : print("wrong plane number") return hx=self.height() if hx>wx: self.rotate_z_cw_90() wx=self.width() hx=self.height() dx=self.depth() if hx>dx: self.rotate_x_cw_90() wx=self.width() hx=self.height() dx=self.depth() if dx>wx: self.rotate_y_cw_90() wx=self.width() hx=self.height() dx=self.depth() if self.there_is_over_hang(): self.rotate_x_cw_90() self.rotate_x_cw_90() wx=self.width() hx=self.height() dx=self.depth() if self.there_is_space_at_the_first_corner(): if hx<=1.01: self.rotate_x_cw_90() self.rotate_x_cw_90() if i>len(self.coms) : print("wrong plane number") try: if self.coms[i] is not None: print("coms["+str(i)+"] is not None, connect, ") self.coms[i].connect_address(address) else: self.rotate_y_cw_90() self.rotate_y_cw_90() print("coms["+str(i)+"] is None, connect, ") self.coms[i]=Com(address,self.message_window) self.coms[i].connect() #self.coms[i].set_message_window(self.message_window) except Exception as e: print("coms["+str(i)+"] connect error") tb=sys.exec_info()[2] print("message:{0}".format(e.with_traceback(tb))) def min_x(self): min_x=10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.x<min_x: min_x=ccp.x return min_x def min_y(self): min_y=10.0 for cx in self.polygons_3D: ccp=cx.get_cube_center() if ccp.y<min_y: min_y=ccp.y return min_y def there_is_over_hang(self): self.make_cxyz() imin_y=round(self.min_y()) for i in range(7): for j in range(7): if self.cxyz[i][imin_y][j]==0: if imin_y<6: if self.cxyz[i][imin_y+1][j]==1: return True def there_is_space_at_the_first_corner(self): self.make_cxyz() imin_y=round(self.min_y()) imin_x=6 imin_z=6 for i in range(7): for j in range(7): if self.cxyz[i][imin_y][j]==1: if i<imin_x: imin_x=i if j<imin_z: imin_z=j fc=self.cxyz[imin_x][imin_y][imin_z] return fc==0 #ダイスの回転 # org: top red, forward green, left yellow, right black, back blue, bottom white # #top red の場合 # forward green : 回転無し。 # yellow : 右回転 ３回 # blue : 右回転２回 # black : 右回転１回 # #top white の場合 # 縦回転: 2回 # forward green : 右回転２回 # yellow : 右回転 1回 # blue : 回転無し # black : 右回転３回 # #top blue の場合 # 縦回転: １回 # forward red : 回転なし # yellow : 右回転 ３回 # white : 右回転２回 # black : 右回転１回 # #top yellow の場合 # 右回転１回 # 縦回転: １回 # forward red : 回転なし # blue : 右回転１回 # white : 右回転２回 # green : 右回転３回 # # top green の場合 # 右回転 ２回 # 縦回転: １回 # forward red : 回転なし # yellow : 右回転１回 # white : 右回転２回 # black : 右回転３回 # #top black の場合 # 右回転 3回 # 縦回転: １回 # forward red : 回転なし # green : 右回転１回 # white : 右回転２回 # blue : 右回転３回 def generate_rotation_procedure(self,cube): top_face_id=cube.get_top_face() top_face_color=cube.cls[top_face_id] front_face_id=cube.get_front_face() front_face_color=cube.cls[front_face_id] rotation_proc="" if top_face_color=='red': #top red の場合 # forward black : 回転無し。 # green : 右回転 ３回 # yellow : 右回転２回 # blue : 右回転１回 if front_face_color=='green': rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='yellow': rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='blue': rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='gray': rotation_proc='#no need to rotate\n' return rotation_proc elif top_face_color=='white': #top white の場合 # 縦回転: 2回 # forward black : 右回転２回 # blue : 右回転 1回 # yellow : 回転無し # green : 右回転３回 rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc=rotation_proc+'self.v_rotate_f()\nself.v_rotate_f()\n' if front_face_color=='green': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='yellow': return rotation_proc elif front_face_color=='blue': rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='gray': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif top_face_color=='yellow': #top yellow の場合 # 縦回転: １回 # forward red : 回転なし # : 右回転 ３回 # white : 右回転２回 # blue : 右回転１回 rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc=rotation_proc+'self.v_rotate_f()\n' if front_face_color=='green': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='red': return rotation_proc elif front_face_color=='blue': rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='white': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif top_face_color=='green': #top green の場合 # 右回転: 1回 # 縦回転: １回 # forward red : 回転なし # black : 右回転 ３回 # white : 右回転２回 # blue : 右回転１回 rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc=rotation_proc+'self.v_rotate_f()\n' if front_face_color=='gray': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='red': return rotation_proc elif front_face_color=='yellow': rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='white': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif top_face_color=='gray': #top green の場合 # 右回転: 2回 # 縦回転: １回 # forward red : 回転なし # blue : 右回転 ３回 # white : 右回転２回 # green : 右回転１回 rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc=rotation_proc+'self.v_rotate_f()\n' if front_face_color=='blue': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='red': return rotation_proc elif front_face_color=='green': rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='white': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif top_face_color=='blue': #top green の場合 # 右回転: 2回 # 縦回転: １回 # forward red : 回転なし # blue : 右回転 ３回 # white : 右回転２回 # green : 右回転１回 rotation_proc='self.place_at_the_rotation_point()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc='self.h_rotate_cw()\n' rotation_proc=rotation_proc+'self.v_rotate_f()\n' if front_face_color=='yellow': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc elif front_face_color=='red': return rotation_proc elif front_face_color=='gray': rotation_proc=rotation_proc+'self.h_rotate_cw()\n' return rotation_proc elif front_face_color=='white': rotation_proc=rotation_proc+'self.h_rotate_cw()\nself.h_rotate_cw()\n' return rotation_proc def set_message_window(self,window): #print(window) self.message_window=window for i in range(len(self.coms)): if self.coms[i]!=None: self.coms[i].set_message_window(window) else: print("coms["+str(i)+"] is None, set_message_window, ") def send_command_to_i(self,i,command): if self.coms[i]!=None: self.coms[i].send_command(command+"\n") def send_command_to_all(self,command): for i in range(len(self.coms)): if self.coms[i]!=None: self.coms[i].send_command(command+"\n") def close_all(self): for i in range(len(self.coms)): if self.coms[i]!=None: self.coms[i].close() dice_array_xz=[[0,0,0,0,0],[0,0,0,0,0],[0,0,0,0,0],[0,0,0,0,0],[0,0,0,0,0]] class Command_Panel: def __init__(self,root,coms): self.coms=coms self.recv_queue=[] self.root=root self.root.title("command panel") self.root.configure(background="white") self.root.geometry("850x800") self.notebook =ttk.Notebook(root) self.main_tab=tk.Frame(self.notebook, bg='white') self.script_tab=tk.Frame(self.notebook, bg='white') self.notebook.add(self.main_tab, text="main", underline=0) self.notebook.add(self.script_tab, text="script") self.notebook.pack(expand=True, fill='both', padx=10, pady=10) def init_assemble_info(self): for i in range(5): for j in range(5): self.dice_array_xz[i][j]=0 def generate_move_procedure(self,i,j,k): proc='' if k==0: if i==0 and j==0: proc=proc+'self.place_dice_at_0_0()\n' self.dice_array_xz[i][j]=1 return proc if i==1 and j==0: if self.dice_array_xz[0][0]==1: proc=proc+'self.place_dice_at_1_0_nx_00()\n' self.dice_array_xz[1][0]=1 return proc if i==2 and j==0: if self.dice_array_xz[1][0]==1: proc=proc+'self.place_dice_at_2_0_nx_10()\n' self.dice_array_xz[2][0]=1 return proc if i==3 and j==0: if self.dice_array_xz[2][0]==1: proc=proc+'self.place_dice_at_3_0_nx_20()\n' self.dice_array_xz[3][0]=1 return proc if i==0 and j==1: proc=proc+'self.place_dice_at_0_1()\n' self.dice_array_xz[i][j]=1 return proc if i==1 and j==1: if self.dice_array_xz[0][1]==1: proc=proc+'self.place_dice_at_1_1_nx_01()\n' self.dice_array_xz[1][1]=1 return proc if self.dice_array_xz[0][1]==0: proc=proc+'self.place_dice_at_1_1_nx_01()\n' self.dice_array_xz[1][1]=1 return proc if i==2 and j==1: if self.dice_array_xz[1][0]==1: proc=proc+'self.place_dice_at_2_1_nx_11()\n' self.dice_array_xz[2][1]=1 return proc if self.dice_array_xz[1][0]==0: proc=proc+'self.place_dice_at_2_1_nx_11()\n' self.dice_array_xz[2][1]=1 return proc if i==3 and j==1: if self.dice_array_xz[2][0]==1: proc=proc+'self.place_dice_at_3_1_nx_21()\n' self.dice_array_xz[3][1]=1 return proc if self.dice_array_xz[2][0]==0: proc=proc+'self.place_dice_at_3_1_nx_21()\n' self.dice_array_xz[3][1]=1 return proc if k==1: if i==0 and j==0: proc=proc+'self.place_dice_at_0_0_1()\n' self.dice_array_xz[i][j]=1 return proc if i==1 and j==0: if self.dice_array_xz[0][0]==1: proc=proc+'self.place_dice_at_1_0_1_nx_00()\n' self.dice_array_xz[1][0]=1 return proc if i==2 and j==0: if self.dice_array_xz[1][0]==1: proc=proc+'self.place_dice_at_2_0_1_nx_10()\n' self.dice_array_xz[2][0]=1 return proc if i==3 and j==0: if self.dice_array_xz[2][0]==1: proc=proc+'self.place_dice_at_3_0_1_nx_20()\n' self.dice_array_xz[3][0]=1 return proc if i==0 and j==1: proc=proc+'self.place_dice_at_0_1_1()\n' self.dice_array_xz[i][j]=1 return proc if i==1 and j==1: if self.dice_array_xz[0][1]==1: proc=proc+'self.place_dice_at_1_1_1_nx_01()\n' self.dice_array_xz[1][1]=1 return proc if i==2 and j==1: if self.dice_array_xz[1][0]==1: proc=proc+'self.place_dice_at_2_1_1_nx_11()\n' self.dice_array_xz[2][1]=1 return proc if i==3 and j==1: if self.dice_array_xz[2][0]==1: proc=proc+'self.place_dice_at_3_1_1_nx_21()\n' self.dice_array_xz[3][1]=1 return proc yy=10 self.connect_label=Label(self.main_tab,text="Servers",width=10) self.connect_label.place(x=30,y=yy) self.connect_button=Button(self.main_tab, text="connect",bd=2,bg='white',command=self.click_connect_button, width=8, relief=RIDGE) self.connect_button.place(x=100,y=yy) self.set_id_label=Label(self.main_tab,text="set id",width=10) self.set_id_label.place(x=200,y=yy) self.set_id_button=Button(self.main_tab, text="set id",bd=2,bg='white',command=self.click_set_id_button, width=8, relief=RIDGE) self.set_id_button.place(x=270,y=yy) self.clear_message_button=Button(self.main_tab, text="clear",bd=2,bg='white',command=self.clear_message, width=8, relief=RIDGE) self.clear_message_button.place(x=370,y=yy) def generate_assemble_procedure(self): min_y=round(self.min_height()) max_y=round(self.max_height()) min_x=round(self.min_width()) max_x=round(self.max_width()) min_z=round(self.min_depth()) max_z=round(self.max_depth()) proc='' for k in range(min_z, max_z+1): for j in range(min_y, max_y+1): for i in range(min_x, max_x+1): for cx in self.polygons_3D: ccx=cx.get_cube_center() iccx_x=round(ccx.x) iccx_y=round(ccx.y) iccx_z=round(ccx.z) if i==iccx_x and j==iccx_y and k==iccx_z: proc=proc+'self.get_dice_from_the_belt()\n' rproc=self.generate_rotation_procedure(cx) if rproc!='#no need to rotate\n': proc=proc+rproc mx=i-min_x mz=k-min_z my=j-min_y mproc=self.generate_move_procedure(mx,mz,my) proc=proc+mproc return proc yy=40 self.command_label=Label(self.main_tab,text="command", width=8) self.command_label.place(x=30,y=yy) self.command_field=Entry(self.main_tab,width=50) self.command_field.place(x=150,y=yy) self.enter_command_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_command,width=8,relief=RIDGE) self.enter_command_button.place(x=550,y=yy) self.run_script_button=Button(self.main_tab,text="run script",bd=2,bg='white',command=self.run_script,width=8,relief=RIDGE) self.run_script_button.place(x=650,y=yy) def __init__(self): print("start Space.__init__") self.root = Tk() self.root.resizable(False, False) self.root.title('3D') #self.chat_client=chat_client left = (self.root.winfo_screenwidth() - Space.WIDTH) / 2 top = (self.root.winfo_screenheight() - Space.HEIGHT) / 2 self.root.geometry('%dx%d+%d+%d' % (Space.WIDTH, Space.HEIGHT, left, top)) self.graph = Canvas(self.root, width=Space.WIDTH, height=Space.HEIGHT, background='black') self.graph.pack() self.walls = [matrix.Vector3D(7.0, 0.0, 0.0), matrix.Vector3D(-7.0, 0.0, 0.0), matrix.Vector3D(0.0, 7.0, 0.0), matrix.Vector3D(0.0, -7.0, 0.0), matrix.Vector3D(0.0, 0.0, 7.0), matrix.Vector3D(0.0, 0.0, -7.0)] self.wallsnormals = [matrix.Vector3D(-1.0, 0.0, 0.0), matrix.Vector3D(1.0, 0.0, 0.0), matrix.Vector3D(0.0, -1.0, 0.0), matrix.Vector3D(0.0, 1.0, 0.0), matrix.Vector3D(0.0, 0.0, -1.0), matrix.Vector3D(0.0, 0.0, 1.0)] ################# # self.cubenormals = [] # for i in range(len(self.cubefaces)): # poly = [] # for j in range(len(self.cubefaces[i])): # poly.append(self.cube[self.cubefaces[i][j]]) yy=70 self.command_field_label_1=Label(self.main_tab,text="command for ith", width=15) self.command_field_label_1.place(x=30,y=yy) self.command_field_for_ith_server=Entry(self.main_tab,width=40) self.command_field_for_ith_server.place(x=150,y=yy) self.ith_server_field=Entry(self.main_tab,width=5) self.ith_server_field.place(x=450,y=yy) self.ith_enter_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_ith_command,width=8,relief=RIDGE) self.ith_enter_button.place(x=550,y=yy) # self.cubenormals.append(self.calcNormalVec(poly)) ################# self.ang = [0.0, 0.0, 0.0] # phi(x), theta(y), psi(z) of the camera self.trans = [0.0, 0.0, 2.0] # translation (x, y, z) of the camera yy=100 self.command_field_label_2=Label(self.main_tab,text="command for all", width=15) self.command_field_label_2.place(x=30,y=yy) self.command_field_for_all_server=Entry(self.main_tab,width=50) self.command_field_for_all_server.place(x=150,y=yy) self.all_enter_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_all_command,width=8,relief=RIDGE) self.all_enter_button.place(x=550,y=yy) self.clear_message_button=Button(self.main_tab,text="clear_message",bd=2,bg='white',command=self.clear_message,width=8,relief=RIDGE) self.clear_message_button.place(x=650,y=yy) #The matrices (Scale, Shear, Rotate, Translate) apply to the View/Camera yy=130 self.message_frame=tk.Frame(self.main_tab,width=50,height=30) self.message_frame.place(x=30,y=yy) self.message_area=scrolledtext.ScrolledText(self.message_frame) self.message_area.pack() #The Scale Matrix self.Scale = matrix.Matrix(4, 4) Scalex = 1.0 Scaley = 1.0 Scalez = 1.0 self.Scale[(0,0)] = Scalex self.Scale[(1,1)] = Scaley self.Scale[(2,2)] = Scalez #The Shear Matrix self.Shearxy = matrix.Matrix(4, 4) self.Shearxy[(0,2)] = 0.0 self.Shearxy[(1,2)] = 0.0 self.Shearxz = matrix.Matrix(4, 4) self.Shearxz[(0,1)] = 0.0 self.Shearxz[(2,1)] = 0.0 self.Shearyz = matrix.Matrix(4, 4) self.Shearyz[(1,0)] = 0.0 self.Shearyz[(2,0)] = 0.0 self.Shear = self.Shearxy*self.Shearxz*self.Shearyz #The Rotation Matrices self.Rotx = matrix.Matrix(4,4) self.Roty = matrix.Matrix(4,4) self.Rotz = matrix.Matrix(4,4) #The Translation Matrix (will contain xoffset, yoffset, zoffset) self.Tr = matrix.Matrix(4, 4) self.Tr[(0,3)] = self.trans[0] self.Tr[(1,3)] = self.trans[1] self.Tr[(2,3)] = self.trans[2] #The Projection Matrix self.Proj = matrix.Matrix(4, 4) #foc controls how much of the screen is viewed fov = 60.0 #between 30 and 90 ? zfar = 100.0 znear = 0.1 S = 1/(math.tan(math.radians(fov/2))) A = Space.WIDTH/Space.HEIGHT self.Proj[(0,0)] = S/A self.Proj[(1,1)] = S self.Proj[(2,2)] = (zfar+znear)/(znear-zfar) self.Proj[(3,2)] = -1.0 self.Proj[(2,3)] = 2*(zfar*znear)/(znear-zfar) # self.Proj[(3,3)] = 0.0 #script tab yy=10 self.dir_name_label=Label(self.script_tab,text="dir name",width=10) self.dir_name_label.place(x=30,y=yy) self.dir_name_field=Entry(self.script_tab,width=50) self.dir_name_field.place(x=100,y=yy) #self.read_button=Button(self.script_tab, text="read",bd=2,bg='white',command=self.click_read_button, width=8, relief=RIDGE) #self.read_button.place(x=500,y=yy) self.dir_button=Button(self.script_tab, text="directory",bd=2,bg='white',command=self.click_dir_button, width=10, relief=RIDGE) self.dir_button.place(x=570,y=yy) #ToScreen Matrix self.toSC = matrix.Matrix(4, 4) self.toSC[(0,0)] = Space.WIDTH/2 self.toSC[(1,1)] = -Space.HEIGHT/2 self.toSC[(0,3)] = Space.WIDTH/2 self.toSC[(1,3)] = Space.HEIGHT/2 self.run_script_button2=Button(self.script_tab,text="run script",bd=2,bg='white',command=self.run_script,width=8, relief=RIDGE) self.run_script_button2.place(x=680,y=yy) yy=50 self.file_name_label=Label(self.script_tab,text="file name",width=10) self.file_name_label.place(x=30,y=yy) self.file_name_field=Entry(self.script_tab,width=50) self.file_name_field.place(x=100,y=yy) self.read_button=Button(self.script_tab, text="read",bd=2,bg='white',command=self.click_read_button, width=8, relief=RIDGE) self.read_button.place(x=500,y=yy) self.dir_button=Button(self.script_tab, text="select",bd=2,bg='white',command=self.click_select_file_button, width=10, relief=RIDGE) self.dir_button.place(x=570,y=yy) yy=100 self.script_frame=tk.Frame(self.script_tab,width=50,height=100) self.script_frame.place(x=50,y=yy) self.script_area=scrolledtext.ScrolledText(self.script_frame) self.script_area.pack() # self.root.bind("<B1-Motion>", self.dragcallback) # self.root.bind("<ButtonRelease-1>", self.releasecallback) self.root.bind("<Key>", self.keycallback) # self.root.bind("<KeyRelease>", self.keyreleasecallback) self.fnt = tkFont.Font(family='Helvetica', size=12, weight='bold') cube1=Cube(self) cube1.set_name("s*") self.cube1=cube1 cc1=cube1.get_cube_center() self.cxyz[round(cc1.x)+self.pcx][round(cc1.y)+self.pcy][round(cc1.z)+self.pcz]=1 print(self.cxyz) self.polygons_3D=[cube1] self.update(self.polygons_3D) #self.client_start() #comment out for debug #self.mainloop() def start_3d(self): self.root.mainloop() #self.run_script() self.set_script() self.display_help() def lookUpCube(self,name): for i in range(len(self.polygons_3D)): cx=self.polygons_3D[i] if name==cx.name : return cx return None def v_rotate_f(self): print("virtical_rotate") #--- virtical rotate self.send_arm_motion_command("7") #--- grasp --- time.sleep(2.0) self.send_magnet_command(0,"go") time.sleep(3.0) self.send_arm_motion_command("8") #--- virtical rotate, top to front time.sleep(7.0) self.send_magnet_command(1,"back") time.sleep(4.0) self.send_move_command(28.0,85.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],20.0) time.sleep(5.0) self.send_arm_motion_command("9") #--- release --- time.sleep(6.0) self.send_magnet_command(0,"back") self.send_magnet_command(1,"back") self.send_move_command(28.0,77.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],20.0) self.send_magnet_command(0,"stop") self.send_magnet_command(1,"stop") self.send_arm_motion_command("5") #--- to home--- time.sleep(6.0) self.send_move_command(28.0,100.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],20.0) time.sleep(2.0) def update(self,polygons_3D): # Main simulation loop. self.graph.delete(ALL) def h_rotate_cw(self): print("horizontal rotate, clock wise.") #---- horizontal rotate , clock wise self.send_move_command(28.0,110.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],20.0) self.send_arm_motion_command("17") #--- grasp for rotate, clock-wize---\n") self.send_magnet_command(0,"go") time.sleep(2.0) self.send_magnet_command(1,"back") self.send_arm_motion_command("18") #--- horizontal rotate, clock-wise time.sleep(10.0) self.send_magnet_command(1,"stop") self.send_magnet_command(0,"stop") time.sleep(8.0) self.send_magnet_command(0,"back") self.send_magnet_command(1,"back") time.sleep(2.0) self.send_arm_motion_command("16") #--- release time.sleep(8.0) self.send_arm_motion_command("5") #--- to home--- time.sleep(2.0) self.send_magnet_command(0,"stop") self.send_magnet_command(1,"stop") self.send_move_command(28.0,100.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],20.0) time.sleep(2.0) def place_at_the_rotation_point(self): print("place the dice at the rotation point.") #--- move to rotate point self.send_move_command(0.0,100.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_move_command(28.0,100.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #---- place the dice self.send_magnet_command(0,"go") self.send_arm_motion_command("3") #--- from home to place-01 time.sleep(3.0) self.send_arm_motion_command("6") #--- place-02 time.sleep(4.0) self.send_magnet_command(1,"back") time.sleep(2.0) self.send_magnet_command(0,"back") self.send_arm_motion_command("4") #--- release 01 time.sleep(3.0) self.send_magnet_command(1,"stop") self.send_magnet_command(0,"stop") time.sleep(2.0) self.send_arm_motion_command("5") #--to home--- time.sleep(5.0) def get_at_the_rotation_point(self): print("get the dice at the rotation point.") self.send_move_command(28.0,100.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("7") #--- grasp --- time.sleep(2.0) self.send_magnet_command(0,"go") self.send_magnet_command(1,"go") time.sleep(6.0) self.send_arm_motion_command("5") #--- to home --- time.sleep(9.0) self.send_magnet_command(0,"stop") self.send_magnet_command(1,"stop") def place_at_0_0(self): print("place the dice at 0,0.") self.send_move_command(168.0,20.0) #--- place on the destination self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #---- place the dice time.sleep(3.0) self.send_arm_motion_command("21") #--- from home to place-0-0 time.sleep(3.0) self.send_move_command(168.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #--- place on the destination self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(2.0) self.send_arm_motion_command("19") #--- release at place-0-0 time.sleep(4.0) self.send_magnet_command(0,"stop") self.send_magnet_command(3,"stop") self.send_arm_motion_command("22") #--- to home at place-0-0 #---- next dice time.sleep(5.0) self.send_move_command(168.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_1_0_nx_00(self): print("place the dice at 1,0, next to the dice at 0,0.") self.send_move_command(113.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(128.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(128.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_2_0_nx_10(self): print("place the dice at 2,0, next to the dice at 1,0.") self.send_move_command(73.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(88.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(88.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_3_0_nx_20(self): print("place the dice at 2,0, next to the dice at 2,0.") self.send_move_command(33.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(48.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(48.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_0_1(self): print("place the dice at 0,1.") self.send_move_command(168.0,60.0) #--- place on the destination self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #---- place the dice time.sleep(3.0) self.send_arm_motion_command("21") #--- from home to place-0-0 time.sleep(3.0) self.send_move_command(168.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #--- place on the destination self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(2.0) self.send_arm_motion_command("19") #--- release at place-0-0 time.sleep(4.0) self.send_magnet_command(0,"stop") self.send_magnet_command(3,"stop") self.send_arm_motion_command("22") #--- to home at place-0-0 #---- next dice time.sleep(5.0) self.send_move_command(168.0, 60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_1_1_nx_01(self): print("place the dice at 1,0, next to the dice at 0,0.") self.send_move_command(113.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(128.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(128.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_2_1_nx_11(self): print("place the dice at 2,0, next to the dice at 1,0.") self.send_move_command(73.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(88.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(88.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_3_1_nx_21(self): print("place the dice at 2,0, next to the dice at 2,0.") self.send_move_command(33.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(48.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(48.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_0_0_1(self): print("place the dice at 0,0.") self.send_move_command(168.0,20.0) #--- place on the destination self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #---- place the dice time.sleep(3.0) self.send_arm_motion_command("21") #--- from home to place-0-0 time.sleep(3.0) self.send_move_command(168.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #--- place on the destination self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(2.0) self.send_arm_motion_command("19") #--- release at place-0-0 time.sleep(4.0) self.send_magnet_command(0,"stop") self.send_magnet_command(3,"stop") self.send_arm_motion_command("22") #--- to home at place-0-0 #---- next dice time.sleep(5.0) self.send_move_command(168.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_1_0_1_nx_00(self): print("place the dice at 1,0, next to the dice at 0,0.") self.send_move_command(113.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(128.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(128.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_2_0_1_nx_10(self): print("place the dice at 2,0, next to the dice at 1,0.") self.send_move_command(73.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(88.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(88.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_3_0_1_nx_20(self): print("place the dice at 2,0, next to the dice at 2,0.") self.send_move_command(33.0,20.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(48.0,10.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(48.0,20.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_0_1_1(self): print("place the dice at 0,1.") self.send_move_command(168.0,60.0) #--- place on the destination self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #---- place the dice time.sleep(3.0) self.send_arm_motion_command("21") #--- from home to place-0-0 time.sleep(3.0) self.send_move_command(168.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) #--- place on the destination self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(2.0) self.send_arm_motion_command("19") #--- release at place-0-0 time.sleep(4.0) self.send_magnet_command(0,"stop") self.send_magnet_command(3,"stop") self.send_arm_motion_command("22") #--- to home at place-0-0 #---- next dice time.sleep(5.0) self.send_move_command(168.0, 60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_1_1_1_nx_01(self): print("place the dice at 1,0, next to the dice at 0,0.") self.send_move_command(113.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(128.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(128.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_2_1_1_nx_11(self): print("place the dice at 2,0, next to the dice at 1,0.") self.send_move_command(73.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(88.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(88.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def place_at_3_1_1_nx_21(self): print("place the dice at 2,0, next to the dice at 2,0.") self.send_move_command(33.0,60.0) # little bit before at the place self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_arm_motion_command("20") #--- down --- time.sleep(4.0) self.send_move_command(48.0,50.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_magnet_command(0,"back") self.send_magnet_command(3,"back") time.sleep(1.0) self.send_arm_motion_command("19") #--- release 01 time.sleep(2.0) self.send_arm_motion_command("22") time.sleep(2.0) self.send_magnet_command(3,"stop") self.send_magnet_command(0,"stop") self.send_move_command(48.0,60.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) def get_dice_from_the_belt(self): self.send_move_command(0.0,0.0) self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],60.0) self.send_init_belt_command() self.send_arm_motion_command("1") time.sleep(4.0) self.send_magnet_command(0,"go") time.sleep(3.0) self.send_stop_belt_command() time.sleep(1.0) self.send_arm_motion_command("2") #--- from home to place-01 time.sleep(8.0) self.send_magnet_command(0,"stop") self.Rotx[(1,1)] = math.cos(math.radians(360.0-self.ang[0])) self.Rotx[(1,2)] = -math.sin(math.radians(360.0-self.ang[0])) self.Rotx[(2,1)] = math.sin(math.radians(360.0-self.ang[0])) self.Rotx[(2,2)] = math.cos(math.radians(360.0-self.ang[0])) def set_script(self): self.clear_message() #---- start self.script_area.insert(tk.END,"time.sleep(10.0)\n") #---- pic a dice self.script_area.insert(tk.END,"self.get_dice_from_the_belt()\n") #--- move to rotate point self.script_area.insert(tk.END,"self.place_at_the_rotation_point() #---- place the dice at the rotation point\n") self.script_area.insert(tk.END,"time.sleep(5.0)\n") #--- virtical rotate self.script_area.insert(tk.END,"self.v_rotate_f() #---- virtical rotate \n") self.script_area.insert(tk.END,"time.sleep(5.0)\n") #------ horizontal rotate self.script_area.insert(tk.END,"self.h_rotate_cw() #--- horizontal rotate\n") self.script_area.insert(tk.END,"time.sleep(3.0)\n") self.script_area.insert(tk.END,"self.get_at_the_rotation_point()\n") #... grasp the dice at the rotation point #--- move to the destination self.script_area.insert(tk.END, "time.sleep(4.0)\n") self.script_area.insert(tk.END,"self.place_at_0_0()\n") #--- to next dice self.script_area.insert(tk.END,"self.send_move_command(0.0,20.0)\n") self.script_area.insert(tk.END,"self.wait_until_receive(['done mv-x','done mv-yl','done mv-yr'],80.0)\n") self.script_area.insert(tk.END,"self.get_dice_from_the_belt() #---- get the next dice \n") self.script_area.insert(tk.END,"self.place_at_the_rotation_point() #---- place the dice at the rotation point\n") self.script_area.insert(tk.END,"self.h_rotate_cw() #--- horizontal rotate\n") self.script_area.insert(tk.END,"time.sleep(5.0)\n") self.script_area.insert(tk.END,"self.h_rotate_cw() #--- horizontal rotate\n") self.script_area.insert(tk.END,"time.sleep(5.0)\n") self.script_area.insert(tk.END,"self.get_at_the_rotation_point()\n") #... grasp the dice at the rotation point #--- move to the destination self.script_area.insert(tk.END,"time.sleep(4.0)\n") self.script_area.insert(tk.END,"self.place_at_1_0_nx_00()\n") self.Roty[(0,0)] = math.cos(math.radians(360.0-self.ang[1])) self.Roty[(0,2)] = math.sin(math.radians(360.0-self.ang[1])) self.Roty[(2,0)] = -math.sin(math.radians(360.0-self.ang[1])) self.Roty[(2,2)] = math.cos(math.radians(360.0-self.ang[1])) self.Rotz[(0,0)] = math.cos(math.radians(360.0-self.ang[2])) self.Rotz[(0,1)] = -math.sin(math.radians(360.0-self.ang[2])) self.Rotz[(1,0)] = math.sin(math.radians(360.0-self.ang[2])) self.Rotz[(1,1)] = math.cos(math.radians(360.0-self.ang[2])) def click_read_button(self): fn=self.file_name_field.get() print("fn="+fn) f=None try: f=open(fn,"r") except Exception as e: print("file open error.") tb=sys.exec_info()[2] print("message:{0}".format(e.with_traceback(tb))) return try: self.script_area.delete(1.0,END) line=f.readline() while line: print(line) self.script_area.insert(END,line) line=f.readline() f.close() except Exception as e: print("file "+fn+" read error") tb=sys.exec_info()[2] print("message:{0}".format(e.with_traceback(tb))) def click_dir_button(self): iDir = os.path.abspath(os.path.dirname(__file__)) folder_name=tk.filedialog.askdirectory(initialdir=iDir) if len(folder_name)==0: print("cancel selcting folder") else: self.dir_name_field.insert(END,folder_name) def click_select_file_button(self): fType = [("python", "*.py")] iDir="" xdir=self.dir_name_field.get() if xdir==None or xdir=="": iDir=os.path.abspath(os.path.dirname(__file__)) else: iDir=xdir iFilePath=filedialog.askopenfilename(filetypes=fType, initialdir=iDir) if iFilePath==None or iFilePath=="": print("cancel") else: self.file_name_field.delete(0,END) self.file_name_field.insert(END,iFilePath) def enter_command(self): print("enter_command") command=self.command_field.get() self.ex(command) #The Rotation matrix self.Rot = self.Rotx*self.Roty*self.Rotz def run_script(self): print('run_script') #exec(self.script) script_thread=threading.Thread(target=self.start_script) print('new thread, start thread') #self.start_script() script_thread.start() #Translation (just copying) self.Tr[(0,3)] = -self.trans[0] self.Tr[(1,3)] = -self.trans[1] self.Tr[(2,3)] = -self.trans[2] def start_script(self): print('start_script') self.script=self.script_area.get(0.,END) print("run_script:"+self.script) exec(self.script) #The Transformation matrix self.Tsf = self.Scale*self.Shear*self.Rot*self.Tr #Computing the normals in __init__ and only rotating them here is the same. # print '+++++++++++++++' # for i in range(len(self.cubenormals)): # print self.Rot*self.cubenormals[i] # print '+++++++++++++++' # print '************' #Cube for ic in range(len(polygons_3D)): cubex=polygons_3D[ic] cubex.update() cubefaces=cubex.cubefaces cls=cubex.cls for i in range(len(cubefaces)): poly = [] #transformed polygon for j in range(len(cubefaces[0])): cube=cubex.cube v = cube[cubefaces[i][j]] #Scale, Shear, Rotate the vertex around X axis, then around Y axis, and finally around Z axis and Translate. r=self.Tsf*v poly.append(r) n=self.calcNormalVec(poly) #print n if not self.isPolygonFrontFace(poly[0], n): #Backface culling continue poly2d = [] inviewingvolume = False for j in range(len(poly)): #put the point from 3D to 2D ps =self.Proj*poly[j] # Puut the screenpoint in the list of transformed vertices p=self.toSC*ps x=int(p.x) y = int(p.y) poly2d.append((x, y)) #if only one point is in the screen (x[-1,1], y[-1,1], z[-1,1]) then draw the whole polygon if (-1.0 <= ps.x <= 1.0) and (-1.0 <= ps.y <= 1.0) and (-1.0 <= ps.z <= 1.0): inviewingvolume = True if inviewingvolume: self.graph.create_polygon(*poly2d, fill=cls[i]) cfx=cubex.cfaces xface=cfx[i] xface_led=xface[0] for k in range(len(xface_led)): #LED v=xface_led[k] r=self.Tsf*v ps=self.Proj*r p = self.toSC*ps x, y = int(p.x), int(p.y) self.graph.create_rectangle(x, y, x+3, y+3, fill="magenta") v=xface[1] #photo tr r=self.Tsf*v ps=self.Proj*r p=self.toSC*ps x,y=int(p.x), int(p.y) self.graph.create_rectangle(x,y, x+3, y+3, fill="cyan") # cfx=cubex.cfaces # for i in range(len(cfx)): # xface=cfx[i] # xface_led=xface[0] # for j in range(len(xface_led)): # v=xface_led[j] # r=self.Tsf*v # ps=self.Proj*r # p = self.toSC*ps # x, y = int(p.x), int(p.y) # self.graph.create_rectangle(x, y, x+3, y+3, fill="magenta") # v=xface[1] # r=self.Tsf*v # ps=self.Proj*r # p=self.toSC*ps # x,y=int(p.x), int(p.y) # self.graph.create_rectangle(x,y, x+3, y+3, fill="cyan") #Texts (these are in camera-CS) txt1 = 'xpos: '+str(self.trans[0])+' ypos: '+str(self.trans[1])+' zpos: '+str(self.trans[2]) txt2 = 'xrot: '+str(self.ang[0])+' yrot: '+str(self.ang[1])+' zrot: '+str(self.ang[2]) self.idTxt1 = self.graph.create_text(30,30, text=txt1, fill='white', anchor=SW, font=self.fnt) self.idTxt2 = self.graph.create_text(30,60, text=txt2, fill='white', anchor=SW, font=self.fnt) vFwd = self.getForwardVec2(self.ang[1]) txt3 = 'Fwd: '+str(vFwd) self.idTxt3 = self.graph.create_text(30,90, text=txt3, fill='white', anchor=SW, font=self.fnt) def isPolygonFrontFace(self, v, n):#Clockwise? '''v is a vertex of the polygon, n is the normal-vector of the polygon.''' #The camera should be at (0.0, 0.0, 0.0) but that doesn't work well. #It seems that the camera is at (0.0, 0.0, 1.0) c = matrix.Vector3D(0.0, 0.0, 1.0) vv = v-c r = vv.dot(n) return r < 0.0 def calcNormalVec(self, p): '''p is an array of vertices of the polygon/face''' v1 = p[0]-p[1] v2 = p[0]-p[3] v = v1.cross(v2) v.normalize() return v def getForwardVec1(self): #Where the camera is facing. Inverting a matrix is slow m = self.Rot.invert() f1 = m[(0,2)] f2 = m[(1,2)] f3 = m[(2,2)] v = matrix.Vector3D(f1, f2, f3) v.normalize() #Forward vector is really backward one, so need to be negated return -v def getForwardVec2(self, yrot): #Where the camera is facing. f1 = -math.sin(math.radians(yrot)) f2 = 0.0 f3 = -math.cos(math.radians(yrot)) v = matrix.Vector3D(f1, f2, f3) return v def isCollisionPlanes(self, campos, camdir): num = len(self.walls) for i in range(num): iscol, dist = self.isCollisionPlane(self.walls[i], self.wallsnormals[i], campos, camdir) if iscol and dist < 0.1: #print( i+1, dist) return True def ini_recv_queue(self): self.recv_queue=[] def is_not_empty_recv_queue(self): if len(self.recv_queue)>0 : print('recv_queue is empty') return True print('recv_queue is not empty') return False def isCollisionPlane(self, p, n, campos, camdir): '''instead of pointonplane (p) and normalofplane (n) we could pass a polygon and calculate its normal vector here. campos is the position of the camera and camdir is the forward vector of the camera''' def are_in_the_recv_queue(self,messages): print('start are_in_the_recv_queue') for x in messages: print('is '+x+' in the recv_queue?') flag=False for m in self.recv_queue: print(m+' in the received queue') if x in m: print(x + ' is in the '+m) flag=True self.write_message("message "+x+" found!") break if flag==False: return False return True def enter_ith_command(self): command=self.command_field_for_ith_server.get() ith_s=self.ith_server_field.get() ith=int(ith_s) print("send "+command+" to "+str(ith)) self.write_message("send "+command+" to "+str(ith)) self.coms.send_command_to_i(ith,command) self.command_field_for_ith_server.delete(0,END) def enter_all_command(self): command=self.command_field_for_all_server.get() print("send "+command+" to all") self.write_message("send "+command+" to all") self.coms.send_command_to_all(command) self.command_field_for_all_server.delete(0,END) def ex(self,command): print("parse "+command) command=self.r_b(command) if self.parse_command(command): self.write_message("OK") print("OK") else: self.write_message("ERROR") print("ERROR") def write_message(self, message): self.message_area.insert(tk.END,message) self.message_area.insert(tk.END,'\n') self.message_area.yview_moveto(1) dist = 0.0 #Dot Product Between Plane Normal And Ray Direction dotprod = camdir.dot(n) #Determine If Ray Parallel To Plane if ((dotprod < Space.ZERO) and (dotprod > -Space.ZERO)): return False, dist #Find Distance To Collision Point dist = (n.dot(p-campos))/dotprod #Test If Collision Behind Start if (dist < -Space.ZERO): return False, dist return True, dist def isCollisionCube(self, campos,polygons_3D): '''Checks if the camera is inside the cube (this is the bounding-box-technique (bbt) but we have a cube so we dont't need to calculate a bb) ''' for i in range(len(polygons_3D)): cube=(polygons_3D[i]).cube cubefaces=(polygons_3D[i]).cubefaces if (campos.z >= cube[cubefaces[0][0]].z and campos.z <= cube[cubefaces[2][0]].z) and (campos.y >= cube[cubefaces[5][0]].y and campos.y <= cube[cubefaces[4][0]].y) and (campos.x >= cube[cubefaces[3][0]].x and campos.x <= cube[cubefaces[1][0]].x): return True return False def keycallback(self, event): # print event.char # print event.keycode # print event.keysym #Foward if event.keysym == 'Up': vFwd = self.getForwardVec2(self.ang[1]) #Is there a collison? pos = matrix.Vector3D(self.trans[0]+vFwd.x*Space.SPEED, self.trans[1], self.trans[2]+vFwd.z*Space.SPEED) if self.isCollisionPlanes(pos, vFwd): def clear_message(self): self.message_area.delete("1.0",END) def click_connect_button(self): self.coms.connect_all() def click_set_id_button(self): self.ex("send \"set id=\\\"x\\\"\" to 2") self.ex("send \"set id=\\\"y-l\\\"\" to 0") self.ex("send \"set id=\\\"y-r\\\"\" to 1") self.ex("send \"set id=\\\"b-0\\\"\" to 3") self.ex("send \"set id=\\\"m-0\\\"\" to 4") self.ex("send \"set id=\\\"m-1\\\"\" to 5") self.ex("send \"set id=\\\"m-2\\\"\" to 6") self.ex("send \"set id=\\\"m-3\\\"\" to 7") #self.ex("send \"set id=\\\"x\\\"\" to 2") self.ex("send \"set id=\\\"a-0\\\"\" to 8") # #self.ex("send \"set id 90 \" to 3") #self.ex("send \"set id 120 \" to 4") #self.ex("send \"set id 150 \" to 5") #self.ex("send \"set id 180 \" to 6") #self.ex("send \"set id 210 \" to 7") def send_move_command(self,x,y): self.ex("send \"id=\\\"x\\\" arduino moveTo "+str(x)+" \\\"mv-x\\\" \" to 2") self.ex("send \"id=\\\"y-l\\\" arduino moveTo "+str(y)+" \\\"mv-yl\\\" \" to 0") self.ex("send \"id=\\\"y-r\\\" arduino moveTo "+str(y)+" \\\"mv-yr\\\" \" to 1") def send_init_belt_command(self): self.ex("send \"id=\\\"b-0\\\" arduino set thresh 100\" to 3") def send_stop_belt_command(self): self.ex("send \"id=\\\"b-0\\\" arduino set thresh 0\" to 3") def send_arm_motion_command(self,n): self.ex("send \"id=\\\"a-0\\\" rcb4 motion "+n+" \" to 8") def send_magnet_command(self,i,subcommand): self.ex("send \"id=\\\"m-"+str(i)+"\\\" arduino "+subcommand+" \" to "+str(4+i)) def wait_until_receive(self,messages,timeout): last_time=time.time() for m in messages: self.write_message("start receive "+m) self.ini_recv_queue() while True: if self.is_not_empty_recv_queue(): if self.are_in_the_recv_queue(messages): self.write_message("all messages received.") self.ini_recv_queue() return time_now=time.time() if time_now - last_time >timeout: self.write_message("time over to receive "+messages[0]+' and others') print('time over') self.ini_recv_queue() return if self.isCollisionCube(pos,self.polygons_3D): return self.trans[0] += vFwd.x*Space.SPEED self.trans[2] += vFwd.z*Space.SPEED #Backward elif event.keysym == 'Down': vFwd = self.getForwardVec2(self.ang[1]) vBck = -vFwd #Is there a collison? pos = matrix.Vector3D(self.trans[0]-vFwd.x*Space.SPEED, self.trans[1], self.trans[2]-vFwd.z*Space.SPEED) if self.isCollisionPlanes(pos, vBck): return if self.isCollisionCube(pos,self.polygons_3D): return self.trans[0] -= vFwd.x*Space.SPEED self.trans[2] -= vFwd.z*Space.SPEED #Turn right elif event.keysym == 'Right': self.ang[1] -= 1.5 #Turn left elif event.keysym == 'Left': self.ang[1] += 1.5 #Upwards elif event.keysym == 'u': #Is there a collison? pos = matrix.Vector3D(self.trans[0], self.trans[1]+Space.SPEED, self.trans[2]) vUp = matrix.Vector3D(0.0, 1.0, 0.0) if self.isCollisionPlanes(pos, vUp): return if self.isCollisionCube(pos,self.polygons_3D): return self.trans[1] += Space.SPEED #Downwards elif event.keysym == 'd': #Is there a collison? pos = matrix.Vector3D(self.trans[0], self.trans[1]-Space.SPEED, self.trans[2]) vDwn = matrix.Vector3D(0.0, -1.0, 0.0) if self.isCollisionPlanes(pos, vDwn): return if self.isCollisionCube(pos,self.polygons_3D): return self.trans[1] -= Space.SPEED #Quit elif event.keysym == 'Escape': self.quit() if self.ang[1] >= 360.0: self.ang[1] -= 360.0 if self.ang[1] < 0.0: self.ang[1] += 360.0 self.update(self.polygons_3D) time.sleep(1.0) def quit(self): self.root.quit() def display_help(self): self.write_message("send \"<command>\" to <i>\"") self.write_message("send \"<command>\" to all\"") self.write_message("connect <i> to \"<host ip>\"") self.write_message("moveTo (<f> , <f>)") self.write_message("arm motion <i>") self.write_message("init belt") self.write_message("stop belt") self.write_message("magnet <i> go") self.write_message("magnet <i> back") self.write_message("magnet <i> off") self.write_message("magnet <i> stop") def client_start(self): """ start client """ print("start client_start") handle_thread = threading.Thread(target=self.handler, daemon=True) self.parse("send up \"str (this s* f2d1 next s*4 f4d1)\".") self.parse("send up \"str (this s*4 f2d1 next s*44 f4d1)\".") self.update(self.polygons_3D) handle_thread = threading.Thread(target=self.handler, daemon=True) handle_thread.start() def handler(self): """ receive commands from terminal """ while True: print("input...") line=input() self.parse(line) self.update(self.polygons_3D) def clear(self): self.polygons_3D=[self.cube1] self.clear_cxyz() cc1=self.cube1.get_cube_center() self.cxyz[round(cc1.x)+self.pcx][round(cc1.y)+self.pcy][round(cc1.z)+self.pcz]=1 self.update(self.polygons_3D) def parse(self,line): #self.python2fwb(">"+line) print("Space:parse "+line) p=line.find("send up ") if(p<0): return xl=line[(p+len("send up ")):] #print("xl="+xl) px=parser(xl) px.rb() if not (px.key("\"")): return if not px.key("str "): return px.rb() if not px.key("("): return px.rb() if not px.key("this "): return px.rb() xn=[""] if not px.d_name(xn): return px.rb() dn1=xn[0] cn=[(0,0)] if not px.connect(cn): return cn1=cn[0] px.rb() if not px.key("next "): return px.rb() xn=[""] if not px.d_name(xn): return dn2=xn[0] px.rb() if not px.connect(cn): return cn2=cn[0] px.rb() if not px.key(")"): return px.rb() if not px.key("\""): return px.rb() if not px.key("."): return print("dn1="+dn1) cc2=None cube1=self.lookUpCube(dn1) if cube1 == None: print("new "+dn1) cube1=Cube(self) cube1.set_name(dn1) self.polygons_3D.append(cube1) cc1=cube1.get_cube_center() self.cxyz[round(cc1.x)+self.pcx][round(cc1.y)+self.pcy][round(cc1.z)+self.pcz]=1 print(self.cxyz) print("dn2="+dn2) cube2=self.lookUpCube(dn2) if cube2 == None: print("new "+dn2) cube2=Cube(self) cube2.set_name(dn2) self.polygons_3D.append(cube2) print(cn1) print(cn2) if1=cn1[0] if2=cn2[0] id1=cn1[1] id2=cn2[1] v2=cube1.get_next_place_position(if1) cube2.moveTo(v2.x,v2.y,v2.z) cube1.rotate_nextdoor_cube_until_match_the_face(if1, cube2, if2) cube1.rotate_nextdoor_cube_until_match_the_direction(if1,cube2,if2,id2) self.update(self.polygons_3D) cc2=cube2.get_cube_center() self.cxyz[round(cc2.x)+self.pcx][round(cc2.y)+self.pcy][round(cc2.z)+self.pcz]+=1 #print(self.cxyz) #self.make_pjxy() #self.make_pjyz() #self.make_pjzx() #print("rotate_z_cw_90") #self.rotate_z_cw_90() #self.make_pjxy() #self.make_pjyz() #self.make_pjzx() #print("rotate_x_cw_90") #self.rotate_x_cw_90() #self.make_pjxy() #self.make_pjyz() #self.make_pjzx() class parser: # # srs. # ok. # fid=2. received=ack1 face 4 dir 0 uDir 0. # this face(2):dir(0)<-> next face(4):dir(0) # send up "str (this s* f2d0 next s*4 f4d0)". # fid=2, received=send up "str (this s*4 f2d0 next s*44 f4d0)". # str (this s*4 f2d0 next s*44 f4d0) # ok # fid=2, received=send up "str (this s*44 f2d0 next s*444 f4d0)". # str (this s*44 f2d0 next s*444 f4d0)". # ok # def __init__(self,line): self.line=line print("parser init "+line) def alpha(self,p2a): #print("alpha line="+self.line) c=self.line[0] p2a[0]=c if(c.isalpha()): self.line=self.line[1:] return True return False def digit(self,p2i): #print("digit line="+self.line) c=self.line[0] if(c.isdigit()): p2i[0]=int(c) self.line=self.line[1:] return True return False def d_name(self,xn): #print("d_name line="+self.line) cl=[''] if not self.alpha(cl): # # command # send "<command>" to <i> # send "<command>" to all # connect <i> to "<host ip>" # moveTo (<f> , <f>) # arm motion <i> # init belt # stop belt # def parse_command(self,command): rest=[""] strc=[""] ix=[0] command=self.r_b(command) print("parse_command("+command+")") if self.parse_key("send ",command,rest): print("parse_key(send "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_String_Constant(command,strc,rest): print("strc="+strc[0]+",rest="+rest[0]) command=self.r_b(rest[0]) if self.parse_key("to ",command,rest): command=self.r_b(rest[0]) if self.parse_key("all",command,rest): self.write_message("send \""+strc[0]+"\" to all") self.coms.send_command_to_all(strc[0]) #self.command_field_for_all_server.delete(0,END) return True elif self.parse_p_int(command,ix,rest): self.write_message("send \""+strc[0]+"\" to "+str(ix[0])) self.coms.send_command_to_i(ix[0],strc[0]) return True return False return False return False xn[0]=xn[0]+cl[0] if not self.key("*"): elif self.parse_key("connect ",command,rest): print("parse_key(connect "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_p_int(command,ix,rest): print("parse_p_int("+command+","+str(ix[0])+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_key("to ",command,rest): print("parse_key(to "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_String_Constant(command,strc,rest): self.write_message("connect "+str(ix[0])+" to "+strc[0]) print("connect "+str(ix[0])+" to "+strc[0]) self.coms.connect_com_to_address(ix[0],strc[0]) return True return False return False return False xn[0]=xn[0]+"*" dx=[0] while self.digit(dx): xn[0]=xn[0]+str(dx[0]) return True def connect(self,tx): #print("connect line="+self.line) if not self.key("f"): elif self.parse_key("moveTo ",command,rest): print("parse_key(moveTo "+command+","+rest[0]+")") command=self.r_b(rest[0]) fx=0.0 fy=0.0 if self.parse_key("(",command,rest): print("parse_key(( "+command+","+rest[0]+")") fxx=[0.0] fyy=[0.0] command=self.r_b(rest[0]) if self.parse_p_float(command,fxx,rest): command=self.r_b(rest[0]) fx=fxx[0] if self.parse_key(",",command,rest): command=self.r_b(rest[0]) if self.parse_p_float(command,fyy,rest): command=self.r_b(rest[0]) fy=fyy[0] if self.parse_key(")",command,rest): self.send_move_command(fx,fy) return True return False fx=[0] if not self.digit(fx): elif self.parse_key("init ",command,rest): print("parse_key(init "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_key("belt", command,rest): print("parse_key(belt"+command+","+rest[0]+")") self.send_init_belt_command() return True return False if not self.key("d"): elif self.parse_key("stop ",command,rest): print("parse_key(stop "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_key("belt", command,rest): print("parse_key(belt"+command+","+rest[0]+")") self.send_stop_belt_command() return True return False dx=[0] if not self.digit(dx): elif self.parse_key("arm ",command,rest): print("parse_key(arm "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_key("motion ", command,rest): print("parse_key(motion "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_p_int(command,ix,rest): print("parse_p_int("+command+","+str(ix[0])+","+rest[0]+")") self.send_arm_motion_command(str(ix[0])) return True return False return False tx[0]=(fx[0],dx[0]) return True def key(self,key): #print("key line="+self.line) #print("key="+key) tf=self.line.startswith(key) print(tf) if(self.line.startswith(key)): lk=len(key) self.line=self.line[lk:] elif self.parse_key("magnet ",command,rest): print("parse_key(magnet "+command+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_p_int(command,ix,rest): print("parse_p_int("+command+","+str(ix[0])+","+rest[0]+")") command=self.r_b(rest[0]) if self.parse_key("go",command,rest): self.send_magnet_command(ix[0],"go") return True if self.parse_key("back",command,rest): self.send_magnet_command(ix[0],"back") return True if self.parse_key("off",command,rest): self.send_magnet_command(ix[0],"stop") return True if self.parse_key("stop",command,rest): self.send_magnet_command(ix[0],"stop") return True return False return False return False def parse_key(self,key,inx,rest): keylen=len(key) inlen=len(inx) if inx.startswith(key): rest[0]=inx[keylen:inlen] return True rest[0]=inx return False def rb(self): while self.key(' '): continue # # +-----------------------------------------------------+ # | TESLA | # | +-----------+ +--------------+ DICE | # | | mbed | |m5atom | | # | | |<--UART-->|tcp_server_ex1| | # | +-----------+ +--------------+ | # | ^ | # +--------------------------- | -----------------------+ # | # wifi.... SSID YAMA-M5ATOM-EX01 # | PASS 12345678 # | # +--------------------------- | -----------------------+ # | v PC | # | +------------------+ | # | |Chat_Client | # | | | # | v | # | handler | # | | | # | v | # | parser | # | | | # +----|-------------+ # | # | put # v # +------------------+ # |ex04.py | # | | | # | v | # | handler | # +------------------+ # # coding: utf-8 #from PIL import Image, ImageFont, ImageDraw #import time #import sys #import requests #import os #import socket #import threading #from collections import deque #import subprocess #import copy class Chat_Client: sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) HOST = '192.168.4.1' PORT = 23 def __init__(self): print("start Chat_Client.__init__") def parse_p_int(self,command,ix,rest): print("parse_p_int("+command+",ix,rest)") i=0 c=command[0] if c in ['0','1','2','3','4','5','6','7','8','9']: clen=len(command) while True: i=i*10+int(c) command=command[1:clen] clen=len(command) if clen==0: ix[0]=i rest[0]="" return True print("parse_p_int command="+command) c=command[0] if not (c in ['0','1','2','3','4','5','6','7','8','9']): ix[0]=i rest[0]=command return True rest[0]=command ix[0]=0 return False def set_space(self,space): self.space=space def start_input_thread(self): handle_thread2 = threading.Thread(target=self.start_input, args=(self.sock,), daemon=True) handle_thread2.start() def start_input(self,sock): print("sock=") print(sock) print("input...") line="" cx=bytearray(8) while True: try: line=input() if line=="clear.": self.send_message("srr.") self.space.clear() def parse_p_float(self,command,fx,rest): ixx=[0] sxx=[0] if self.parse_p_int(command,ixx,rest): ix=ixx[0] command=rest[0] if self.parse_key(".",command,rest): command=rest[0] if self.parse_p_int(command,sxx,rest): sx=sxx[0] subscale=len(str(sx)) fxw=float(ix)+float(sx)/pow(10,subscale) print("fx="+str(fxw)) fx[0]=fxw else: fxw=float(ix) print("fx="+str(fxw)) fx[0]=fxw return True else: for i in range(len(line)): c=line[i] #c=readchar.readkey() #print(c) #cx[0]=c.encode('utf-8') #cx[1]=0x00 sock.sendall(c.encode('ascii',errors='replace')) time.sleep(0.01) except: print("error") fxw=float(ix) print("fx="+str(fxw)) fx[0]=fxw return True else: return False def send_message_nl(self,py2x_message): msg=py2x_message+'\n' self.sock.sendall(msg.encode('ascii',errors='replace')) def send_message(self,py2x_message): msg=py2x_message self.sock.sendall(msg.encode('ascii',errors='replace')) def r_b(self,inx): rx=[""] while self.parse_key(" ",inx,rx): inx=rx[0] return rx[0] def parse(self,line): vf.parse(line) def client_start(self): """start client""" self.sock.connect((self.HOST, self.PORT)) handle_thread = threading.Thread(target=self.handler, args=(self.sock,), daemon=True) handle_thread.start() def handler(self,sock): """receive the message from the server and print it.""" while True: lx="" while True: data = sock.recv(1024) dl=len(data) try: rd=data.decode('utf-8') except: rd='?'*dl for i in range(dl): c=rd[i] lx=lx+c if c=="." or c=="\n": print("[receive] "+lx) self.parseLines(lx) lx="" #print(c) if c=="." or c=="\n": break print("[receive]-"+lx) if self.space!=None: print("parse-"+lx) self.parseLines(lx) def parseLines(self,ls): lsx=ls.splitlines() for i in range(len(lsx)): self.space.parse(lsx[i]) class Offline_Client: def __init__(self): print("start Offline_Client.__init__") def set_space(self,space): self.space=space def start_input_thread(self): handle_thread2 = threading.Thread(target=self.start_input, args=(self.space,), daemon=True) handle_thread2.start() def start_input(self,space): print("input...") line="" while True: try: line=input() if line=="clear.": space.clear() if line.startswith("print "): line=line[len("print "):] while line.startswith(" "): line=line[1:] if line.startswith("xy."): space.make_pjxy() elif line.startswith("yz."): space.make_pjyz() elif line.startswith("zx."): space.make_pjzx() if line.startswith("rotate "): line=line[len("rotate "):] while line.startswith(" "): line=line[1:] if line.startswith("z."): space.rotate_z_cw_90() elif line.startswith("y."): space.rotate_y_cw_90() elif line.startswith("x."): space.rotate_x_cw_90() elif line.startswith("f."): space.move_and_rotate_for_factory() if line.startswith("generate."): proc=space.generate_assemble_procedure() print("generated proc=") print(proc) def parse_String_Constant(self,inx,strc,rest): rx=[""] xstr="" if self.parse_key('"',inx,rx): inx=rx[0] fx=inx[0] xlen=len(inx) while fx!='"': if xlen==0: return False if fx=='\\': inx=inx[1:xlen] fx=inx[0] xstr=xstr+fx else: space.parse(line) except Exception as e: print(e) print("error") xstr=xstr+fx xlen=len(inx) if xlen==0: return False inx=inx[1:xlen] #print("inx="+inx) #print("xstr="+xstr) fx=inx[0] if self.parse_key('"',inx,rx): strc[0]=xstr rest[0]=rx[0] return True return False if __name__ == "__main__": #client=Chat_Client() client=Offline_Client() space=Space() client.set_space(space) #client.client_start() client.start_input_thread() space.start_3d() if __name__=="__main__": root=Tk() coms=Coms() p=Command_Panel(root,coms) print('Command_Panel') coms.set_message_window(p) print('set_message_window') root.mainloop() }} ---- #counter
