From 912a3ff63cc01c07271a4bc2b4dc7a28640c1d9d Mon Sep 17 00:00:00 2001 From: gac-S_Changer Date: Tue, 27 Jun 2017 15:34:13 +0200 Subject: [PATCH] Startup --- script/devices/OneWire.py | 134 +++++++++ script/devices/RobotModbus.py | 46 +++ script/devices/RobotSC.py | 87 ++++++ script/devices/RobotTCP.py | 434 +++++++++++++++++++++++++++ script/imgproc/CoverDetection.py | 113 +++++++ script/imgproc/LedDetectionFilter.py | 102 +++++++ script/imgproc/LedDetectionProc.py | 68 +++++ script/imgproc/PuckDetection.py | 24 ++ script/imgproc/SampleDetection.py | 23 ++ script/setup/ExposureScan.py | 29 ++ script/test/imgtest.py | 47 +++ script/test/ip | 15 + 12 files changed, 1122 insertions(+) create mode 100644 script/devices/OneWire.py create mode 100644 script/devices/RobotModbus.py create mode 100644 script/devices/RobotSC.py create mode 100644 script/devices/RobotTCP.py create mode 100644 script/imgproc/CoverDetection.py create mode 100644 script/imgproc/LedDetectionFilter.py create mode 100644 script/imgproc/LedDetectionProc.py create mode 100644 script/imgproc/PuckDetection.py create mode 100644 script/imgproc/SampleDetection.py create mode 100644 script/setup/ExposureScan.py create mode 100644 script/test/imgtest.py create mode 100644 script/test/ip diff --git a/script/devices/OneWire.py b/script/devices/OneWire.py new file mode 100644 index 0000000..20e590f --- /dev/null +++ b/script/devices/OneWire.py @@ -0,0 +1,134 @@ +import traceback +from datetime import datetime + +class Detector(ReadonlyRegisterBase): + def __init__(self, name): + ReadonlyRegisterBase.__init__(self, name) + self.reset() + + def set_inputs(self, inputs): + self.inputs = inputs + if self.take() != inputs.values(): + self.setCache(inputs.values(), None) + if self.getParent()!=None: + self.getParent().updateCache() + if (len(self.take()) == 0): + self.setState(State.Offline) + else: + self.setState(State.Ready) + + def set_input(self, index, val): + self.inputs[index] = val + self.set_inputs(self.inputs) + + def reset(self): + self.id = None + self.sn = None + self.status = None + self.type = None + self.set_inputs({}) + +class Esera(TcpDevice): + def __init__(self, name, server, timeout = 1000, retries = 1): + TcpDevice.__init__(self, name, server) + self.setMode(self.Mode.FullDuplex) + self.detectors = [] + for i in range(30): + self.detectors.append(Detector("Detector " + str(i+1))) + self.setChildren(self.detectors) + self.completed_initializatiod = False + self.debug = False + + def start(self): + self.getLogger().info("Starting controller") + self.write("set,sys,run,1\n") + + def stop(self): + self.getLogger().info("Stopping controller") + self.write("set,sys,run,0\n") + + def list(self): + self.write("get,owb,listall1\n") + + def req_data(self): + self.write("get,sys,data\n") + + def doInitialize(self): + super(Esera, self).doInitialize() + self.init_timestamp = time.time() + try: + self.setState(State.Ready) #So can communicate + for det in self.detectors: + det.reset() + self.list() + time.sleep(1.0) + self.check_started() + self.req_data() + except: + print >> sys.stderr, traceback.format_exc() + self.getLogger().warning(traceback.format_exc()) + raise + + def doUpdate(self): + self.check_started() + self.req_data() + + def updateCache(self): + #print "Update" + cache = [] + for det in self.detectors: + cache.append(det.take()) + self.setCache(cache, None) + + + def check_started(self): + if not self.completed_initializatiod: + init = True + for det in self.detectors: + if det.id == None: + init = False + break + if init: + self.completed_initializatiod = True + print("Completed initialization") + self.getLogger().info("Completed initialization") + self.start() + + def onString(self, msg): + if self.debug: + print datetime.now() , " - " , msg + tokens = msg.split("|") + if len(tokens)>1: + try: + if msg[:3] == "LST": + #LST|1_OWD1|3AF361270000009E|S_0|DS2413| + if tokens[1] > 1: + index = int(tokens[1].split("_")[1][3:]) - 1 + if index < len(self.detectors): + det = self.detectors[index] + det.id = tokens[1] + det.sn= tokens[2] if len(tokens)>2 else None + det.status = int(tokens[3][2:]) if len(tokens)>3 else None + det.type = tokens[4] if len(tokens)>4 else None + if det.status!= 0: + det.set_inputs({}) + else: + for det in self.detectors: + if det.id is not None and msg.startswith(det.id): + det_id = int(tokens[0][len(det.id)+1:]) + det.set_input(det_id, int(tokens[1])) + except: + print >> sys.stderr, traceback.format_exc() + self.getLogger().log(traceback.format_exc()) + + + + + + + +add_device(Esera("onewire", "129.129.126.83:5000"), force = True) +onewire.setPolling(500) +add_device(onewire.detectors[0], force = True) +add_device(onewire.detectors[1], force = True) +add_device(onewire.detectors[2], force = True) diff --git a/script/devices/RobotModbus.py b/script/devices/RobotModbus.py new file mode 100644 index 0000000..cd8abaf --- /dev/null +++ b/script/devices/RobotModbus.py @@ -0,0 +1,46 @@ +class RobotModbus(DeviceBase): + def __init__(self, name): + DeviceBase.__init__(self, name) + robot_req.write(0) + + def execute(self, command, *argv): + if robot_req.read() != 0: + raise Exception("Ongoing command") + if robot_ack.read() != 0: + raise Exception("Robot is not ready") + robot_cmd.write(command) + args = [0] * robot_args.size + index = 0 + for arg in argv: + args[index] = arg + index = index + 1 + if index == robot_args.size: + raise Exception("Invalid number of arguments") + robot_args.write(to_array(args, 'i')) + try: + self.request() + err = robot_ack.take() + if err == 1: + ret = robot_ret.read() + return ret + if err == 2: + raise Exception("Invalid command: " + str(command)) + raise Exception("Unknown error: " + str(err)) + finally: + self.cancel_request() + + def request(self): + robot_req.write(1) + while robot_ack.read() == 0: + time.sleep(0.001) + + def cancel_request(self): + robot_req.write(0) + while robot_ack.read() != 0: + time.sleep(0.001) + + def mount(self, puck, sample): + return self.execute('1', '1', puck, sample) + +add_device(RobotModbus("robot_mb"), force = True) + diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py new file mode 100644 index 0000000..c15676c --- /dev/null +++ b/script/devices/RobotSC.py @@ -0,0 +1,87 @@ +run("devices/RobotTCP") + +simulation = True + + +class RobotSC(RobotTCP): + def mount(self, puck, sample): + return self.execute('mount',segment, puck, sample) + + def firstmount(self, puck, sample): + return self.execute('firstmount', segment, puck, sample) + + def unmount(self, puck, sample): + return self.execute('unmount',segment, puck, sample) + + def on_event(self,ev): + #print "EVT: " + ev + pass + + def doUpdate(self): + #start = time.time() + RobotTCP.doUpdate(self) + global simulation + if not simulation: + if self.state != State.Offline: + self.get_joint_forces() + for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: + dev.update() + #print time.time() -start + + def start_task(self, program, *args, **kwargs): + #TODO: Check safe position + RobotTCP.start_task(self, program, *args, **kwargs) + + def stop_task(self): + RobotTCP.stop_task(self) + #TODO: Restore safe position + + + + + + +if simulation: + add_device(RobotSC("robot","129.129.126.74:1000"),force = True) +else: + add_device(RobotSC("robot", "129.129.126.100:1000"), force = True) + +robot.high_level_tasks = ["mount", "firstmount"] +robot.setPolling(20) +#robot.set_monitor_speed(20) + + + +class jf1(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[0] +class jf2(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[1] +class jf3(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[2] +class jf4(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[3] +class jf5(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[4] +class jf6(ReadonlyRegisterBase): + def doRead(self): + return None if robot.joint_forces == None else robot.joint_forces[5] +class jfc(ReadonlyRegisterBase): + def doRead(self): + if robot.joint_forces == None: + return float('NaN') + if robot.powered == False: + return float('NaN') + return (robot.joint_forces[1]+74)/4 + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2 + +add_device(jf1(), force = True) +add_device(jf2(), force = True) +add_device(jf3(), force = True) +add_device(jf4(), force = True) +add_device(jf5(), force = True) +add_device(jf6(), force = True) +add_device(jfc(), force = True) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py new file mode 100644 index 0000000..2260cb2 --- /dev/null +++ b/script/devices/RobotTCP.py @@ -0,0 +1,434 @@ +import threading + +class RobotTCP(TcpDevice, Stoppable): + def __init__(self, name, server, timeout = 1000, retries = 1): + TcpDevice.__init__(self, name, server) + self.timeout = timeout + self.retries = retries + self.header = None + self.trailer = "\n" + self.array_separator = '|' + self.cmd_separator = ' ' + self.msg_id = 0 + self.working_mode = "invalid" + self.status = "invalid" + self.powered = None + self.settled = None + self.empty = None + self.working_mode = None + self.status = None + self.lock = threading.Lock() + self.joint_forces = None + self.current_task = None + self.high_level_tasks = [] + + def doInitialize(self): + super(RobotTCP, self).doInitialize() + self.reset = True + + def _sendReceive(self, msg_id, msg = "", timeout = None): + tx = self.header if (self.header != None) else "" + tx = tx + msg_id + " " + msg + if (len(tx)>127): + raise Exception("Exceeded maximum message size") + self.getLogger().finer("TX = '" + str(tx)+ "'") + if (self.trailer != None): tx = tx + self.trailer + rx = self.sendReceive(tx, msg_id, self.trailer , 0, self.timeout if timeout is None else timeout, self.retries) + rx=rx[:-1] #Remove 0A + self.getLogger().finer("RX = '" + str(rx) + "'") + if rx[:3] != msg_id: + if (time.time()-start) >= timeout: + raise Exception("Received invalid message id: " + str(rx[:3]) + " - expecting:" + msg_id ) + if len(rx)<4: + raise Exception("Invalid message size: " + str(len(rx)) ) + if rx[3] == "*": + raise Exception(rx[4:]) + return rx[4:] + + def call(self, msg, timeout = None): + self.lock.acquire() + try: + id = "%03d" % self.msg_id + self.msg_id = (self.msg_id+1)%1000 + return self._sendReceive(id, msg, timeout) + finally: + self.lock.release() + + def execute(self, command, *args, **kwargs): + timeout = None if (kwargs is None) or (not kwargs.has_key("timeout")) else kwargs["timeout"] + msg = str(command) + if len(args)>10: + raise Exception("Exceeded maximum number of parameters") + for i in range(len(args)): + msg += (self.cmd_separator if (i==0) else self.array_separator) + str(args[i]) + rx = self.call(msg, timeout) + if rx.count(self.array_separator)>0: + return rx.split(self.array_separator) + return rx + + def evaluate(self, cmd, timeout=None): + ret = self.execute('eval', cmd, timeout=timeout) + if type(ret) is str: + if ret.strip() != "": raise Exception(ret) + + def get_var(self, name): + return self.execute('get_var', name) + + #Makes app crash + #def get_str(self, name='s'): + # return self.execute('get_str', name) + + def get_arr(self, name, size): + return self.execute('get_arr', name, size) + + def get_bool(self, name = "tcp_b"): + return True if (self.execute('get_bool', name).strip() == '1') else False + + def get_int(self, name ="tcp_n"): + return int(self.get_var(name)) + + def get_float(self, name ="tcp_n"): + return float(self.get_var(name)) + + def get_int_arr(self, size, name="tcp_a"): + # not working. A Jython bug in PyUnicaode? + #return [int(x) for x in self.get_arr("tcp_a", size)] + ret = [] + a=self.get_arr(name, size) + for i in range(size): + ret.append(int(a[i])) + return ret + + def get_float_arr(self, size, name="tcp_a"): + #return [float(x) for x in self.get_arr("tcp_a", size)] + a=self.get_arr(name, size) + ret = []; + for i in range(size): ret.append(float(a[i])); + return ret + + def get_trf(self, name="tcp_t"): + a = self.execute('get_trf', name) + ret = [] + for i in range(6): ret.append(float(a[i])) + return ret + + def set_tr(self, l, name="tcp_t"): + self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}") + + def get_jnt(self, name="tcp_j"): + a = self.execute('get_jnt', name) + ret = [] + for i in range(6): ret.append(float(a[i])) + return ret + + def set_jnt(self, l, name="tcp_j"): + self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}") + + def get_pnt(self, name="tcp_p"): + a = self.execute('get_pnt', name) + ret = [] + for i in range(6): ret.append(float(a[i])) + return ret + + def eval_int(self, cmd): + self.evaluate("tcp_n=" + cmd) + return self.get_int() + + def eval_float(self, cmd): + self.evaluate("tcp_n=" + cmd) + return self.get_float() + + def eval_bool(self, cmd): + self.evaluate("tcp_b=" + cmd) + return self.get_bool() + + #def eval_str(self, cmd): + # self.evaluate("s=" + cmd) + # return self.get_str() + + def eval_jnt(self, cmd): + self.evaluate("tcp_j=" + cmd) + return self.get_jnt() + + def eval_trf(self, cmd): + self.evaluate("tcp_t=" + cmd) + return self.get_trf() + + def eval_pnt(self, cmd): + self.evaluate("tcp_p=" + cmd) + return self.get_pnt() + + + #Robot control + def is_powered(self): + self.powered = self.eval_bool("isPowered()") + return self.powered + + def enable(self): + self.evaluate("enablePower()") + if not self.is_powered(): + raise Exception("Cannot enable power") + + #waits for power to be actually cut off + def disable(self): + self.evaluate("disablePower()", timeout=5000) + + def get_monitor_speed(self): + self.speed = self.eval_int("getMonitorSpeed()") + + def set_monitor_speed(self, speed): + ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")") + if (ret==-1): raise Exception("The robot is not in remote working mode") + if (ret==-2): raise Exception("The monitor speed is under the control of the operator") + if (ret==-3): raise Exception("The specified speed is not supported") + + def is_calibrated(self): + return self.eval_bool("isCalibrated()") + + def _update_working_mode(self, mode, status): + if mode==1: + self.working_mode = "manual" + self.status = "hold" if status==6 else "move" + elif mode==2: + self.working_mode = "test" + self.status = "hold" if status==3 else "move" + elif mode==3: + self.working_mode = "local" + self.status = "hold" if status==2 else "move" + elif mode==4: + self.working_mode = "remote" + self.status = "hold" if status==2 else "move" + else: + self.working_mode = "invalid" + self.status = "invalid" + + def read_working_mode(self): + try: + mode = self.eval_int("workingMode(tcp_a)") + status = int(self.get_var("tcp_a[0]")) + self._update_working_mode(mode, status) + self._update_state() + except: + self.working_mode = "invalid" + self.status = "invalid" + return self.working_mode + + def get_emergency_stop_sts(self): + st = self.eval_int("esStatus()") + if (st== 1): return "active" + if (st== 2): return "activated" + return "off" + + def get_safety_fault_signal(self): + fault = self.eval_bool("safetyFault(s)") + #if (fault): + # return get_str() + return fault + + #Motion control + def stop(self): + self.evaluate("stopMove()") + + def resume(self): + self.evaluate("restartMove()") + + def resetMotion(self): + self.evaluate("resetMotion()") + + def is_empty(self): + self.empty = self.eval_bool("isEmpty()") + self._update_state() + return self.empty + + def is_settled(self): + self.settled = self.eval_bool("isSettled()") + self._update_state() + return self.powered + + def get_move_id(self): + return self.eval_int("getMoveId()") + + def set_move_id(self, id): + return self.evaluate("setMoveId(" + str(id) + " )") + + def get_joint_forces(self): + try: + self.evaluate("getJointForce(tcp_a)") + self.joint_forces = self.get_float_arr(6) + return self.joint_forces + except: + self.joint_forces = None + raise + + def movej(self, joint_or_point, tool, desc): + return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") + + def movel(self, point, tool, desc): + return self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") + + def movec(self, point_interm, point_target, tool, desc): + return self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") + + + #Tool + #This function can timeout since it synchronizes move. + #Better check state before otherwise it can freeze the communication + def open_tool(self, tool): + return self.evaluate("open(" + tool + " )", timeout=3000) + + #This function can timeout since it synchronizes move. Better check state before + #Better check state before otherwise it can freeze the communication + def close_tool(self, tool): + return self.evaluate("close(" + tool + " )", timeout=3000) + + #Arm position + def herej(self): + return self.eval_jnt("herej()") + + def distance_t(self, trsf1, trsf2): + return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")") + + def distance_p(self, pnt1, pnt2): + return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")") + + def compose(self, pnt, frame, trsf): + return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")") + + def here(self, tool, frame): + return self.eval_pnt("here(" + tool + ", " + frame + ")") + + def joint_to_point(self, tool, frame, joint): + return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")") + + def point_to_joint(self, tool, initial_joint, point): + if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"): + return self.get_jnt() + + def position(self, point, frame): + return self.eval_trf("position(" + point + ", " + frame + ")") + + #Task control + def task_create(self, program, *args, **kwargs): + program = str(program) + priority = 10 if (kwargs is None) or (not kwargs.has_key("priority")) else kwargs["priority"] + name = str(program if (kwargs is None) or (not kwargs.has_key("name")) else kwargs["name"]) + + if self.get_task_status(name)[0] != -1: + raise Exception("Task already exists: " + name) + + #taskCreate "t1", 10, read(sMessage) + cmd = 'taskCreate "' + name + '", ' + str(priority) + ', ' + program + '(' + for i in range(len(args)): + cmd += str(args[i]) + (',' if (i<(len(args)-1)) else '') + cmd+=')' + self.evaluate(cmd) + + def task_suspend(self, name): + self.evaluate('taskSuspend("' + str(name)+ '")') + + #waits until the task is ready for restart + def task_resume(self, name): + self.evaluate('taskResume("' + str(name)+ '",0)', timeout = 2000) + + #waits for the task to be actually killed + def task_kill(self, name): + self.evaluate('taskKill("' + str(name)+ '")', timeout = 2000) + + def get_task_status(self, name): + code = self.eval_int('taskStatus("' + str(name)+ '")') + #TODO: String assignments in $exec causes application to freeze + #status = self + if code== -1: status = "Stopped" + elif code== 0: status = "Paused" + elif code== 1: status = "Running" + #else: status = self.execute('get_help', code) + else: status = "Error" + return (code,status) + + def _update_state(self): + #self.setState(State.Busy if self.status=="move" else State.Ready) + if self.state==State.Offline: + print "Communication resumed" + if self.reset or (self.state==State.Offline): + self.get_task() + if self.current_task is not None: + print "Ongoing task: " + self.current_task + + if (not self.settled) or (self.current_task is not None): self.setState(State.Busy) + elif not self.empty: self.setState(State.Paused) + else: self.setState(State.Ready) + + def doUpdate(self): + try: + start = time.time() + #sts = self._sendReceive("EVT").strip().split(self.array_separator) + sts = self.execute("get_status", self.current_task) + self._update_working_mode(int(sts[0]), int(sts[1])) + self.powered = sts[2] == "1" + self.speed = int(sts[3]) + self.empty = sts[4] == "1" + self.settled = sts[5] == "1" + ev = sts[7] if len(sts)>7 else "" + if len(ev.strip()) >0: + self.getLogger().info(ev) + self.on_event(ev) + #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0): + if int(sts[6]) < 0: + self.current_task = None + self._update_state() + self.reset = False + self.setCache({"powered": self.powered, + "speed": self.speed, + "empty": self.empty, + "settled": self.settled, + "task": self.current_task }, None) + + #print time.time() - start + except: + if self.state != State.Offline: + print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) + self.setState(State.Offline) + + #High-level, exclusive motion task. + def start_task(self, program, *args, **kwargs): + tasks = [t for t in self.high_level_tasks] + if (self.current_task is not None) and (not self.current_task in tasks): + tasks.append(pro) + if not program in tasks: + tasks.append(pro) + for task in tasks: + if self.get_task_status(task)[0]>=0: + raise Exception("Ongoing high-level task: " + task) + self.task_create(program, *args, **kwargs) + start = time.time() + while self.get_task_status(program)[0] < 0: + if time.time() - start > 5000: + raise Exception("Cannot start task " + task) + time.sleep(0.1) + self.update() + self.current_task = program + self._update_state() + + def stop_task(self): + #tasks = [t for t in self.high_level_tasks] + tasks = [] + if (self.current_task is not None) and (not self.current_task in tasks): + tasks.append(self.current_task) + for task in tasks: + #if self.get_task_status(task)[0]>=0: + self.task_kill(task) + + def get_task(self): + if self.current_task is not None: + return self.current_task + for task in self.high_level_tasks: + if self.get_task_status(task)[0]>=0: + self.current_task = task + return task + return None + + + def on_event(self,ev): + pass + + \ No newline at end of file diff --git a/script/imgproc/CoverDetection.py b/script/imgproc/CoverDetection.py new file mode 100644 index 0000000..e742856 --- /dev/null +++ b/script/imgproc/CoverDetection.py @@ -0,0 +1,113 @@ +################################################################################################### +# Procedure to detect the cover orientation +################################################################################################### +import ch.psi.pshell.imaging.Utils.integrateVertically as integrateVertically +img.backgroundEnabled=False + +REF = (0,96,125) + +line = load_image("{images}/line.png", title="Line") + +line.getProcessor().setBackgroundValue(0.0) + + +#ip = get_image() +ip = integrate_frames(10) +ip = grayscale(ip, True) + + +smooth(ip) +#bandpass_filter(ip, 30, 1000) +edges(ip) + +#invert(ip) +#auto_threshold(ip, method = "Default") + + +#auto_threshold(ip, method = "Li") +auto_threshold(ip, method = "MaxEntropy") + +""" +for m in AutoThresholder.getMethods(): + print m + aux = ip.duplicate() + auto_threshold(aux, method = m) + binary_fill_holes(aux, dark_background=False) + renderer = show_panel(aux.bufferedImage) + time.sleep(1.0) +""" +binary_dilate(ip, dark_background=False) +#binary_fill_holes(ip, dark_background=False) +#binary_open(ip, dark_background=Tr) + +renderer = show_panel(ip.bufferedImage) + + + + + + + + +line = sub_image(line, 325, 325, 512, 512) +ip = sub_image(ip, 325, 325, 512, 512) +#op = op_fft(ip, line, "correlate") + + +renderer = show_panel(ip.bufferedImage) + +#renderer = show_panel(op.bufferedImage) +#line.show() +ydata = [] +xdata = range (0,180,1) +for i in xdata: + l = line.duplicate() + l.getProcessor().setBackgroundValue(0.0) + l.getProcessor().rotate(float(i)) + op = op_fft(ip, l, "correlate") + bi = op.getBufferedImage() + p = integrateVertically(bi) + ydata.append(sum(p)) + + #renderer = show_panel(op.bufferedImage) + #time.sleep(0.001) + +def moving_average(arr, n) : + ret = [] + for i in range(len(arr)): + ret.append(mean(arr[max(i-n,0):min(i+n,len(arr)-1)])) + return ret +av = moving_average(ydata, 1) + +p = plot(ydata, xdata=xdata)[0] + +p.addSeries(LinePlotSeries("Moving Average")) +p.getSeries(1).setData(xdata, av) + +peaks = estimate_peak_indexes(ydata, xdata, (min(ydata) + max(ydata))/2, 25.0) +left, right = min(peaks), max(peaks) +if xdata[left]<5 and xdata[right]>(xdata[-1]-5): + #del peaks[0 if ydata[right] > ydata[left] else -1] + peaks.remove(right if ydata[right] > ydata[left] else left) + +peaks = sorted(peaks[:3]) +peaks_x = map(lambda x:xdata[x], peaks) +peaks_y = map(lambda x:ydata[x], peaks) + +print "Peaks", peaks +print "Peak indexes: " + str(peaks_x) +print "Peak values: " + str(peaks_y) + + +for i in range(len(peaks)): + peak = xdata[peaks[i]] + p.addMarker(peak, None, "N="+str(round(peak,2)), Color(80,0,80)) + if ((peaks[i]>160) and (REF[i]<20)): + peaks[i] = peaks[i] - 180.0 + +print "Peaks x: " + str(peaks_x) + + +d = mean(arrabs(arrsub(REF, peaks_x))) + +print "Angle = ", d \ No newline at end of file diff --git a/script/imgproc/LedDetectionFilter.py b/script/imgproc/LedDetectionFilter.py new file mode 100644 index 0000000..a4818d8 --- /dev/null +++ b/script/imgproc/LedDetectionFilter.py @@ -0,0 +1,102 @@ +################################################################################################### +# Example of using ImageJ functionalities through ijutils. +################################################################################################### + +import datetime +from ijutils import * +import java.awt.Color as Color + +import ch.psi.pshell.imaging.Filter as Filter +from ch.psi.pshell.imaging.Overlays import * +import ch.psi.pshell.imaging.Pen as Pen + + +integration_count = 10 +integration_continuous = False +integration_partial = False +frames = [] +roi = get_roi() + + +color_roi = Color(0, 128, 0) + +renderer = show_panel(img) +renderer.clearOverlays() +ov_roi_shape = Ellipse(Pen(color_roi, 0,), java.awt.Point(roi[0], roi[1]), java.awt.Dimension(roi[2], roi[3])) +ov_roi_bound = Rect(Pen(color_roi, 0, Pen.LineStyle.dotted), java.awt.Point(roi[0], roi[1]), java.awt.Dimension(roi[2], roi[3])) +ov_roi_center = Crosshairs(Pen(color_roi, 0), java.awt.Point(roi_center[0],roi_center[1]), java.awt.Dimension(15,15)) + +renderer.addOverlays([ov_roi_shape, ov_roi_bound,ov_roi_center]) + + + + +last_ret = (None, None) +def detect_led(ip): + roi = get_roi() + global roi_center, roi_radius, integration_count, integration_continuous, integration_partial, frames + global count , last_ret + aux = sub_image(ip, roi[0], roi[1], roi[2], roi[3]) + grayscale(aux) + #gaussian_blur(aux) + if (integration_count>1): + frames.append(aux) + if len(frames) >integration_count: + del frames[0] + if not integration_continuous: + if (len(frames)< integration_count): + if last_ret[1] is not None: invert(last_ret[1]) + return last_ret + if (not integration_partial) and len(frames)