Startup

script/tools/CheckGripper.py

@@ -0,0 +1,23 @@
+
+
+
+vector = [1.0,0.0]
+
+
+
+class GripperTool(RegisterBase):    
+    def doRead(self):
+        return 1.0 if robot.is_tool_open()  else 0.0
+
+    def doWrite(self, val):
+        if val:
+            robot.open_tool()
+        else:
+            robot.close_tool()
+        
+
+add_device(GripperTool("gripper_tool"), True)
+
+
+vscan(gripper_tool, [laser_distance,], vector, latency = 3, passes=50)
+

script/tools/CheckPuckDetection.py

@@ -0,0 +1,6 @@
+USR,PWD = "pi", "raspberry"
+HOST,PORT = "tell6d-raspberrypi", 22
+CMD= "sudo systemctl status puck_detection.service"
+
+ret = run("tools/SshExec")
+set_return(ret)

script/tools/Math.py

@@ -0,0 +1,68 @@
+###################################################################################################
+#  Math utilities
+###################################################################################################
+
+
+from mathutils import estimate_peak_indexes, fit_gaussians, create_fit_point_list, Gaussian
+import java.awt.Color as Color
+ 
+import mathutils
+mathutils.MAX_ITERATIONS = 100000
+
+def fit(ydata, xdata = None, draw_plot = True):
+    if xdata is None:
+        xdata = frange(0, len(ydata), 1)
+    max_y= max(ydata)    
+    index_max = ydata.index(max_y)    
+    max_x= xdata[index_max]
+    #print "Max index:" + str(index_max),
+    #print " x:" + str(max_x),
+    #print " y:" + str(max_y)
+    
+    if draw_plot:
+        plots = plot([ydata],["data"],[xdata], title="Fit" )
+        p = None if plots is None else plots[0]
+
+    gaussians = fit_gaussians(ydata, xdata, [index_max,])
+    if gaussians[0] is None:
+        if draw_plot and (p is not None):
+            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)    
+        print "Fitting error"
+        return (None, None, None)
+        
+    (norm, mean, sigma) = gaussians[0]
+    if draw_plot:
+        fitted_gaussian_function = Gaussian(norm, mean, sigma)    
+        scale_x = [float(min(xdata)), float(max(xdata)) ]
+        points = max((len(xdata)+1), 100)
+        resolution = (scale_x[1]-scale_x[0]) / points    
+        fit_y = []
+        fit_x = frange(scale_x[0],scale_x[1],resolution, True)
+        for x in fit_x:
+            fit_y.append(fitted_gaussian_function.value(x))            
+        #Server
+        if p is None:
+            plot([ydata,fit_y],["data","fit"],[xdata,fit_x], title="Fit")             
+            draw_plot = False
+        else:
+            p.addSeries(LinePlotSeries("fit"))
+            p.getSeries(1).setData(fit_x, fit_y)            
+
+    if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):        
+        if draw_plot:
+            p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
+        #print "Mean -> " +  str(mean)
+        return (norm, mean, sigma)
+    else:
+        if draw_plot:
+            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
+        #print "Invalid gaussian fit: " +  str(mean)
+        return (None, None, None)
+
+
+def enforce_monotonic(x):
+    epsilon = 1e-8
+    for i in range(len(x)-1):
+        if x[i+1]<=x[i]:
+            x[i+1] = x[i]+ epsilon        
+    return x

script/tools/RestartPuckDetection.py

@@ -0,0 +1,8 @@
+USR,PWD = "pi", "raspberry"
+HOST,PORT = "tell6d-raspberrypi", 22
+CMD= "sudo systemctl stop puck_detection.service;sudo systemctl start puck_detection.service"
+
+ret = run("tools/SshExec")
+set_return(ret)
+
+puck_detection.initialize()

script/tools/SshExec.py

@@ -0,0 +1,61 @@
+import com.jcraft.jsch.Channel as Channel
+import com.jcraft.jsch.ChannelShell as ChannelShell
+import com.jcraft.jsch.JSch as JSch
+import com.jcraft.jsch.JSchException as JSchException
+import com.jcraft.jsch.Session as Session
+import java.lang.System as System
+import java.io.PrintStream as PrintStream
+
+
+#Parameters:
+#CMD
+#USR
+#PWD
+#HOST
+#PORT
+
+jsch= JSch()
+session=jsch.getSession(USR, HOST, PORT)
+session.setPassword(PWD)
+session.setConfig("StrictHostKeyChecking", "no")
+session.connect()
+
+#channel=session.openChannel("shell")
+#input_stream=channel.getInputStream()
+#output_stream = channel.getOutputStream()
+#print_stream = PrintStream(output_stream, True)
+#print_stream.println("ls")   
+
+channel=session.openChannel("exec")
+channel.setCommand(CMD)
+channel.setInputStream(None)
+channel.setOutputStream(System.out)
+channel.setErrStream(System.err)
+input_stream=channel.getInputStream()
+
+
+def wait_ret():
+    global input_stream
+    rx = ""
+    while True:                
+        while (input_stream.available() > 0):
+        	i = input_stream.read()
+        	if i < 0: 
+        	    break
+        	rx=rx+chr(i)    	        	        
+        if channel.closed: 
+            break
+        time.sleep(0.1)              
+    return rx  
+    
+
+channel.connect()
+
+try:
+    ret = wait_ret()    
+except:
+    channel.disconnect()
+    session.disconnect()
+    raise
+
+set_return(ret)

script/tools/StopPuckDetection.py

@@ -0,0 +1,8 @@
+USR,PWD = "pi", "raspberry"
+HOST,PORT = "tell6d-raspberrypi", 22
+CMD= "sudo systemctl stop puck_detection.service"
+
+ret = run("tools/SshExec")
+set_return(ret)
+
+