@@ -0,0 +1,2835 @@
+#!/usr/bin/env python%0A%0Afrom CRS_commander import Commander%0Aimport argparse%0Afrom poly import circle_points, interpolate%0Aimport numpy as np%0A%0Aif __name__ == '__main__':%0A%0A    help_msg = 'SYNOPSIS: CRS_commander.py %5B-l /dev/ttyXXX%5D'%0A%0A    parser = argparse.ArgumentParser(description='CRS robot commander')%0A    parser.add_argument('-s', '--skip-setup', dest='skip_setup', action='store_true',%0A                        default=False, help='skip hard-home inicialization of robot')%0A    parser.add_argument('-d', '--tty-device', dest='tty_dev', type=str,%0A                        default='/dev/ttyUSB0', help='tty line/device to robot')%0A    parser.add_argument('-a', '--action', dest='action', type=str,%0A                        default='home', help='action to run')%0A    parser.add_argument('-m', '--max-speed', dest='max_speed', type=int,%0A                        default=None, help='maximal motion speed')%0A    parser.add_argument('-t', '--reg-type', dest='reg_type', type=int,%0A                        default=None, help='controller type selection')%0A    parser.add_argument('-p', '--target-position', dest='target_position', type=int,%0A                        default=None, help='target position')%0A%0A    parser.add_argument('-x', '--x-coord', dest='x0', type=int,%0A                        default=500, help='x coord. of circle center')%0A    parser.add_argument('-y', '--y-coord', dest='y0', type=int,%0A                        default=250, help='y coord. of circle center')%0A    parser.add_argument('-z', '--z-coord', dest='z0', type=int,%0A                        default=500, help='z coord. of circle center')%0A    parser.add_argument('-r', '--radius', dest='r', type=int,%0A                        default=50, help='radius of circle')%0A    parser.add_argument('-st', '--step', dest='step', type=int,%0A                        default=5, help='step of circle sampling in degs')%0A%0A    args = parser.parse_args()%0A%0A    tty_dev = args.tty_dev%0A    skip_setup = args.skip_setup%0A    max_speed = args.max_speed%0A    reg_type = args.reg_type%0A    action = 'graph' #args.action%0A    x0 = args.x0%0A    y0 = args.y0%0A    z0 = args.z0%0A    radius = args.r%0A    step = args.step%0A%0A    c = Commander()%0A    c.open_comm(tty_dev)%0A%0A    if not skip_setup or action == 'home':%0A        c.init(reg_type=reg_type, max_speed=max_speed)%0A%0A    if action == 'graph':%0A        sol = circle_points(c, x0, y0, z0, radius, step)%0A        interpolate(sol, graph=True)%0A%0A    if action == 'circle':%0A        c.circle(c, x0, y0, z0, radius)%0A%0A    if action == 'poly':%0A        sol = circle_points(c, x0, y0, z0, radius, step)%0A        interpolate(sol, graph=False)%0A%0A        params = np.load('param.npy')%0A        pos = %5Bx0, y0+radius, z0, 0, 0, 0%5D%0A        prev_a = c.move_to_pos(pos)%0A        c.wait_ready()%0A        for i in range(len(params)):%0A            c.splinemv( params%5Bi%5D, order=3)%0A        c.wait_ready()%0A%0A%0A%0A

Create testing script.