#!/usr/bin/python import time,hal import serial try: port = '/dev/ttyUSB0' ser = serial.Serial( port, baudrate=9600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS ) ser.open() ser.isOpen() except: print "ERROR : sx_drive - No serial interface found at %s"% port raise SystemExit # disable limits and set default speed/direction ser.write('LD3\n') ser.write('MC\n') ser.write('MR11\n')# 25000 steps/rev ser.write('V0\n') ser.write('H-\n') ser.write('A25\nAD25\n') # set up HAL pins h = hal.component("sx_drive") h.newpin("enable", hal.HAL_BIT, hal.HAL_IN) h.newpin("debug", hal.HAL_BIT, hal.HAL_IN) h.newpin("position-mode", hal.HAL_BIT, hal.HAL_IN) h.newpin("up-to-speed", hal.HAL_BIT, hal.HAL_OUT) h.newpin("velocity", hal.HAL_FLOAT, hal.HAL_IN) h.newpin("position", hal.HAL_S32, hal.HAL_IN) h.newpin("acceleration", hal.HAL_FLOAT, hal.HAL_IN) h.newpin("resolution", hal.HAL_FLOAT, hal.HAL_IN) h.newpin("position-scale", hal.HAL_FLOAT, hal.HAL_IN) h.ready() enable = direction = running = False; position_mode = False scale = position = velocity = 0.0; out = '' accel = 25 ready_time = 0.0 try: while 1: if h['position-scale'] > 0: scale = h['position-scale'] else: scale = 25000 if not h['enable'] == enable: if h['enable']: ser.write('ON\n') else: ser.write('OFF\n') running = False if not h['position-mode'] == position_mode: if h['position-mode']: print "Position mode" ser.write('S\nMN\nPZ\n') ser.write('1PR\n') else: print "continuous Mode" ser.write('S\nMC\n') velocity = running = 0 position_mode = h['position-mode'] if h['position-mode']: # Absolute position mode if not h['position'] == position: if not h['velocity']: continue ser.write('D%s\n'% (int(h['position']*(25000/scale)) ) ) ser.write('1PR\n') ser.write('V%s\n'%(abs(h['velocity']) ) ) ser.write('G\n') ser.write('1PR\n') position = h['position'] else: # continuous rotation mode if not h['velocity'] == velocity or (not h['enable'] == enable and h['enable']): if h['velocity'] < 0 and direction: ser.write('S\nH-\n') direction = running = False elif h['velocity'] > 0 and not direction: ser.write('S\n') ser.write('H+\n') direction = True running = False if not h['acceleration'] == accel and not h['acceleration'] == 0 and not running: accel = h['acceleration'] ser.write('A%f\nAD%f\n'%(accel,accel) ) if running: command = "IV" else: command = "V" ser.write('%s%s\n'%(command,abs(h['velocity']) ) ) delta_speed = abs(0 - abs(h['velocity']) - abs(velocity)) if h['velocity'] < 0 and velocity > 0 or h['velocity'] > 0 and velocity > 0: delta_speed = delta_speed * 2 ready_time = time.time() + delta_speed / accel print ready_time if h['velocity'] == 0: ser.write('S\n') running = False elif not running and h['enable']: ser.write('G\n') running = True enable = h['enable'] velocity = h['velocity'] if time.time() > ready_time: h['up-to-speed'] = True else:h['up-to-speed'] = False time.sleep(.1) if h['debug']: while ser.inWaiting() > 0: out += ser.read(1) if out != '': print out out = '' except KeyboardInterrupt: raise SystemExit