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()