diff --git a/cm700reader.py b/cm700reader.py index f06e030..925f197 100644 --- a/cm700reader.py +++ b/cm700reader.py @@ -2,6 +2,7 @@ import datetime import os import sys +import threading import time import winsound @@ -11,6 +12,7 @@ from scipy.interpolate import interp1d import config +from dobot import DobotMeasurer class CM700D: @@ -27,6 +29,7 @@ self.K = 100.0 / (self.d65[:, 0] @ self.cmf[:, 1]) self.XYZw = ((self.d65.T @ self.cmf) * self.K)[0] print("White point of D65 = ", self.XYZw) + self.dobot = None return True @@ -56,7 +59,12 @@ for port in ports: try: print(f"Checking {port.device} ... ", end="") - self.ser = serial.Serial(port.device, config.BAUD_RATE, timeout=config.TIMEOUT, writeTimeout=config.TIMEOUT) + self.ser = serial.Serial( + port.device, + config.BAUD_RATE, + timeout=config.TIMEOUT, + writeTimeout=config.TIMEOUT, + ) if self.ser.is_open: recv = cm700d.send_command("IDR") print(recv) @@ -132,9 +140,9 @@ @staticmethod def save_csv(data, header): # 保存ファイル名の生成 - datestr = datetime.datetime.now().strftime('%m%d') + datestr = datetime.datetime.now().strftime("%m%d") filename = "" - for i in range(2,6): + for i in range(2, 6): for ab in ["a", "b"]: fn = f"tcc6-{i}_{datestr}{ab}.csv" if (filename == "") and (not os.path.exists(fn)): @@ -146,7 +154,7 @@ filename = infn if len(filename) < 1: return - + # csv保存 np.savetxt( filename, @@ -162,11 +170,12 @@ def mesuring_condition(self): recv = self.send_command("CPR") if recv[0] == config.CODE_OK: - print(f"測定条件　測定径 {config.MEAS_DIAMETER[recv[1]]}" - + f"　測定モード {config.MEAS_MODE[recv[2]]}" - + f"　待ち時間 {float(recv[3])/10}秒" - + f"　自動平均回数 {recv[4]}回" - + f"　手動平均回数 {recv[5]}回" + print( + f"測定条件　測定径 {config.MEAS_DIAMETER[recv[1]]}" + + f"　測定モード {config.MEAS_MODE[recv[2]]}" + + f"　待ち時間 {float(recv[3]) / 10}秒" + + f"　自動平均回数 {recv[4]}回" + + f"　手動平均回数 {recv[5]}回" ) # 手動計測 @@ -204,20 +213,60 @@ header = "no,X,Y,Z,L*,a*,b*," + (",".join(map(str, self.wl))) self.save_csv(data, header) + # Dobotを使った計測 + def dobot_measurement(self): + self.mesuring_condition() + + self.dobot = DobotMeasurer() + self.move_next(0) # Move to the first position + + data = [] + num_samples = len(config.CC_XY) + # num_samples = 3 + for i in range(num_samples): + print("\r", "%3d/%3d" % (i + 1, num_samples), end="") + self.measurement(data, i + 1 if i < num_samples - 1 else -1) + winsound.Beep(800, 1000) + self.dobot.up(wait=True) + + self.dobot = None # Close the Dobot connection + + if len(data) > 0: + header = "no,X,Y,Z,L*,a*,b*," + (",".join(map(str, self.wl))) + self.save_csv(data, header) + + # Dobotを使って次の位置へ移動 + def move_next(self, idx): + self.dobot.up(wait=True) + self.dobot.move(config.CC_XY[idx][0], config.CC_XY[idx][1], wait=True) + self.dobot.down(wait=True) + # 測定 - def measurement(self, data): + def measurement(self, data, idx=-1): print("測定中...", end="") recv = self.send_command("MES,1") - repeat = 0 + start_time = time.time() + while time.time() - start_time < config.MEAS_TO_MOVE: + time.sleep(0.1) + if idx >= 0: + thread1 = threading.Thread(target=self.move_next, args=(idx,)) + thread1.start() + while time.time() - start_time < config.MEAS_TO_RECEIVE: + time.sleep(0.1) + while True: - time.sleep(0.5) + elapsed_time = time.time() - start_time recv = self.send_command("MDR,2", validate=False) + # elapsed_time2 = time.time() - start_time + # print(elapsed_time1, elapsed_time2, recv[0]) + if recv[0] == config.CODE_OK: break - if repeat > 10: + if elapsed_time > config.MEAS_TIMEOUT: print("timeout") return - repeat += 1 + if idx >= 0: + thread1.join() print("done") spectrum = np.asarray(recv[1:], dtype=np.float32) / 10000.0 @@ -269,6 +318,7 @@ print("2. 一部の本体データを取得") print("3. 手動計測") print("4. インターバル計測") + print("5. Dobot制御計測") print("0. 終了") n = input("選択>") @@ -280,6 +330,8 @@ cm700d.manual_measurement() elif n == "4": cm700d.interval_measurement() + elif n == "5": + cm700d.dobot_measurement() else: break diff --git a/config.py b/config.py index 84d4389..13d907d 100644 --- a/config.py +++ b/config.py @@ -4,5 +4,34 @@ MEAS_TYPE = "SCE" MEAS_TYPE_DICT = {"SCI": 1, "SCE": 2} SAVE_FILE = "spectrum.csv" -MEAS_DIAMETER = {"1":"SAV", "2":"MAV"} -MEAS_MODE = {"1":"SCI", "2":"SCE", "3":"SCI+SCE"} +MEAS_DIAMETER = {"1": "SAV", "2": "MAV"} +MEAS_MODE = {"1": "SCI", "2": "SCE", "3": "SCI+SCE"} +MEAS_TIMEOUT = 10.0 # seconds +MEAS_TO_MOVE = 2.0 # seconds +MEAS_TO_RECEIVE = 2.5 # seconds +CC_XY = [ + [149.6, -57], + [147.9, -32.5], + [147.1, -9.7], + [147.3, 12.5], + [148.9, 35], + [150.9, 58.2], + [178.9, -59.6], + [177.5, -34.9], + [177, -10.8], + [177.9, 12.7], + [178.7, 36.5], + [180.7, 60.4], + [210.1, -59.6], + [208.8, -35], + [208.2, -10.4], + [208.2, 13.7], + [208.6, 37.8], + [209.8, 62], + [239.5, -60.5], + [238.7, -35.7], + [237.7, -10.4], + [238.1, 15.1], + [239.1, 39.4], + [240, 64.4], +] diff --git a/dobot.py b/dobot.py new file mode 100644 index 0000000..c7aa732 --- /dev/null +++ b/dobot.py @@ -0,0 +1,82 @@ +import pydobot +from serial.tools import list_ports + + +class DobotMeasurer: + def __init__(self, verbose=False): + self.Z_UP = 0 + self.Z_DOWN = -25 + port = self.find_port() + self.device = pydobot.Dobot(port=port, verbose=verbose) + if self.device is None: + raise RuntimeError("Failed to initialize Dobot.") + print(f"Dobot found on {port}") + + def __del__(self): + self.close() + + def init(self): + if self.device is not None: + self.close() + self.device = pydobot.Dobot(port=self.port, verbose=self.verbose) + if self.device is None: + raise RuntimeError("Failed to initialize Dobot.") + # Set initial position to Z_UP + (x, y, z, r, j1, j2, j3, j4) = self.device.pose() + if z < 0: + self.device.move_to(x + 3, y, self.Z_UP, 0, wait=True) + + def find_port(self): + available_ports = list_ports.comports() + dobot_port = [x for x in available_ports if "CH340" in x.description] + if not dobot_port: + raise RuntimeError( + "No Dobot port found. Please connect the Dobot and try again." + ) + return dobot_port[0].device + + def set_speed(self, velocity=100.0, acceleration=100.0): + if self.device is None: + raise RuntimeError("Dobot not initialized.") + self.device.speed(velocity, acceleration) + + def up(self, wait=True): + if self.device is None: + raise RuntimeError("Dobot not initialized.") + (x, y, z, r, j1, j2, j3, j4) = self.device.pose() + self.device.move_to(x, y, self.Z_UP, 0, wait) + + def down(self, wait=True): + if self.device is None: + raise RuntimeError("Dobot not initialized.") + (x, y, z, r, j1, j2, j3, j4) = self.device.pose() + self.device.move_to(x, y, self.Z_DOWN, 0, wait) + + def move(self, x, y, wait=True): + if self.device is None: + raise RuntimeError("Dobot not initialized.") + self.device.move_to(x, y, self.Z_UP, 0, wait=wait) + + def wait(self, ms): + if self.device is None: + raise RuntimeError("Dobot not initialized.") + self.device.wait(ms) + + def close(self): + if self.device is not None: + self.device.close() + self.device = None + + +if __name__ == "__main__": + from config import CC_XY as xy + + dobot = DobotMeasurer() + dobot.up() + num_cc = len(xy) + for i in range(num_cc): + print(f"Moving to point {i + 1}: {xy[i]}") + dobot.move(xy[i][0], xy[i][1]) + dobot.down() + dobot.wait(1000) # Wait for 1 second + dobot.up() diff --git a/requirements.txt b/requirements.txt index 09502e9..53f3ce4 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,4 @@ pyserial numpy scipy +pydobot