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 get_neck_info(self): """Returns NeckInfo instance""" # XXX: we are not using the same reference: create a mapping function # tp = KNI.TPos() # KNI.getPosition(tp) # self._neck_info.pos= self.round([tp.X, tp.Y, tp.Z]) # self._neck_info.rot= self.round([tp.phi, tp.theta, tp.psi]) self._neck_info.rot = self.round( \ [ SpineHW.enc2rad(i, KNI.getEncoder(i)) for i in \ xrange(4, len(self.AXIS_LIMITS)) ]) return self._neck_info
def get_torso_info(self): """Returns TorsoInfo instance""" self._torso_info.rot= self.round([SpineHW.enc2rad(2,KNI.getEncoder(2)), 0.0, SpineHW.enc2rad(1,KNI.getEncoder(1))]) return self._torso_info