def sendMsg(self, msg) : if self.isConnected : (axis, action) = msg if axis == 0 : if action == 'CALIB' : KNI.calibrate(0) elif action == 'SETHOME' : from KNI import TMovement self.home = TMovement() KNI.getPosition(self.home.pos) self.home.transition = KNI.PTP self.home.velocity = 50 self.home.acceleration = 2 elif action == 'MOVEHOME' : if self.home != None : KNI.executeMovement(self.home) else : if action == 'STOP' : KNI.moveMot(axis, KNI.getEncoder(axis), 50, 2) elif action == 'RIGHT' : if axis in self.minusMotors : KNI.moveMot(axis, 0, 50, 2) else : KNI.moveMot(axis, 31000, 50, 2) elif action == 'LEFT' : if axis in self.minusMotors : KNI.moveMot(axis, -31000, 50, 2) else : KNI.moveMot(axis, 0, 50, 2) else : print "Port is not connected"
def check_motors(self):
"""Test each motor status"""
motors = [None]*len(self.AXIS_LIMITS)
for i in xrange(1, len(self.AXIS_LIMITS)):
LOG.debug('checking motor %i', i)
enc = KNI.getEncoder(i)
motors[i] = KNI.moveMot(i, enc, self._speed, self._accel) != -1
return motors
def set_neck_orientation(self, xyz, wait=True):
"""Absolute orientation:
Our own version since the IK is useless (ERROR: No solution found)"""
encs = [ (4+i, SpineHW.rad2enc(4+i, v)) for i,v in enumerate(xyz) ]
for axis, enc in encs:
LOG.debug('moving axis %i to encoder %i', axis, enc)
if KNI.moveMot(axis, enc, self._speed, self._accel) == -1:
raise SpineError('failed to reach rotation (axis %i)' % axis)
if not wait:
return
for axis, enc in encs:
if KNI.waitForMot(axis, enc, self._tolerance) != 1:
raise SpineError('failed to wait for motor %i' % (axis))
