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)
