factory_controller_01.py のバックアップ(No.2)

バックアップ一覧
差分を表示
現在との差分を表示
ソースを表示
factory_controller_01.py へ行く。
- 1 (2024-08-13 (火) 11:21:41)
- 2 (2024-08-13 (火) 15:35:52)
- 3 (2024-08-15 (木) 07:16:10)
- 4 (2024-08-17 (土) 16:47:30)
- 5 (2024-09-17 (火) 18:22:15)

TESLA202407

## 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 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 を作成し、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")
        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()
                if self.message_window:
                    self.message_window.write_message("from "+self.host+":"+str(PORT)+", "+line)
                    if line.startsWith('assemble '):
                        self.message_window.set_script_from_remote(line[len('assemble '):])
                    elif 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
 
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")
 
        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_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 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")
 
    def set_script_from_remote(self,script):
        self.script_area.delete(1.0,END)
        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 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()