def __init__(self): """ """ super(Servos, self).__init__() self._servosPRU = ServosPRU()
class Servos(object): """ """ def __init__(self): """ """ super(Servos, self).__init__() self._servosPRU = ServosPRU() def pulse(self, channel=None, pulse=None): """ """ if pulse is None: if channel is None: pulses = self._servosPRU.getPulses() for channel in range(NB_SERVOS): print "channel %02d = %04dus" % (channel, pulses[channel]) else: print "channel %02d = %04dus" % (channel, self._servosPRU.getPulse(channel)) else: if not 0 <= channel <= 31: raise ValueError("channel must be in range [0-31]") if pulse != 0 and not 5 <= pulse <= 5000: raise ValueError("pulse must be in range [5-5000], or 0 to disable servo") self._servosPRU.setPulse(channel, pulse) def move(self, channel, pulse, delay): """ """ if not 0 <= channel <= 31: raise ValueError("channel must be in range [0-31]") if not 5 <= pulse <= 5000: raise ValueError("pulse must be in range [5-5000]") if not 20 <= delay <= 20000: raise ValueError("delay must be in range [20-20000]") self._servosPRU.moveTo({channel: pulse}, delay) def test(self, iterations, pause): """ """ random.seed() # Init random generator startTime = time.time() totalTime = 0 for i in range(iterations): delay = random.randint(20, 2000) # ms #delay = 2000 nbSteps = delay / 20 print "Move #%d (delay=%d, nbSteps=%d):" % (i, delay, nbSteps) # Generate random positions pulsesDict = {} currentPulses = self._servosPRU.getPulses() #for channel in random.sample(range(32), random.randint(1, 32)) #for channel in (30, 31) for channel in range(NB_SERVOS): pulseFrom = currentPulses[channel] pulseTo = random.randint(1, 5000) #pulseTo = random.randint(900, 2100) inc = (pulseTo - pulseFrom) / nbSteps print " channel %02d: %04d to %04d" % (channel, pulseFrom, pulseTo) pulsesDict[channel] = pulseTo self._servosPRU.moveTo(pulsesDict, delay) time.sleep(pause) totalTime += delay print "total=%.3f, real=%.3f" % (totalTime/1000, time.time()-startTime) def run(self): # Main parser parser = argparse.ArgumentParser(prog="channels.py", description="This tool is used to synchronise channels.", epilog="Under developement...") #commonParser = argparse.ArgumentParser(add_help=False) subparsers = parser.add_subparsers(title="subcommands", description="valid subcommands", help="sub-command help") # Pulse parser pulseParser = subparsers.add_parser("pulse", help="Immediate set/get pulse width") pulseParser.set_defaults(func=self.pulse) pulseParser.add_argument("channel", type=int, nargs='?', help="channel num [0-31] (omit to read all current pulses)") pulseParser.add_argument("pulse", type=int, nargs='?', help="pulse width pulse, in us [5-5000], or 0 to disable servo (omit to read current pulse)") # Move parser moveParser = subparsers.add_parser("move", help="move servo to pulse target in a given delay") moveParser.set_defaults(func=self.move) moveParser.add_argument("-d", "--delay", type=int, default=1000, help="delay for moving, in ms [20-20000] (defaut 1000)") moveParser.add_argument("channel", type=int, help="channel num [0-31]") moveParser.add_argument("pulse", type=int, help="pulse target to reach, in us [5-5000]") # Test parser testParser = subparsers.add_parser("test", help="test several moves of all channels") testParser.set_defaults(func=self.test) testParser.add_argument("-i", "--iterations", type=int, default=50, help="number of iterations (default 50)") testParser.add_argument("-p", "--pause", type=float, default=0, help="pause between moves, in s (default 1)") # Parse args args = parser.parse_args() options = dict(vars(args)) options.pop("func") args.func(**options)