grbl probe test

#!/usr/bin/python
"""
do a test of the probe functionality
probe in a single position over and over again
"""
import serial
import time
import numpy

import commands
import re
import sys
import math

grbl_dev = "/dev/ttyUSB0"
grbl_baud = 9600
grbl_serial = serial.Serial(grbl_dev, grbl_baud)

# everything in mm

probe_feed = 10
probe_acceleration = 10

#z_max = 0.1
#z_max = 1.0
z_max = 0.0
z_min = -1.0
z_del = 0.01


x_pos = 0.0
y_pos = 0.0

verbose = 0


def send_grbl_command( cmd ) :
  if verbose:
    print "# sending '" + cmd + "'"
  grbl_serial.write(cmd + "\n")
  grbl_out = grbl_serial.readline()
  if verbose:
    print "#  got :", grbl_out.strip()
  while ( not re.search("ok", grbl_out) ):
    if verbose:
      print "#  got :", grbl_out.strip()
    grbl_out = grbl_serial.readline()
  if verbose:
    print "#", grbl_out

def get_grbl_probe_state( ) :
  grbl_serial.write("$S\n")
  r = grbl_serial.readline()
  grbl_serial.readline()
  return r

def get_grbl_var_position( var_name ):
  var_seen = 0
  var_pos = 0.0
  #grbl_serial.write("$?\n")
  grbl_serial.write("?")
  grbl_out = grbl_serial.readline()

  if verbose:
    print "#  get_grbl_var_position(", var_name, "): got :", grbl_out.strip()

  m = re.search( "^<([^,]*),MPos:([^,]*),([^,]*),([^,]*),", grbl_out)
  if ( m ):
    if verbose:
      print "# matched", m.group(0)
    state = m.group(1)
    x = float(m.group(2))
    y = float(m.group(3))
    z = float(m.group(4))

    if ( var_name == 'x') or ( var_name == 'X'):
      return x
    if ( var_name == 'y') or ( var_name == 'Y'):
      return y
    if ( var_name == 'z') or ( var_name == 'Z'):
      return z

def wait_for_var_position( var_name, var_val ):
  #sleepy = 0.05
  sleepy = 1
  var_epsilon = 0.001
  cur_val = get_grbl_var_position( var_name )
  if verbose:
    print "#", str(var_val), " var_epsilon", str(var_epsilon), "cur_x", str(cur_val)
  while (math.fabs(var_val - cur_val) > var_epsilon):
    if verbose:
      print "# cur_val", str(cur_val), ", waiting for ", var_name, str(var_val)
    time.sleep(sleepy)
    cur_val = get_grbl_var_position( var_name )

send_grbl_command( "" )
send_grbl_command( "" )
send_grbl_command( "g90" )
send_grbl_command( "g21" )

send_grbl_command( "g1z" + str(z_max) )
send_grbl_command( "g0 x" + str(x_pos) + " y" + str(y_pos) )

send_grbl_command( "$" )

send_grbl_command( "$23=" + str(probe_feed) )
send_grbl_command( "$24=" + str(probe_acceleration) )
send_grbl_command( "$25=-1" )  # z lower bound
send_grbl_command( "$26=1" )   # turn on probe

if verbose:
  print "# grbl setup done\n"
  print "setup done\n"

get_grbl_var_position( "Z" )

height = {}

even_odd = 0


for it in range(1000):

  time.sleep(2)

  send_grbl_command( "g1z" + str(z_max) )
  wait_for_var_position("Z", z_max)
  send_grbl_command( "g0 x" + str(x_pos) + " y" + str(y_pos) )
  wait_for_var_position("X", x_pos)
  wait_for_var_position("Y", y_pos)

  time.sleep(1)

  send_grbl_command( "$P" )

  z = get_grbl_var_position( "Z" )

  c = get_grbl_probe_state( )

  if (verbose):
    print " z position: ", str(z)


  if (z <= z_min):
    sys.exit("ERROR: z_min (" + str(z_min) + ") reached")

  print str(x_pos), str(y_pos), str(z), str(c)
  #print str(x_pos), str(y_pos), str(z)
  sys.stdout.flush()


send_grbl_command( "g1z" + str(z_max) )
send_grbl_command( "g0x0y0" )