![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
#author("2024-10-21T15:18:28+09:00","default:TESLA202407","TESLA202407") #author("2024-10-21T15:19:22+09:00","default:TESLA202407","TESLA202407") [[TESLA202407]] #code(Python){{ # -*- coding: utf-8 -*- ## License: Apache 2.0. See LICENSE file in root directory. ## Copyright(c) 2017 Intel Corporation. All Rights Reserved. #20240710 #20241021 ##################################################### ## 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 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 threading PORT = 9998 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(self): 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 繧剃ｽ懈・縺励《oc縺ｫ莉｣蜈･ self.soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #IP繧｢繝峨Ξ繧ｹ縺敬ost縺ｮ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繧痴tart 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") sf=self.soc.makefile() 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) line=sf.readline() rx="[receive]- "+line print(rx) if self.message_window: self.message_window.write_message("from "+self.host+":"+str(PORT)+", "+rx) if line.startswith('asm '): self.message_window.set_script_from_remote(line[len('asm '):]) elif self.message_window.recv_queue!=None: self.message_window.recv_queue.append(rx) 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.exc_info()[2] print("message:{0}".format(e.with_traceback(tb))) break def close(self): self.soc.close() self.soc=None class Coms: def __init__(self): self.coms=[None,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[9]=Com("192.168.14.65",self.message_window) # 9:tesla_transformer # #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") 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 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: 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 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() 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) 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) 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) 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) 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) 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() #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) 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.run_script() self.set_script() self.display_help() 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_magnet_command(0,"back") self.send_arm_motion_command("9") #--- release --- time.sleep(6.0) 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 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 h_rotate_acw(self): print("horizontal rotate, anti clock wise.") #---- horizontal rotate , anti 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_magnet_command(0,"go") self.send_magnet_command(1,"go") self.send_arm_motion_command("25") #--- grasp for rotate, clock-wize---\n") time.sleep(10.0) self.send_magnet_command(1,"stop") self.send_arm_motion_command("24") #--- horizontal rotate, clock-wise time.sleep(20.0) self.send_magnet_command(0,"back") self.send_magnet_command(1,"back") 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 self.send_magnet_command(3,"back") 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") 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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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("23") #--- 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) self.send_move_command(20.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") def set_script_from_remote(self,script): if script.startswith('start.'): self.script_area.delete(1.0,END) if script.startswith('m '): script=script[len('m '):] self.script_area.insert(END,script) 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") 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) 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() def start_script(self): print('start_script') self.script=self.script_area.get(0.,END) print("run_script:"+self.script) exec(self.script) 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 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) 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=\\\"tesla\\\"\" to 9") # #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 time.sleep(1.0) 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") # # 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 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 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 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 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 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 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 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 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: fxw=float(ix) print("fx="+str(fxw)) fx[0]=fxw return True else: return False def r_b(self,inx): rx=[""] while self.parse_key(" ",inx,rx): inx=rx[0] return rx[0] 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: 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__": root=Tk() coms=Coms() p=Command_Panel(root,coms) print('Command_Panel') coms.set_message_window(p) print('set_message_window') root.mainloop() }}
