class RobotPi(): '''''' def __init__(self, silentNetFail=False, expectedIds=None, commandRate=40, loud=False, skipInit=False): '''Initialize the robot. Keyword arguments: silentNetworkFail -- Whether or not to fail silently if the network does not find all the dynamixel servos. nServos -- How many servos are connected to the robot, i.e. how many to expect to find on the network. commandRate -- Rate at which the motors should be commanded, in Hertz. Default: 40. ''' # The number of Dynamixels on the bus. self.expectedIds = expectedIds if expectedIds is not None else range(8) self.nServos = len(self.expectedIds) self.silentNetFail = silentNetFail self.sleep = 1. / float(commandRate) self.loud = loud #if self.nServos != 9: # pass # #raise Exception('Unfortunately, the Robot class currently assumes 9 servos.') # Default baud rate of the USB2Dynamixel device. self.baudRate = 1000000 if not skipInit: self.initServos() def initServos(self): # Set your serial port accordingly. if os.name == "posix": possibilities = ['dev/ttyACM' ] + ['/dev/ttyACM' + str(i) for i in range(25)] portName = None for pos in possibilities: if os.path.exists(pos): portName = pos if portName is None: raise Exception('Could not find any of %s' % repr(possibilities)) else: portName = "COM9" serial = SerialStream(port=portName, baudrate=self.baudRate, timeout=1) self.net = DynamixelNetwork(serial) #print 'Prescan...' #print self.net.get_dynamixels() print "Scanning for Dynamixels...", self.net.scan(min(self.expectedIds), max(self.expectedIds)) self.actuators = [] self.actuatorIds = [] for dyn in self.net.get_dynamixels(): print dyn.id, self.actuatorIds.append(dyn.id) self.actuators.append(self.net[dyn.id]) print "...Done" if len(self.actuators) != self.nServos and not self.silentNetFail: raise RobotFailure( 'Expected to find %d servos on network, but only got %d (%s)' % (self.nServos, len(self.actuators), repr(self.actuatorIds))) for actuator in self.actuators: #actuator.moving_speed = 90 #actuator.synchronized = True #actuator.torque_enable = True #actuator.torque_limit = 1000 #actuator.max_torque = 1000 actuator.moving_speed = 250 actuator.synchronized = True actuator.torque_enable = True actuator.torque_limit = 1000 actuator.max_torque = 1000 actuator.ccw_compliance_margin = 3 actuator.cw_compliance_margin = 3 self.net.synchronize() #print 'options are:' #for op in dir(self.actuators[0]): # print ' ', op #for ac in self.actuators: # print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load # ac.read_all() self.currentPos = None self.resetClock() def run(self, motionFunction, runSeconds=10, resetFirst=True, interpBegin=0, interpEnd=0, timeScale=1, logFile=None, extraLogInfoFn=None): '''Run the robot with a given motion generating function. Positional arguments: motionFunction -- Function used to generate the desired motor positions. This function must take a single argument -- time, in seconds -- and must return the desired length 9 vector of motor positions. The current implementation expects that this function will be deterministic. Keyword arguments: runSeconds -- How many seconds to run for. This is in addition to the time added for interpBegin and interpEnd, if any. Default: 10 resetFirst -- Begin each run by resetting the robot to its base position, currently implemented as a transition from CURRENT -> POS_FLAT -> POS_READY. Default: True interpBegin -- Number of seconds over which to interpolate from current position to commanded positions. If this is not None, the robot will spend the first interpBegin seconds interpolating from its current position to that specified by motionFunction. This should probably be used for motion models which do not return POS_READY at time 0. Affected by timeScale. Default: None interpEnd -- Same as interpBegin, but at the end of motion. If interpEnd is not None, interpolation is performed from final commanded position to POS_READY, over the given number of seconds. Affected by timeScale. Default: None timeScale -- Factor by which time should be scaled during this run, higher is slower. Default: 1 logFile -- File to log time/positions to, should already be opened. Default: None extraLogInfoFn -- Function to call and append info to every line the log file. Should return a string. Default: None ''' #net, actuators = initialize() #def run(self, motionFunction, runSeconds = 10, resetFirst = True # interpBegin = 0, interpEnd = 0): if self.loud: print 'Starting motion.' failures = self.pingAll() if failures: self.initServos() failures = self.pingAll() if failures: raise RobotFailure( 'Could not communicate with servos %s at beginning of run.' % repr(failures)) self.resetClock() self.currentPos = self.readCurrentPosition() if logFile: #print >>logFile, '# time, servo goal positions (9), servo actual positions (9), robot location (x, y, age)' print >> logFile, '# time, servo goal positions (9), robot location (x, y, age)' # Reset the robot position, if desired if resetFirst: self.interpMove(self.readCurrentPosition(), POS_FLAT, 3) print "POS_FLAT: " + str(POS_FLAT) print "POS_READY: " + str(POS_READY) self.interpMove(POS_FLAT, POS_READY, 3) #self.interpMove(POS_READY, POS_HALFSTAND, 4) self.currentPos = POS_READY self.resetClock() # Begin with a segment smoothly interpolated between the # current position and the motion model. if interpBegin is not None: self.interpMove(self.currentPos, scaleTime(motionFunction, timeScale), interpBegin * timeScale, logFile, extraLogInfoFn) self.currentPos = motionFunction(self.time) # Main motion segment self.interpMove(scaleTime(motionFunction, timeScale), scaleTime(motionFunction, timeScale), runSeconds * timeScale, logFile, extraLogInfoFn) self.currentPos = motionFunction(self.time) # End with a segment smoothly interpolated between the # motion model and a ready position. if interpEnd is not None: self.interpMove(scaleTime(motionFunction, timeScale), POS_READY, interpEnd * timeScale, logFile, extraLogInfoFn) failures = self.pingAll() if failures: # give it a second chance sleep(1) failures = self.pingAll() if failures: raise RobotFailure('Servos %s may have died during run.' % repr(failures)) def interpMove(self, start, end, seconds, logFile=None, extraLogInfoFn=None): '''Moves between start and end over seconds seconds. start and end may be functions of the time.''' self.updateClock() timeStart = self.time timeEnd = self.time + seconds ii = 0 tlast = self.time while self.time < timeEnd: #print 'time:', self.time ii += 1 posS = start(self.time) if isinstance(start, FunctionType) else start posE = end(self.time) if isinstance(end, FunctionType) else end print "Start: " + str(posS) print "End: " + str(posE) goal = lInterp(self.time, [timeStart, timeEnd], posS, posE) #startSend = datetime.now() cmdPos = self.commandPosition(goal) #deltaTimeSend = datetime.now() - startSend #print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing if logFile: extraInfo = '' if extraLogInfoFn: extraInfo = extraLogInfoFn() print >> logFile, self.time, ' '.join([str(x) for x in cmdPos]), #print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators), print >> logFile, extraInfo #volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)] #print ' '.join(volts) #[ac.read_all() for ac in self.actuators] #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)] #print ' '.join(positions) #print ''.join(['x' if ac.led else ' ' for ac in self.actuators]) #sleep(self.sleep) #sleep(float(1)/100) self.updateClock() secElapsed = self.time - tlast tosleep = self.sleep - secElapsed if tosleep > 0: sleep(tosleep) self.updateClock() tlast = self.time # # currentPos = None # # if resetFirst: # currentPos = self.currentPostion() # time0 = datetime.datetime.now() # seconds = 0 # # while seconds < 10: # timeDiff = datetime.datetime.now() - time0 # seconds = timeDiff.seconds + timeDiff.microseconds/1e6 # # if seconds < 3: # goal = lInterp(seconds, [0, 3], startingPos, POS_FLAT) # elif seconds < 6: # goal = lInterp(seconds, [3, 6], POS_FLAT, POS_READY) # elif seconds < 10: # goal = lInterp(seconds, [6, 10], POS_READY, POS_HALFSTAND) # else: # break # # self.commandPosition(goal) # sleep(self.sleep) # # currentPos = POS_HALFSTAND # # if interpBegin is not None: # if currentPos is None: # currentPos = self.currentPostion() # # time0 = datetime.datetime.now() # seconds = 0 # # while seconds < interpBegin: # timeDiff = datetime.datetime.now() - time0 # seconds = timeDiff.seconds + timeDiff.microseconds/1e6 # # goal = lInterp(seconds, [0, interpBegin], currentPos, motionFunction(seconds)) # # self.commandPosition(goal) # time.sleep(self.sleep) # def resetClock(self): '''Resets the robot time to zero''' self.time0 = datetime.now() self.time = 0.0 def updateClock(self): '''Updates the Robots clock to the current time''' timeDiff = datetime.now() - self.time0 self.time = timeDiff.seconds + timeDiff.microseconds / 1e6 def readyPosition(self, persist=False): if persist: self.resetClock() while self.time < 2.0: self.commandPosition(POS_READY) sleep(.1) self.updateClock() else: self.commandPosition(POS_READY) sleep(2) def commandPosition(self, position, crop=True, cropWarning=False): '''Command the given position commandPosition will command the robot to move its servos to the given position vector. This vector is cropped to the physical limits of the robot and converted to integer Positional arguments: position -- A length 9 vector of desired positions. Keyword arguments: cropWarning -- Whether or not to print a warning if the positions are cropped. Default: False. ''' if len(position) != self.nServos: raise Exception( 'Expected postion vector of length %d, got %s instead' % (self.nServos, repr(position))) if crop: goalPosition = self.cropPosition([int(xx) for xx in position], cropWarning) else: goalPosition = [int(xx) for xx in position] if self.loud: posstr = ', '.join(['%4d' % xx for xx in goalPosition]) print '%.2fs -> %s' % (self.time, posstr) for ii, actuator in enumerate(self.actuators): actuator.goal_position = goalPosition[ii] self.net.synchronize() #[ac.read_all() for ac in self.actuators] #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)] #print ' '.join(positions) #print ''.join(['x' if ac.led else ' ' for ac in self.actuators]) + ' ' ,#XXX:THIS IS THE SLOW PART!!! #print ' '.join(['%.1f' % ac.current_voltage for ac in self.actuators])#XXX:OR THIS return goalPosition def cropPosition(self, position, cropWarning=False): '''Crops the given positions to their appropriate min/max values. Requires a vector of length 9 to be sure the IDs are in the assumed order.''' if len(position) != self.nServos: raise Exception('cropPosition expects a vector of length %d' % self.nServos) ret = copy(position) for ii in [0, 2, 4, 6]: ret[ii] = max(MIN_INNER, min(MAX_INNER, ret[ii])) ret[ii + 1] = max(MIN_OUTER, min(MAX_OUTER, ret[ii + 1])) if cropWarning and ret != position: print 'Warning: cropped %s to %s' % (repr(position), repr(ret)) return ret def readCurrentPosition(self): ret = [] if len(self.actuators) != self.nServos: raise RobotFailure('Lost some servos, now we only have %d' % len(self.actuators)) for ac in self.actuators: #ac.read_all() #ret.append(ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) ret.append(ac.current_position) #sleep(.001) return ret def pingAll(self): failures = [] for ii in self.actuatorIds: result = self.net.ping(ii) if result is False: failures.append(ii) return failures def printStatus(self): pos = self.readCurrentPosition() print 'Positions:', ' '.join( ['%d:%d' % (ii, pp) for ii, pp in enumerate(pos)]) def shimmy(self): '''Moves through a set of checks and makes sure the robot is still moving.''' self.commandPosition(POS_READY) sleep(.8) success = True success &= self.checkMove(POS_READY, POS_CHECK_1) success &= self.checkMove(POS_CHECK_1, POS_CHECK_2) success &= self.checkMove(POS_CHECK_2, POS_CHECK_3) success &= self.checkMove(POS_CHECK_3, POS_CHECK_2) success &= self.checkMove(POS_CHECK_2, POS_CHECK_1) success &= self.checkMove(POS_CHECK_1, POS_READY) return success def checkMove(self, aa, bb): aa = array(aa) bb = array(bb) self.commandPosition(aa) posAA = array(self.readCurrentPosition()) self.commandPosition(bb) sleep(.4) posBB = array(self.readCurrentPosition()) success = True success &= all(abs((aa - bb) - (posAA - posBB)) < 50) success &= all(abs(aa - posAA) < 50) success &= all(abs(bb - posBB) < 50) if not success and False: print 'shimmy errors' print(aa - bb) - (posAA - posBB) print aa - posAA print bb - posBB return success
class RobotPi(): '''''' def __init__(self, silentNetFail = False, expectedIds = None, commandRate = 40, loud = False, skipInit = False): '''Initialize the robot. Keyword arguments: silentNetworkFail -- Whether or not to fail silently if the network does not find all the dynamixel servos. nServos -- How many servos are connected to the robot, i.e. how many to expect to find on the network. commandRate -- Rate at which the motors should be commanded, in Hertz. Default: 40. ''' # The number of Dynamixels on the bus. self.expectedIds = expectedIds if expectedIds is not None else range(8) self.nServos = len(self.expectedIds) self.silentNetFail = silentNetFail self.sleep = 1. / float(commandRate) self.loud = loud #if self.nServos != 9: # pass # #raise Exception('Unfortunately, the Robot class currently assumes 9 servos.') # Default baud rate of the USB2Dynamixel device. self.baudRate = 1000000 if not skipInit: self.initServos() def initServos(self): # Set your serial port accordingly. if os.name == "posix": possibilities = ['dev/ttyACM'] + ['/dev/ttyACM' + str(i) for i in range(25)] portName = None for pos in possibilities: if os.path.exists(pos): portName = pos if portName is None: raise Exception('Could not find any of %s' % repr(possibilities)) else: portName = "COM9" serial = SerialStream(port = portName, baudrate = self.baudRate, timeout = 1) self.net = DynamixelNetwork(serial) #print 'Prescan...' #print self.net.get_dynamixels() print "Scanning for Dynamixels...", self.net.scan(min(self.expectedIds), max(self.expectedIds)) self.actuators = [] self.actuatorIds = [] for dyn in self.net.get_dynamixels(): print dyn.id, self.actuatorIds.append(dyn.id) self.actuators.append(self.net[dyn.id]) print "...Done" if len(self.actuators) != self.nServos and not self.silentNetFail: raise RobotFailure('Expected to find %d servos on network, but only got %d (%s)' % (self.nServos, len(self.actuators), repr(self.actuatorIds))) for actuator in self.actuators: #actuator.moving_speed = 90 #actuator.synchronized = True #actuator.torque_enable = True #actuator.torque_limit = 1000 #actuator.max_torque = 1000 actuator.moving_speed = 250 actuator.synchronized = True actuator.torque_enable = True actuator.torque_limit = 1000 actuator.max_torque = 1000 actuator.ccw_compliance_margin = 3 actuator.cw_compliance_margin = 3 self.net.synchronize() #print 'options are:' #for op in dir(self.actuators[0]): # print ' ', op #for ac in self.actuators: # print 'Voltage at', ac, 'is', ac.current_voltage, 'load is', ac.current_load # ac.read_all() self.currentPos = None self.resetClock() def run(self, motionFunction, runSeconds = 10, resetFirst = True, interpBegin = 0, interpEnd = 0, timeScale = 1, logFile = None, extraLogInfoFn = None): '''Run the robot with a given motion generating function. Positional arguments: motionFunction -- Function used to generate the desired motor positions. This function must take a single argument -- time, in seconds -- and must return the desired length 9 vector of motor positions. The current implementation expects that this function will be deterministic. Keyword arguments: runSeconds -- How many seconds to run for. This is in addition to the time added for interpBegin and interpEnd, if any. Default: 10 resetFirst -- Begin each run by resetting the robot to its base position, currently implemented as a transition from CURRENT -> POS_FLAT -> POS_READY. Default: True interpBegin -- Number of seconds over which to interpolate from current position to commanded positions. If this is not None, the robot will spend the first interpBegin seconds interpolating from its current position to that specified by motionFunction. This should probably be used for motion models which do not return POS_READY at time 0. Affected by timeScale. Default: None interpEnd -- Same as interpBegin, but at the end of motion. If interpEnd is not None, interpolation is performed from final commanded position to POS_READY, over the given number of seconds. Affected by timeScale. Default: None timeScale -- Factor by which time should be scaled during this run, higher is slower. Default: 1 logFile -- File to log time/positions to, should already be opened. Default: None extraLogInfoFn -- Function to call and append info to every line the log file. Should return a string. Default: None ''' #net, actuators = initialize() #def run(self, motionFunction, runSeconds = 10, resetFirst = True # interpBegin = 0, interpEnd = 0): if self.loud: print 'Starting motion.' failures = self.pingAll() if failures: self.initServos() failures = self.pingAll() if failures: raise RobotFailure('Could not communicate with servos %s at beginning of run.' % repr(failures)) self.resetClock() self.currentPos = self.readCurrentPosition() if logFile: #print >>logFile, '# time, servo goal positions (9), servo actual positions (9), robot location (x, y, age)' print >>logFile, '# time, servo goal positions (9), robot location (x, y, age)' # Reset the robot position, if desired if resetFirst: self.interpMove(self.readCurrentPosition(), POS_FLAT, 3) print "POS_FLAT: " + str(POS_FLAT) print "POS_READY: " + str(POS_READY) self.interpMove(POS_FLAT, POS_READY, 3) #self.interpMove(POS_READY, POS_HALFSTAND, 4) self.currentPos = POS_READY self.resetClock() # Begin with a segment smoothly interpolated between the # current position and the motion model. if interpBegin is not None: self.interpMove(self.currentPos, scaleTime(motionFunction, timeScale), interpBegin * timeScale, logFile, extraLogInfoFn) self.currentPos = motionFunction(self.time) # Main motion segment self.interpMove(scaleTime(motionFunction, timeScale), scaleTime(motionFunction, timeScale), runSeconds * timeScale, logFile, extraLogInfoFn) self.currentPos = motionFunction(self.time) # End with a segment smoothly interpolated between the # motion model and a ready position. if interpEnd is not None: self.interpMove(scaleTime(motionFunction, timeScale), POS_READY, interpEnd * timeScale, logFile, extraLogInfoFn) failures = self.pingAll() if failures: # give it a second chance sleep(1) failures = self.pingAll() if failures: raise RobotFailure('Servos %s may have died during run.' % repr(failures)) def interpMove(self, start, end, seconds, logFile=None, extraLogInfoFn=None): '''Moves between start and end over seconds seconds. start and end may be functions of the time.''' self.updateClock() timeStart = self.time timeEnd = self.time + seconds ii = 0 tlast = self.time while self.time < timeEnd: #print 'time:', self.time ii += 1 posS = start(self.time) if isinstance(start, FunctionType) else start posE = end(self.time) if isinstance(end, FunctionType) else end print "Start: " + str(posS) print "End: " + str(posE) goal = lInterp(self.time, [timeStart, timeEnd], posS, posE) #startSend = datetime.now() cmdPos = self.commandPosition(goal) #deltaTimeSend = datetime.now() - startSend #print "sent: " + str(deltaTimeSend) #TODO: remove this line after testing if logFile: extraInfo = '' if extraLogInfoFn: extraInfo = extraLogInfoFn() print >>logFile, self.time, ' '.join([str(x) for x in cmdPos]), #print >>logFile, ' '.join(str(ac.current_position) for ac in self.actuators), print >>logFile, extraInfo #volts = ['%d: %s' % (ii,ac.current_voltage) for ii,ac in enumerate(self.actuators)] #print ' '.join(volts) #[ac.read_all() for ac in self.actuators] #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)] #print ' '.join(positions) #print ''.join(['x' if ac.led else ' ' for ac in self.actuators]) #sleep(self.sleep) #sleep(float(1)/100) self.updateClock() secElapsed = self.time - tlast tosleep = self.sleep - secElapsed if tosleep > 0: sleep(tosleep) self.updateClock() tlast = self.time # # currentPos = None # # if resetFirst: # currentPos = self.currentPostion() # time0 = datetime.datetime.now() # seconds = 0 # # while seconds < 10: # timeDiff = datetime.datetime.now() - time0 # seconds = timeDiff.seconds + timeDiff.microseconds/1e6 # # if seconds < 3: # goal = lInterp(seconds, [0, 3], startingPos, POS_FLAT) # elif seconds < 6: # goal = lInterp(seconds, [3, 6], POS_FLAT, POS_READY) # elif seconds < 10: # goal = lInterp(seconds, [6, 10], POS_READY, POS_HALFSTAND) # else: # break # # self.commandPosition(goal) # sleep(self.sleep) # # currentPos = POS_HALFSTAND # # if interpBegin is not None: # if currentPos is None: # currentPos = self.currentPostion() # # time0 = datetime.datetime.now() # seconds = 0 # # while seconds < interpBegin: # timeDiff = datetime.datetime.now() - time0 # seconds = timeDiff.seconds + timeDiff.microseconds/1e6 # # goal = lInterp(seconds, [0, interpBegin], currentPos, motionFunction(seconds)) # # self.commandPosition(goal) # time.sleep(self.sleep) # def resetClock(self): '''Resets the robot time to zero''' self.time0 = datetime.now() self.time = 0.0 def updateClock(self): '''Updates the Robots clock to the current time''' timeDiff = datetime.now() - self.time0 self.time = timeDiff.seconds + timeDiff.microseconds/1e6 def readyPosition(self, persist = False): if persist: self.resetClock() while self.time < 2.0: self.commandPosition(POS_READY) sleep(.1) self.updateClock() else: self.commandPosition(POS_READY) sleep(2) def commandPosition(self, position, crop = True, cropWarning = False): '''Command the given position commandPosition will command the robot to move its servos to the given position vector. This vector is cropped to the physical limits of the robot and converted to integer Positional arguments: position -- A length 9 vector of desired positions. Keyword arguments: cropWarning -- Whether or not to print a warning if the positions are cropped. Default: False. ''' if len(position) != self.nServos: raise Exception('Expected postion vector of length %d, got %s instead' % (self.nServos, repr(position))) if crop: goalPosition = self.cropPosition([int(xx) for xx in position], cropWarning) else: goalPosition = [int(xx) for xx in position] if self.loud: posstr = ', '.join(['%4d' % xx for xx in goalPosition]) print '%.2fs -> %s' % (self.time, posstr) for ii,actuator in enumerate(self.actuators): actuator.goal_position = goalPosition[ii] self.net.synchronize() #[ac.read_all() for ac in self.actuators] #positions = ['%d: %s' % (ii,ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) for ii,ac in enumerate(self.actuators)] #print ' '.join(positions) #print ''.join(['x' if ac.led else ' ' for ac in self.actuators]) + ' ' ,#XXX:THIS IS THE SLOW PART!!! #print ' '.join(['%.1f' % ac.current_voltage for ac in self.actuators])#XXX:OR THIS return goalPosition def cropPosition(self, position, cropWarning = False): '''Crops the given positions to their appropriate min/max values. Requires a vector of length 9 to be sure the IDs are in the assumed order.''' if len(position) != self.nServos: raise Exception('cropPosition expects a vector of length %d' % self.nServos) ret = copy(position) for ii in [0, 2, 4, 6]: ret[ii] = max(MIN_INNER, min(MAX_INNER, ret[ii])) ret[ii+1] = max(MIN_OUTER, min(MAX_OUTER, ret[ii+1])) if cropWarning and ret != position: print 'Warning: cropped %s to %s' % (repr(position), repr(ret)) return ret def readCurrentPosition(self): ret = [] if len(self.actuators) != self.nServos: raise RobotFailure('Lost some servos, now we only have %d' % len(self.actuators)) for ac in self.actuators: #ac.read_all() #ret.append(ac.cache[dynamixel.defs.REGISTER['CurrentPosition']]) ret.append(ac.current_position) #sleep(.001) return ret def pingAll(self): failures = [] for ii in self.actuatorIds: result = self.net.ping(ii) if result is False: failures.append(ii) return failures def printStatus(self): pos = self.readCurrentPosition() print 'Positions:', ' '.join(['%d:%d' % (ii,pp) for ii,pp in enumerate(pos)]) def shimmy(self): '''Moves through a set of checks and makes sure the robot is still moving.''' self.commandPosition(POS_READY) sleep(.8) success = True success &= self.checkMove(POS_READY, POS_CHECK_1) success &= self.checkMove(POS_CHECK_1, POS_CHECK_2) success &= self.checkMove(POS_CHECK_2, POS_CHECK_3) success &= self.checkMove(POS_CHECK_3, POS_CHECK_2) success &= self.checkMove(POS_CHECK_2, POS_CHECK_1) success &= self.checkMove(POS_CHECK_1, POS_READY) return success def checkMove(self, aa, bb): aa = array(aa) bb = array(bb) self.commandPosition(aa) posAA = array(self.readCurrentPosition()) self.commandPosition(bb) sleep(.4) posBB = array(self.readCurrentPosition()) success = True success &= all( abs((aa-bb) - (posAA-posBB)) < 50) success &= all( abs(aa - posAA) < 50) success &= all( abs(bb - posBB) < 50) if not success and False: print 'shimmy errors' print (aa-bb) - (posAA-posBB) print aa - posAA print bb - posBB return success