def main(args): # move to starting position rl = RobotLibrary(args.robot, joystick=False) rl.move(args.x, args.y, 0.0) # configure twister = Twister(args.robot, rci=rl._rci) twister.dof = 'Rz' # most stable, due to each wheel contributing the same twister.speed = args.vRz twister.repeats = args.numRepeats twister.duration = args.duration twister.disableVisionLoc = True # the point of this procedure is to compare vision with encoders twister.accelerationLimit = 2.0 # must prevent wheel slip # run twister.run()
class MotionRangeTwister(): def __init__(self, args): # setup control and state reader self.rl = RobotLibrary(args.robot, joystick=False) # get ball and move to starting position self.homePosition = (args.x, args.y, 0.0) self.reset() # other settings self.dof = args.dof self.v = None self.accRange = None self.setRange() # sensible defaults for v and accRange # setup twister, reuse connected RCI self.twister = Twister(args.robot, rci=self.rl._rci) self.twister.dof = self.dof self.twister.duration = 1.0 self.twister.sleep = 0.5 self.twister.repeats = args.numRepeats self.twister.disableVisionLoc = True def setRange(self): # default acceleration range and speed setpoint self.v = 0.5 step = 0.2 if (self.dof == 'Rz'): self.v = 2.0 step = 0.5 self.accRange = [step * n for n in range(6,20)] # bh_verification self.accRange = [step*2 * n for n in range(10,20)] # motion_verification def reset(self): self.rl.move(*self.homePosition) def info(self, msg): # write to RDL eventlog os.system('frun diagnostics sendEvent INFO "' + msg + '"') # write to stdout print(msg, end='') sys.stdout.flush() # TODO remove, can use print() argument flush=True when migrating to python3 def run(self): # run for a in self.accRange: self.reset() isRz = (self.dof == 'Rz') self.info("testing {0} a={1:.2f}{2}2 at v={3:.2f}{2}...".format(self.dof, a, ['m/s', 'rad/s'][isRz], self.v)) self.twister.speed = self.v self.twister.accelerationLimit = a self.twister.run()