script/calibration/ScanRZ.py

@@ -1,26 +1,31 @@
+#Imports
 import plotutils
 from mathutils import fit_gaussian, Gaussian
 
-RANGE = [180.0,-180.0]
+#Parameters
+RANGE = [-120.0,120.0]
 STEP = 5.0
 LATENCY = 0.005
 RELATIVE = False
 
-
-robot.set_tool(TOOL_DEFAULT)
-
+#Enabling and checking
+#enable_power()
+#system_check()
 robot.enable()
+
+
+#Body
+robot.set_tool(TOOL_DEFAULT)
 move_to_laser()
-
-
-
-
-
-
-
-robot.set_joint_motors_enabled(True)
+#robot.set_joint_motors_enabled(True)
+robot.set_motors_enabled(True)
+robot_rz.move(0.0)
+robot.set_motors_enabled(True)
 ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2")
 
 
+#Cleanup
+
+
 
 

script/calibration/ScanY.py

@@ -9,7 +9,7 @@ if d<0:
 if d>20:
     raise Exception ("Should be near the laser position to perform the scan")
 
-RANGE = [-1.5, 1.5]
+RANGE = [-2.0, 2.0] #[-1.5, 1.5]
 STEP = 0.02
 Z_OFFSET = 0 #-1.0
 LATENCY = 0.025

script/calibration/ToolCalibration2.py

@@ -2,13 +2,17 @@ import plotutils
 from mathutils import fit_gaussian, Gaussian
 
 
-robot.assert_tool(TOOL_CALIBRATION)
+#robot.assert_tool(TOOL_CALIBRATION)
+#cal_tool = TOOL_DEFAULT 
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
 robot.enable()
-robot.set_motors_enabled(True)
-robot.set_joint_motors_enabled(True)
 move_to_laser()
 
 
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
 initial_pos = robot.get_cartesian_pos()
 
 robot.enable()
@@ -52,7 +56,7 @@ robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
 robot.set_trsf([off_x, 0,0,0,0,0])
 c=robot.compose("pTemp", "fTable", "tcp_t" )
 robot.set_pnt(c, "pTemp")
-robot.movel("pTemp", TOOL_CALIBRATION, DESC_SCAN, sync=True)
+robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
 
 
 pos2 = robot.get_cartesian_pos()
@@ -66,10 +70,17 @@ print "Position 2: ", pos2
 xoff = (pos2[0]-pos1[0])/2
 yoff = (pos2[1]-pos1[1])/2
 
-print "Offset: ", [xoff, yoff]
+#print "Offset: ", [xoff, yoff]
 
 t=robot.get_tool_trsf(TOOL_DEFAULT)
-t[0]=-xoff 
-t[1]=yoff 
+t[0]=xoff 
+t[1]=-yoff 
+print "Offset: ", [t[0], t[1]]
 robot.set_tool_trsf(t, TOOL_DEFAULT)
 
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+else:
+    print "Cannot move calibrated tool to laser: toog big offset"

script/devices/LedCtrl.py

@@ -22,4 +22,25 @@ class LedPositioner(DiscretePositionerBase):
 
 
 add_device(LedPositioner(), True)
-led_ctrl.polling = 1000
+led_ctrl.polling = 1000
+
+
+
+
+import ch.psi.pshell.device.ProcessVariableConfig as ProcessVariableConfig
+class LedLevel(ProcessVariableBase):
+    def __init__(self, name):
+        ProcessVariableBase.__init__(self, name, ProcessVariableConfig())
+        
+    def doRead(self):
+        return get_led_level()
+        
+    def doWrite(self, val):
+        return set_led_level(val)
+        
+led_level = LedLevel("led_level")
+led_level.config.minValue = 0.0
+led_level.config.maxValue = 100.0  
+led_level.config.unit = "%"
+add_device(led_level, True)
+led_level.polling = 1000

script/devices/Wago.py

@@ -36,20 +36,25 @@ led_ctrl_3=ch.psi.pshell.modbus.ProcessVariable|wago 2|||
 #  Leds
 ###################################################################################################
 
+def set_led_level(level):
+     level = max(min(float(level),100.0),0.0)
+     set_setting("led_level", level)
+     led_ctrl_1.write(led_ctrl_1.config.maxValue * level / 100.0)
+     led_ctrl_2.write(led_ctrl_2.config.maxValue * level / 100.0)
+     led_ctrl_3.write(led_ctrl_3.config.maxValue * level / 100.0)        
+
+def get_led_level():
+    level = get_setting("led_level")
+    return float(50 if level is None else level)
+    
 def set_led_state(value):
     """
     Turn leds on and off
     """
     if value:
-        led_ctrl_1.write(led_ctrl_1.config.maxValue)
-        led_ctrl_2.write(led_ctrl_2.config.maxValue)
-        led_ctrl_3.write(led_ctrl_3.config.maxValue)    
+        set_led_level(100.0)
     else:
-        led_ctrl_1.write(0.0)
-        led_ctrl_2.write(0.0)
-        led_ctrl_3.write(0.0)
-        
-    
+        set_led_level(0.0)
 
 def get_led_state():
     """
@@ -85,6 +90,8 @@ def is_led_room_temp():
 
 
 
+
+
 ###################################################################################################
 #  Safety release
 ###################################################################################################

script/imgproc/CameraCalibration2.py

@@ -0,0 +1,102 @@
+import ch.psi.pshell.device.Camera as Camera
+import ch.psi.pshell.imaging.RendererMode as RendererMode
+import ch.psi.pshell.imaging.Calibration as Calibration
+from ch.psi.pshell.imaging.Overlays import *
+SIMULATION = ch.psi.pshell.imaging.FileSource
+"""
+img.camera.setColorMode(Camera.ColorMode.Mono)
+img.camera.setDataType(Camera.DataType.UInt8)
+img.camera.setGrabMode(Camera.GrabMode.Continuous)
+img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+img.camera.setExposure(50.00)
+img.camera.setAcquirePeriod(200.00)
+img.camera.setGain(0.0)
+img.config.rotationCrop=True
+
+
+"""
+
+img.config.rotation=0
+img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =0,0,-1,-1
+img.config.setCalibration(None)
+
+p = show_panel(img)
+p.setMode(RendererMode.Fit)
+ov_text = Text(Pen(java.awt.Color.GREEN.darker()), "", java.awt.Font("Verdana", java.awt.Font.PLAIN, 24), java.awt.Point(20,20))
+ov_text.setFixed(True)
+
+
+CC4 = (-129.9, -150)
+CD5 = (129.9, -150)
+CA5 = (-129.9, 150)
+CF4 = (129.9, 150)
+
+DX = 259.8
+DY = 300.0
+
+ROI_X = 470.0
+ROI_Y = 470.0
+
+def rotate(x,y, degrees):
+    rotation = math.radians(degrees)
+    rw, rh = img.getImage().getWidth(), img.getImage().getHeight()
+    ox, oy = x - (rw / 2), y - (rh / 2)
+    x = ox * math.cos(rotation) - oy * math.sin(rotation) + rw / 2;
+    y = oy * math.cos(rotation) + ox * math.sin(rotation) + rh / 2;
+    return x,y
+
+p.addOverlay(ov_text)
+try:
+    ov_text.update("Click on the center of C4 (19) position...")
+    p.refresh()
+    pc4 = p.waitClick(60000)
+    print pc4
+    ov_text.update("Click on the center of D5 (13) position...")
+    p.refresh()
+    pd5 = p.waitClick(60000)
+    print pd5
+    ov_text.update("Click on the center of A5 (28) position...")
+    p.refresh()
+    pa5 = p.waitClick(60000)
+    print pa5
+    ov_text.update("Click on the center of F4 (04) position...")
+    p.refresh()
+    pf4 = p.waitClick(60000)
+    print pf4      
+   
+    
+    vc1x, vc1y, vc2x, vc2y = (pc4.x + pd5.x )/2.0, (pc4.y + pd5.y )/2.0, (pa5.x + pf4.x )/2.0, (pa5.y + pf4.y )/2.0
+    hc1x, hc1y, hc2x, hc2y = (pc4.x + pa5.x )/2.0, (pc4.y + pa5.y )/2.0, (pd5.x + pf4.x )/2.0, (pd5.y + pf4.y )/2.0
+    cx, cy = (vc1x + vc2x)/2, (hc1y + hc2y)/2
+    
+    a1 = math.degrees(math.atan((cx-vc1x)/(vc1y-cy)))
+    a2 = math.degrees(math.atan((cx-vc2x)/(vc2y-cy)))
+    a = (a1+a2)/2
+
+
+    dy = math.hypot(vc2y - vc1y, vc2x - vc1x)
+    dx = math.hypot(hc2x - hc1x, hc2y - hc1y)
+    print dy, dx, cx, cy
+    sx, sy = DX/dx, DY/dy 
+
+
+    #Rotating center of puck
+    rcx, rcy =   rotate(cx, cy, -a)  
+    
+
+    
+    roi_w, roi_h = int(ROI_X / sx), int(ROI_Y / sy)
+    roi_x, roi_y = int(rcx-roi_w/2), int(rcy-roi_h/2)
+
+    print a, sx, sy, roi_w, roi_h 
+    
+    img.config.rotation=-a
+    img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = roi_x, roi_y, roi_w, roi_h
+    img.config.setCalibration(Calibration(sx, sy, -roi_w/2, -roi_h/2))
+    img.config.save()
+    
+finally: 
+    p.removeOverlay(ov_text)
+    img.refresh()
+
+  

script/local.py

@@ -32,6 +32,19 @@ run("motion/tools")
 run("imgproc/Utils")
 run("tools/Math")
 
+def system_check(robot_move=True):
+    if not air_pressure_ok.read():
+        raise Exception("Air pressure is not ok")
+    if not n2_pressure_ok.read():
+        raise Exception("N2 pressure is not ok")
+    if robot_move:
+        if not feedback_local_safety.read():
+            raise Exception("Local safety not released")
+        if not feedback_psys_safety.read():
+            raise Exception("Psys safety not released")
+        if not guiding_tool_park().read():
+            raise Exception("Guiding tool not parked")
+        
 
 ###################################################################################################
 #  Device initialization

script/motion/tools.py

@@ -30,21 +30,20 @@ def move_home():
     wait_end_of_move()
 
 def move_to_laser():
+    robot.reset_motion()
     tool = robot.tool
     d = robot.get_distance_to_pnt("pLaser")
     if d<0:
         raise Exception ("Error calculating distance to laser: " + str(d))
     if d<POSITION_TOLERANCE:
         print "FROM LASER"
-        robot.reset_motion()
         robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
         return
-    d = robot.get_distance_to_pnt("pLaserAppro")
+    d = robot.get_distance_to_pnt("pLaserHome")
     if d<0:
         raise Exception ("Error calculating distance to laser appro: " + str(d))
     if d<POSITION_TOLERANCE:
         print "FROM APPRO"
-        robot.reset_motion()
         robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
         return
     d = robot.get_distance_to_pnt("pHome")
@@ -52,8 +51,7 @@ def move_to_laser():
         raise Exception ("Error calculating distance to home: " + str(d))
     if d<POSITION_TOLERANCE:
         print "FROM HOME"
-        robot.reset_motion()
-        robot.movel("pLaserAppro", robot.tool, DESC_DEFAULT)
+        robot.movej("pLaserHome", robot.tool, DESC_DEFAULT)
         robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)    
         return
     raise Exception ("Must be in home position to start move to laser")