Commits

Will Dickson  committed 6032ef1

Got command line interface working.
Modified configuration setup for the ff_ind2unit is read from the motor_maps file.
Add Mike's wing kinematics functions.

  • Participants
  • Parent commits 626323c

Comments (0)

Files changed (6)

File lib/sixaxff.c

             for (i=0; i<NUM_FF; i++) {
                 fflush_printf("p[%d]: %3.2f(%s), v[%d]: %3.2f(%s/s), ft[%d]: %s %3.2f(%s)",
                         i,
-                        status_copy.pos[i]*config.ff_ind2unit[i],
+                        status_copy.pos[i],
                         config.ff_axesunits[i],
                         i,
-                        status_copy.vel[i]*config.ff_ind2unit[i],
+                        status_copy.vel[i],
                         config.ff_axesunits[i],
                         i, 
                         FT_NAMES[config.ff_ft[i]],
     ///////////////////////////////////////////////////////////////////
 
 
-    // If we are inside start up wft_indow set the force feedback forces/torques to zero
+    // If we are inside start up window - set the force feedback forces/torques to zero
     if (t < (double)config.startup_t) {
         for (i=0; i<NUM_FF; i++) {
             ft_ind = config.ff_ft[i];

File python/examples/borfrc/ff_conf/ff.conf

 ff_basic_tooltrans = 0.0, 0.0, 0.0, 0.0, 0.0,-1.047197551 
 ff_dynam_tooltrans = 0,1,0
 ff_mass = 1.0, 1.0
-ff_ind2unit = 0.00024543692606170261, 1.0
 ff_axesunits =  deg, m
 ff_damping = 0.0, 0.0
 ff_flag =  FF_ON,FF_OFF

File python/examples/borfrc/motor_map/motor_map.conf

 [stroke_0]
 type = stepper
 number = 4 
-unit_per_ind = 360.0/6400.0
+unit_per_ind = 360.0/6400.0*DEG2RAD
 clk = 14
 dir = 15
 
 [stroke_1]
 type = stepper
 number = 5 
-unit_per_ind = -360.0/6400.0
+unit_per_ind = -360.0/6400.0*DEG2RAD
 clk = 16
 dir = 17
 
 [pitch]
 type = stepper
 number = 6 
-unit_per_ind = 360.0/(2.0*12800)
+unit_per_ind = 360.0/(2.0*12800)*DEG2RAD
 clk = 12
 dir = 13
 

File python/examples/sixaxff_test.py

 #!/usr/bin/env python
+import sys
 import scipy
 import pylab
 import libsixaxff
 import cPickle as pickle
 
 RAD2DEG = 180.0/scipy.pi
+DEG2RAD = scipy.pi/180.0
 
-run_param = {
-    'ff_mass' : (10.0, 1.0), 
-}
+N_flaps = 3
+f = 1.0/6.0
+T = N_flaps/f
+amp_stroke = 70.0*DEG2RAD
+amp_rotation = 45.0*DEG2RAD
 
+run_param = {}
 dev = libsixaxff.Sixaxff(run_param)
 
 if 0:
     dev.print_config()
-else:
-    kine = scipy.zeros((100000,dev.num_motors()))
-    t, pos, vel, ft = dev.run(kine)
+    sys.exit(0)
+    
 
-    # Convert positions and velocities to degrees
-    pos = RAD2DEG*pos
-    vel = RAD2DEG*vel
-    
-    # Print shapes for return arrays
-    print 't.shape = ', t.shape
-    print 'pos.shape = ', pos.shape
-    print 'vel.shape = ', vel.shape
-    print 'ft.shape = ', ft.shape
-    
-    # Plot returned values
-    pylab.figure(1)
-    pylab.subplot(211)
-    pylab.plot(t,pos[:,0])
-    pylab.ylabel('pos[:,0] (deg)')
-    pylab.subplot(212)
-    pylab.plot(t,pos[:,1])
-    pylab.ylabel('pos[:,1] (deg)')
-    
-    pylab.figure(2)
-    pylab.subplot(211)
-    pylab.plot(t,vel[:,0])
-    pylab.ylabel('vel[:,0] (deg/s)')
-    pylab.subplot(212)
-    pylab.plot(t,vel[:,1])
-    pylab.ylabel('vel[:,1] (deg/s)')
-    
-    pylab.figure(3)
-    for i in range(0,6):
-        pylab.subplot(2,3,i+1)
-        pylab.plot(t,ft[:,i])
-    
-    pylab.show()
+
+# Create time array and set control function
+t = dev.get_time_points(T)
+u = scipy.zeros(t.shape)
+
+# Set kinematics
+dev.set_meanstroke_offset_kine(t, f, u, amp_stroke, amp_rotation)
+
+# Run
+t, pos, vel, ft = dev.run()
+
+# Convert positions and velocities to degrees
+pos = RAD2DEG*pos
+vel = RAD2DEG*vel
+
+# Print shapes for return arrays
+print 't.shape = ', t.shape
+print 'pos.shape = ', pos.shape
+print 'vel.shape = ', vel.shape
+print 'ft.shape = ', ft.shape
+
+# Plot returned values
+pylab.figure(1)
+pylab.subplot(211)
+pylab.plot(t,pos[:,0])
+pylab.ylabel('pos[:,0] (deg)')
+pylab.subplot(212)
+pylab.plot(t,pos[:,1])
+pylab.ylabel('pos[:,1] (deg)')
+
+pylab.figure(2)
+pylab.subplot(211)
+pylab.plot(t,vel[:,0])
+pylab.ylabel('vel[:,0] (deg/s)')
+pylab.subplot(212)
+pylab.plot(t,vel[:,1])
+pylab.ylabel('vel[:,1] (deg/s)')
+
+pylab.figure(3)
+for i in range(0,6):
+    pylab.subplot(2,3,i+1)
+    pylab.plot(t,ft[:,i])
+
+pylab.show()

File python/libsixaxff/sixaxff_utils.py

 DFLT_SENSOR_CAL_DIR = os.path.join(BORFRC_DIR,'sensor_cal')
 DFLT_COMEDI_CONF_DIR = os.path.join(BORFRC_DIR, 'comedi_conf')
 DFLT_FF_CONF_DIR = os.path.join(BORFRC_DIR, 'ff_conf')
-DFLT_MOVE_VMAX = 10.0
-DFLT_MOVE_ACCEL = 40.0
-DFLT_MOVE_VMAX_IND = 100.0
+DFLT_MOVE_VMAX = 10.0*DEG2RAD
+DFLT_MOVE_ACCEL = 40.0*DEG2RAD
+DFLT_MOVE_VMAX_IND = 500.0
 DFLT_MOVE_ACCEL_IND = 400.0
 DFLT_MOVE_DT = 1.0/3000.0
 DFLT_PAUSE_T = 2.0
 
+DFLT_K_STROKE = 0.01
+DFLT_K_ROTATION = 1.5
+DFLT_ROTATION_OFFSET = 0.0
+
 class Sixaxff_Base(object):
     """
     Base class for Sixaxff 
 
             # Add force feedback configuration
             config.update(self.ff_conf)
+
+            # Get index to unit conversion for force feedback axes from motor maps 
+            ff_ind2unit = []
+            for n in config['ff_motor']:
+                motor_name = motor_num2name[n]
+                ff_ind2unit.append(1.0/self.motor_maps[motor_name]['unit_per_ind'])
+            config['ff_ind2unit'] = tuple(ff_ind2unit)
+
             
             # Finally, add run parameters
             config.update(self.run_params)
         motor_names = [name for num,name in motor_nums_and_names]
         return motor_names
 
+    def get_time_points(self, T):
+        """
+        Get array of time points from 0 to T with step dt.
+        """
+        config = self.create_config_dict()
+        dt = config['dt']
+        N = int(T/dt)
+        t = scipy.arange(0,N+1)*dt
+        return t
+
 
 class Sixaxff(Sixaxff_Base):
 
         self.kine = kine
         self.t = t
 
-#    ##############################################################################################
-#    # REPLACE WITH PITCH KINEMATICS FUNCITONS 
-#    ##############################################################################################
-#
-#    def set_combo_kine(self, t, f, u_diff_aoa, u_stroke_rot, u_stroke_tilt, u_asym_vel,
-#                       amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE,k_rotation=DFLT_K_ROTATION,
-#                       rotation_offset=DFLT_ROTATION_OFFSET):
-#
-#        # Create control inputs for wings 0 and 1
-#        u_diff_aoa_0, u_diff_aoa_1 = -u_diff_aoa, u_diff_aoa
-#        u_stroke_rot_0, u_stroke_rot_1 = -u_stroke_rot, u_stroke_rot
-#        u_stroke_tilt_0, u_stroke_tilt_1 = u_stroke_tilt, -u_stroke_tilt
-#        u_asym_vel_0, u_asym_vel_1 = 0.5 - u_asym_vel, 0.5 + u_asym_vel 
-#
-#        # Set up base kinematics
-#        ro_0, ro_1 = -rotation_offset, rotation_offset
-#        num_motors = self.num_motors()
-#        kine = scipy.zeros((t.shape[0],num_motors))
-#        kine_base_0 = scipy.zeros((t.shape[0],3))
-#        kine_base_1 = scipy.zeros((t.shape[0],3))
-#
-#        kine_base_0[:,0] = get_stroke_angle(t, f, amp_stroke, u_asym_vel_0, k_stroke)
-#        kine_base_1[:,0] = get_stroke_angle(t, f, amp_stroke, u_asym_vel_1, k_stroke)
-#        kine_base_0[:,1] = get_deviation_angle(t, f, u_stroke_tilt_0) 
-#        kine_base_1[:,1] = get_deviation_angle(t, f, u_stroke_tilt_1) 
-#        kine_base_0[:,2] = get_rotation_angle(t, f, amp_rotation, u_diff_aoa_0 + ro_0, k_rotation)
-#        kine_base_1[:,2] = get_rotation_angle(t, f, amp_rotation, u_diff_aoa_1 + ro_1, k_rotation)
-#
-#        # Rotate kinematics
-#        if type(u_stroke_rot) == float or type(u_stroke_rot) == int or type(u_stroke_rot) == scipy.float_:
-#            ax = scipy.array([1.0,0.0,0.0])
-#        else:
-#            ax = scipy.zeros((kine.shape[0],3))
-#            ax[:,0] = 1.0
-#        kine_base_0 = RAD2DEG*qarray.rotate_euler_array(DEG2RAD*kine_base_0, ax, DEG2RAD*u_stroke_rot_0)
-#        kine_base_1 = RAD2DEG*qarray.rotate_euler_array(DEG2RAD*kine_base_1, ax, DEG2RAD*u_stroke_rot_1)
-#
-#        s0 = self.get_motor_num('stroke_0')
-#        s1 = self.get_motor_num('stroke_1')
-#        r0 = self.get_motor_num('rotation_0')
-#        r1 = self.get_motor_num('rotation_1')
-#        d0 = self.get_motor_num('deviation_0')
-#        d1 = self.get_motor_num('deviation_1')
-#        
-#        kine[:,s0] = kine_base_0[:,0]
-#        kine[:,s1] = kine_base_1[:,0]
-#        kine[:,d0] = kine_base_0[:,1] 
-#        kine[:,d1] = kine_base_1[:,1] 
-#        kine[:,r0] = kine_base_0[:,2] 
-#        kine[:,r1] = kine_base_1[:,2] 
-#        self.kine_deg = kine
-#        self.t = t
-#
-#    def set_stroke_rot_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
-#                            k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
-#        """
-#        Sets current kinematics to those with differential rotation of the stroke
-#        plane. 
-#        """
-#        u_0, u_1 = -u, u
-#        ro_0, ro_1 = -rotation_offset, rotation_offset
-#
-#        # Set up base kinematics
-#        num_motors = self.num_motors()
-#        kine = scipy.zeros((t.shape[0],num_motors))
-#        kine_base_0 = scipy.zeros((t.shape[0],3))
-#        kine_base_1 = scipy.zeros((t.shape[0],3))
-#
-#        kine_base_0[:,0] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine_base_1[:,0] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine_base_0[:,1] = get_deviation_angle(t, f, 0.0) 
-#        kine_base_1[:,1] = get_deviation_angle(t, f, 0.0) 
-#        kine_base_0[:,2] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
-#        kine_base_1[:,2] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
-#
-#        # Rotate kinematics
-#        if type(u) == float or type(u) == int or type(u) == scipy.float_:
-#            ax = scipy.array([1.0,0.0,0.0])
-#        else:
-#            ax = scipy.zeros((kine.shape[0],3))
-#            ax[:,0] = 1.0
-#        kine_base_0 = RAD2DEG*qarray.rotate_euler_array(DEG2RAD*kine_base_0, ax, DEG2RAD*u_0)
-#        kine_base_1 = RAD2DEG*qarray.rotate_euler_array(DEG2RAD*kine_base_1, ax, DEG2RAD*u_1)
-#
-#        s0 = self.get_motor_num('stroke_0')
-#        s1 = self.get_motor_num('stroke_1')
-#        r0 = self.get_motor_num('rotation_0')
-#        r1 = self.get_motor_num('rotation_1')
-#        d0 = self.get_motor_num('deviation_0')
-#        d1 = self.get_motor_num('deviation_1')
-#        
-#        kine[:,s0] = kine_base_0[:,0]
-#        kine[:,s1] = kine_base_1[:,0]
-#        kine[:,d0] = kine_base_0[:,1] 
-#        kine[:,d1] = kine_base_1[:,1] 
-#        kine[:,r0] = kine_base_0[:,2] 
-#        kine[:,r1] = kine_base_1[:,2] 
-#        self.kine_deg = kine
-#        self.t = t
-#    
-#    def set_stroke_tilt_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
-#                             k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
-#        """
-#        Sets current kinemtics to those with differential tilt in stroke-plane angle determined by 
-#        control signal u.
-#        """
-#        u_0, u_1 = u, -u
-#        ro_0, ro_1 = -rotation_offset, rotation_offset
-#        num_motors = self.num_motors()
-#        kine = scipy.zeros((t.shape[0],num_motors))
-#        s0 = self.get_motor_num('stroke_0')
-#        s1 = self.get_motor_num('stroke_1')
-#        r0 = self.get_motor_num('rotation_0')
-#        r1 = self.get_motor_num('rotation_1')
-#        d0 = self.get_motor_num('deviation_0')
-#        d1 = self.get_motor_num('deviation_1')
-#        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
-#        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
-#        kine[:,d0] = get_deviation_angle(t, f, u_0) 
-#        kine[:,d1] = get_deviation_angle(t, f, u_1) 
-#        self.kine_deg = kine
-#        self.t = t
-#
-#    def set_diff_aoa_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
-#                          k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
-#        """
-#        Sets current kinematics to those with a differential angle of attach on the up stroke
-#        and downstroke. The amount of asymmetry is determined by the control input u.
-#        """
-#        u_0, u_1 = -u, u
-#        ro_0, ro_1 = -rotation_offset, rotation_offset
-#        num_motors = self.num_motors()
-#        kine = scipy.zeros((t.shape[0],num_motors))
-#        s0 = self.get_motor_num('stroke_0')
-#        s1 = self.get_motor_num('stroke_1')
-#        r0 = self.get_motor_num('rotation_0')
-#        r1 = self.get_motor_num('rotation_1')
-#        d0 = self.get_motor_num('deviation_0')
-#        d1 = self.get_motor_num('deviation_1')
-#        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5, k_stroke)
-#        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, u_0 + ro_0, k_rotation)
-#        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, u_1 + ro_1, k_rotation)
-#        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
-#        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
-#        self.kine_deg = kine
-#        self.t = t
-#
-#    def set_asym_stroke_vel_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE,
-#                                 k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
-#        """
-#        Sets current kinematics to those with asymmetric stroke velocity. The amount of velocity asymmetry
-#        is determined by the control input u.
-#        """
-#        u_0, u_1 = 0.5 - u, 0.5 + u 
-#        ro_0, ro_1 = -rotation_offset, rotation_offset
-#        num_motors = self.num_motors()
-#        kine = scipy.zeros((t.shape[0], num_motors))
-#        s0 = self.get_motor_num('stroke_0')
-#        s1 = self.get_motor_num('stroke_1')
-#        r0 = self.get_motor_num('rotation_0')
-#        r1 = self.get_motor_num('rotation_1')
-#        d0 = self.get_motor_num('deviation_0')
-#        d1 = self.get_motor_num('deviation_1')
-#        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, u_0, k_stroke)
-#        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, u_1, k_stroke)
-#        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
-#        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
-#        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
-#        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
-#        self.kine_deg = kine
-#        self.t = t
-#
-#    def set_yaw_to_const_vel(self,vel,accel,center=False):
-#        """
-#        Sets the kinematics of the yaw motor to constant velocity.
-#        """
-#        if self.t == None or self.kine_deg == None:
-#            raise RuntimeError, 'cannot set yaw to constant vel - no t or kine_deg'
-#        n = self.get_motor_num('yaw')
-#        self.kine_deg[:,n] = ramp_to_const_vel(self.t,vel,accel)
-#        if center == True:
-#            mid_pt = 0.5*(self.kine_deg[:,n].max() + self.kine_deg[:,n].min())
-#            self.kine_deg[:,n] = self.kine_deg[:,n] - mid_pt
-#
-#    def set_yaw_to_ramp(self,x0,x1,vmax,a,startup_pause=False,startup_t=10):
-#        """
-#        Sets kinematics of the yaw motor to a point to point ramp
-#        """
-#        num_motors = self.num_motors()
-#        n = self.get_motor_num('yaw')
-#        dt = self.config_dict['dt']
-#        ramp = libmove_motor.get_ramp(x0,x1,vmax,a,dt,output='ramp only')
-#        if startup_pause == True:
-#            n_pause = int(startup_t/dt)
-#            pause = ramp[0]*scipy.ones((n_pause,))
-#            ramp = scipy.concatenate((pause,ramp))
-#        if self.kine_deg == None:
-#            self.kine_deg = scipy.zeros((ramp.shape[0],num_motors))
-#            self.t = scipy.arange(0,ramp.shape[0])*dt
-#            self.kine_deg[:,n] = ramp
-#        else:
-#            raise RuntimeError, 'set_yaw_to_ramp w/ existing kinematics not implemented yet'
-#
-#    def set_yaw_to_const(self,val):
-#        """
-#        Set yaw kinematics to constant position
-#        """
-#        num_motors = self.num_motors()
-#        n = self.get_motor_num('yaw')
-#        self.kine_deg[:,n] = val*scipy.ones((self.kine_deg.shape[0],))
-#
-#    def set_cable_test_kine(self,yaw_range,yaw_step,T_meas,dt):
-#        """
-#        Set kinematics for testing for cable effects 
-#        """
-#        num_motors = self.num_motors()
-#        n = self.get_motor_num('yaw')
-#        t, yaw_kine, yaw_meas_pos = get_cable_test_kine(yaw_range,yaw_step,T_meas,dt)
-#        self.kine_deg = scipy.zeros((yaw_kine.shape[0],num_motors))
-#        self.kine_deg[:,n] = yaw_kine
-#        self.t = t
-#        return yaw_meas_pos
-#       
-#    def plot_kine(self,kine_deg=None):
-#        """
-#        Plot wing kinematics
-#        """
-#        if kine_deg == None:
-#            if self.kine_deg == None:
-#                raise RuntimeError, 'no kinematics to plot'
-#            else:
-#                kine_deg = self.kine_deg
-#
-#        dt = self.config_dict['dt']
-#        t = scipy.arange(0.0,kine_deg.shape[0])*dt
-#
-#        for motor, map in self.motor_maps.iteritems():
-#            ind = map['number']
-#            try:
-#                side_num = int(motor[-1])
-#                name = motor[:-2]
-#            except:
-#                side_num = None
-#                name = motor
-#
-#            # Select figure    
-#            if side_num == 0:
-#                pylab.figure(1)
-#            elif side_num == 1:
-#                pylab.figure(2)
-#            else:
-#                pylab.figure(3)
-#
-#            # Select subplot
-#            if name == 'stroke':
-#                pylab.subplot(3,1,1)
-#                pylab.title('side num: %d'%(side_num,))
-#                pylab.ylabel('stroke')
-#            elif name == 'rotation':
-#                pylab.subplot(3,1,2)
-#                pylab.ylabel('rotation')
-#            elif name == 'deviation':
-#                pylab.subplot(3,1,3)
-#                pylab.ylabel('deviation')
-#                pylab.ylabel('deviation')
-#            elif name == 'yaw':
-#                pylab.ylabel('yaw')
-#                pylab.xlabel('t')
-#            pylab.plot(t,kine_deg[:,ind])
-#        pylab.show()
-#
-#    ################################################################################
+    # Methods for setting wing kinematics ----------------------------------------------------
 
+    def set_base_kine(self, t, f, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to nominal.
+        """
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5, 0.0, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5, 0.0, k_stroke)
+        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
+        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+    def set_meanstroke_offset_kine(self, t, f, v, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to those with a mean stroke position offset
+        The amount of offset is determined by the control input v.
+        """
+        
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5, v, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5, v, k_stroke)
+        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
+        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+    def set_diff_velocity_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to those with a different velocity on the upstroke
+        and downstroke. The amount of offset is determined by the control input u.
+        """
+        
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5 + u, 0.0, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5 + u, 0.0, k_stroke)
+        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation)
+        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation)
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+    def set_diff_aoaD_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to those with a different angle of attack on the upstroke
+        and downstroke. The amount of offset is determined by the control input u.
+        """
+        
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, u + ro_0, k_rotation)
+        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, u + ro_1, k_rotation)
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+    def set_diff_aoaL_kine(self, t, f, u, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to those with a different angle of attack on the anterior
+        and posterior sections of the stroke. The amount of offset is determined by the control input u.
+        """
+        
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,r0] = (get_rotation_angle(t, f, (1-u)*amp_rotation, ro_0, k_rotation)+
+                     get_rotation_angle(t, 2*f,u*amp_rotation,ro_0, k_rotation))
+        kine[:,r1] = (get_rotation_angle(t, f, (1.-u)*amp_rotation, ro_0, k_rotation)+
+                     get_rotation_angle(t, 2*f,u*amp_rotation,ro_0, k_rotation))
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+    def set_dorsal_delay_kine(self, t, f, phase_delay, amp_stroke, amp_rotation, k_stroke=DFLT_K_STROKE, 
+                              k_rotation=DFLT_K_ROTATION, rotation_offset=DFLT_ROTATION_OFFSET):
+        """
+        Sets current kinematics to those with a different velocity on the upstroke
+        and downstroke. The amount of offset is determined by the control input u.
+        """
+        phase0 = scipy.zeros(t.shape)
+        T=scipy.fmod(t,1/f)
+        phase = (phase0 + phase_delay*((T>=.25/f)*(T<.75/f)-(1.0-10.0*f*(T-.25/f))*(T>=.25/f)*(T<.35/f)
+            -10.0*f*(T-.65/f)*(T>=.65/f)*(T<.75/f)))
+        ro_0, ro_1 = -rotation_offset, rotation_offset
+        num_motors = self.num_motors()
+        kine = scipy.zeros((t.shape[0],num_motors))
+        s0 = self.get_motor_num('stroke_0')
+        s1 = self.get_motor_num('stroke_1')
+        r0 = self.get_motor_num('rotation_0')
+        r1 = self.get_motor_num('rotation_1')
+        d0 = self.get_motor_num('deviation_0')
+        d1 = self.get_motor_num('deviation_1')
+        kine[:,s0] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,s1] = get_stroke_angle(t, f, amp_stroke, 0.5 , 0.0, k_stroke)
+        kine[:,r0] = get_rotation_angle(t, f, amp_rotation, ro_0, k_rotation, phase = phase)
+        kine[:,r1] = get_rotation_angle(t, f, amp_rotation, ro_1, k_rotation, phase = phase)
+        kine[:,d0] = get_deviation_angle(t, f, 0.0) 
+        kine[:,d1] = get_deviation_angle(t, f, 0.0) 
+        self.kine = kine
+        self.t = t
+
+
+# Configuration file functions -------------------------------------------------
 
 def read_defaults(defaults_file=BORFRC_DIR):
     """
             'ff_basic_tooltrans': 'flt_list', 
             'ff_dynam_tooltrans' : 'int_list', 
             'ff_mass' : 'flt_list', 
-            'ff_ind2unit': 'flt_list', 
             'ff_axesunits': 'str_list',
             'ff_damping': 'flt_list', 
             'ff_flag' : 'str_list',
 
 # Wing kinematics functions ------------------------------------------------------
 
+def get_stroke_angle(t, f, amp, u, v, k, phase=0.5*PI):
+    """
+    Stroke position function w/ J. Wang's shape parameter. The control input 
+    u adds a asymmetric velicty to the stroke positon fuction, while the control
+    input v adds changes the mean stroke position.
+
+    Arguments:
+        t     = array of time points (s)
+        f     = flapping frequency (Hz)
+        amp   = stroke amplitude 
+        u     = control input, scalar or array
+        v     = control input, s
+        k     = shape parameter near 0 => sine wave, near 1=> sawtooth
+    """
+    f1=f/(2*u)
+    f2=1/(2/f-1/f1)
+    phi_A=amp/scipy.arcsin(k)*scipy.arcsin(k*scipy.sin(2*PI*f1*scipy.fmod(t, 1/f)+phase))
+    phi_B=amp/scipy.arcsin(k)*scipy.arcsin(k*scipy.sin(2*PI*f2*(1/(2*f2)-1/(2*f1)+scipy.fmod(t,1/f))+phase))
+    phi=phi_A*(scipy.fmod(t, 1/f)<1/(2*f1))+phi_B*(scipy.fmod(t, 1/f)>=1/(2*f1))+v
+    return phi
+
+def get_rotation_angle(t, f, amp, u, k, phase=0.0):
+    """
+    Rotation angle function w/ J. Wang's shape parameter. The control input shifts
+    the mean rotation angle.
+
+    Arguments:
+        t     = array of time points 
+        f     = flapping frequency
+        amp   = rotation angle amplitude
+        u     = control input, scalar or array
+        k     = shape parameter, k near 0 implies sinewave, k large implies
+                square wave.
+    """
+    
+    alpha=amp/scipy.tanh(k)*scipy.tanh(k*scipy.sin(2*PI*f*t+phase))+u
+    return alpha
+
+def get_deviation_angle(t, f, u, phase=0.0):
+    """
+    Deviation angle function w/ control input u.
+
+    Arguments:
+        t     = array of time points
+        f     = flapping frequency
+        amp   = deviation angle amplitude
+        u     = control input, scalar or array
+    """
+    phase = 0.0
+    theta = u*scipy.cos(2*PI*f*t + phase)
+    return theta
+
+# Kinematics/control utility functions -----------------------------------------
+
 def control_step(t,u0,u1,t0,t1,t2,t3):
     """
     Control function, has value u0 for t < t0, linearly transitions from u0 
     return data_dict_new
 
 
-## ------------------------------------------------------------------------------
-## Command line utilties
-#
-#class sixaxff_cmd_line:
-#
-#    zero_help = """\
-#command: zero 
-#
-#usage: %prog [options] zero 
-#
-#
-#Zeroing routine for pitch and stroke position motors. Run and follow the
-#instructions.
-#"""
-#
-#    move2pos_help = """\
-#command: move2pos 
-#
-#usage: %prog [options] move2pos motor_0, ..., motor_k, pos
-#
-#       or
-#
-#       %prog [options] move2pos motor_0, ..., motor_k, pos_0, ... pos_k
-#
-#Move motors, specified by name, to the given positions specified in degrees. If
-#one position value is given then all motors specified are moved to that position. 
-#Otherwise the number of positions specified must equal the number of motors and 
-#each motor is moved to the corresponding position which is determined by the order.  
-#By default, after the move, the routine will prompt the user press enter and then 
-#return  the motors to the zero position unless the noreturn option is specified.  
-#"""
-#
-#    move2zero_help = """\
-#command: move2zero 
-#
-#usage: %prog [options] move2zero 
-#
-#Move motors to zero (degree) position. By default, after the move, the routine
-#will prompt the user press enter and then return  the motors to the zero
-#position unless the noreturn option is specified.  
-#"""
-#
-#    move_by_ind_help = """\
-#command: move-by-ind 
-#
-#usage: %prog [options] move-by-ind motor_0, ..., motor_k, pos
-# 
-#       or
-#
-#       %prog [options] move-by-ind motor_0, ..., motor_k, pos_0, ..., pos_k
-#
-#Move motors, specified by name, by the given number of indices. If one value 
-#is given then all motors specified are moved by that number of indices. Otherwise 
-#the number of values specified must equal the number of motors and each motor
-#is moved by the corresponding value which is determined by the order. By 
-#default, after the move, the routine will prompt the user press enter and then 
-#return  the motors to the zero position unless the noreturn option is specified.
-#"""
-#
-#    reset_pwm_help = """\
-#command: reset-pwm
-#
-#usage: %prog [options] reset-pwm 
-#
-#Resets all pwm signal to their default (start-up) positions.
-#"""
-#
-#    motor_names_help = """\
-#command: motor-names
-#
-#usage: %prog [options] motor-names
-#
-#Displays a list of all motors names.
-#"""
-#
-#    sensor_cal_help = """\
-#command: sensor-cal
-#    
-#This command has not been implemented yet.
-#"""
-#
-#    help_help = """\
-#command: help
-#
-#usage: %prog [options] help [COMMAND]
-#
-#Prints help information. If the optional argument COMMAND is not given
-#then general usage information for the %prog is displayed. If a specific 
-#command, COMMAND, is given then help for that command will be displayed.
-#
-#Examples:
-# %prog help         # prints general usage information
-# %prog help status  # prints help for the status command 
-#"""
-#
-#    usage = """%prog [OPTION] command [arg0, ...] 
-#
-#%prog is a command line utility providing simple positioning and other useful 
-#commands for working with the sixaxff controlled robot. This utitily is intended 
-#for aid the experimenter with zeroing and calibration.
-#
-#Commands:
-#
-#    zero          - zero yaw and stroke position motors
-#    move2pos      - move motors to specified positions (in degrees)
-#    move2zero     - move motors to zero position (in degrees)
-#    move-by-ind   - move motor by specified number of indices 
-#    reset-pwm     - reset pwm output to their default positions
-#    motor-names   - list motor names
-#    sensor-cal    - calibrate torque sensor
-#    help          - get help  
-#     
-#* To get help for a specific command type: %prog help COMMAND
-#"""
-#
-#    def __init__(self):
-#        self.cmd_table = {
-#            'zero' : self.zero,
-#            'move2pos': self.move2pos,
-#            'move2zero' : self.move2zero,
-#            'move-by-ind': self.move_by_ind,
-#            'reset-pwm': self.reset_pwm, 
-#            'motor-names': self.motor_names,
-#            'sensor-cal': self.sensor_cal,
-#            'help': self.help,
-#        }
-#        self.help_table = {
-#            'zero' : sixaxff_cmd_line.zero_help,
-#            'move2pos': sixaxff_cmd_line.move2pos_help,
-#            'move2zero': sixaxff_cmd_line.move2zero_help,
-#            'move-by-ind': sixaxff_cmd_line.move_by_ind_help,
-#            'reset-pwm': sixaxff_cmd_line.reset_pwm_help,
-#            'motor-names': sixaxff_cmd_line.motor_names_help,
-#            'sensor-cal': sixaxff_cmd_line.sensor_cal_help,
-#            'help': sixaxff_cmd_line.help_help,
-#        }
-#
-#        self.progname = os.path.split(sys.argv[0])[1]
-#        self.options_cmd, self.args, self.parser = self.parse_cmd_options()
-#        
-#        self.run_params = {
-#                'dt'                : 1.0/5000.0, 
-#                'yaw_inertia'       : 3.22,
-#                'yaw_damping'       : 0.0,
-#                'yaw_torq_lim'      : 0.5,
-#                'yaw_torq_deadband' : 1.5,
-#                'yaw_filt_lpcut'    : 3.0,
-#                'yaw_filt_hpcut'    : 0.0,
-#                'yaw_ain_zero_dt'   : 0.01,
-#                'yaw_ain_zero_num'  : 500, 
-#                'integ_type'        : libsixaxff.INTEG_RKUTTA,
-#                'startup_t'         : 0.0,
-#                'ff_flag'           : libsixaxff.FF_ON,
-#        }
-#        
-#
-#    def parse_cmd_options(self):
-#        """
-#        Parse command line options 
-#        """
-#
-#        parser = optparse.OptionParser(usage=sixaxff_cmd_line.usage)
-#
-#        parser.add_option('-v', '--verbose',
-#                               action='store_true',
-#                               dest = 'verbose',
-#                               help = 'verbose mode - print additional information',
-#                               default = False)
-#
-#        parser.add_option('-n', '--noreturn',
-#                               action='store_true',
-#                               dest = 'noreturn',
-#                               help = 'noreturn mode - do not return to starting positin after making move',
-#                               default = False)
-#
-#        options, args = parser.parse_args()
-#
-#        # Convert options to dictionary
-#        options = options.__dict__
-#        return options, args, parser
-#
-#    def run(self):
-#        """
-#        Run command given on the command line
-#        """
-#        if len(self.args) == 0:
-#            print "ERROR: no command given"
-#            print 
-#            self.parser.print_help()
-#            sys.exit(0)
-#        else:
-#            cmd_str = self.args[0]
-#            try:
-#                cmd = self.cmd_table[cmd_str]
-#            except KeyError:
-#                print "ERROR: command, '%s', not found"%(cmd_str,)
-#                print 
-#                self.parser.print_help()
-#                sys.exit(1)
-#            # Run command
-#            cmd()
-#        return
-#
-#    def help(self):
-#        """
-#        Print help messages
-#        """
-#        if len(self.args)==1:
-#            self.parser.print_help()
-#        elif len(self.args)==2:
-#            cmd_str = self.args[1].lower()
-#            try:
-#                help_str = self.help_table[cmd_str]
-#            except KeyError:
-#                print "ERROR: can't get help unkown command"
-#                sys.exit(1)
-#            print help_str.replace('%prog', self.progname)
-#        else:
-#            print "ERROR: too many arguments for command help"
-#            sys.exit(1)
-#
-#    def move2pos(self):
-#        self.args.remove('move2pos')
-#        motor_names, values = self.get_motors_and_value_from_args()
-#        if self.options_cmd['verbose'] == True:
-#            if len(motor_names) == len(values):
-#                print 'moving motors %s to positions %s'%(motor_names,values)
-#            else:
-#                print 'moving motors %s to position %s'%(motor_names,values)
-#        sixaxff = Yawff(self.run_params)
-#        sixaxff.move_to_pos(motor_names, values, noreturn = self.options_cmd['noreturn'])
-#
-#    def move2zero(self):
-#        self.args.remove('move2zero')
-#        if self.options_cmd['verbose'] == True:
-#            if len(motor_names) == len(values):
-#                print 'moving motors %s to positions %s'%(motor_names,values)
-#            else:
-#                print 'moving motors %s to position %s'%(motor_names,values)
-#        sixaxff = Yawff(self.run_params)
-#        motor_names = sixaxff.get_motor_names()
-#        pos = scipy.zeros((len(motor_names),))
-#        sixaxff.move_to_pos(motor_names, pos, noreturn = self.options_cmd['noreturn'])
-#
-#    def zero(self):
-#
-#        print 
-#        print 'zeroing yaw and stroke position motors'
-#        print '-'*60
-#        print 
-#
-#        print '** Step 1: adjust yaw motor until system is squared with tank'
-#        print 
-#        print 'At the prompt enter the angle adjustments to yaw in degrees.'
-#        print "Enter 'done' when system is squared."
-#
-#        sixaxff = Yawff(self.run_params)
-#        motor_names = sixaxff.get_motor_names()
-#        pos = scipy.zeros((len(motor_names),))
-#        sixaxff.move_to_pos(motor_names, pos,noreturn=True,at_zero_ind=True)
-#        self.zero_adj_loop(sixaxff,'yaw')
-#
-#        print
-#        print 'Rotating system by 90 degrees'
-#        pos = scipy.zeros((len(motor_names),))
-#        pos[sixaxff.get_motor_num('yaw')] = 90.0
-#        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
-#        print 
-#
-#        print '** Step 2: adjust position of stroke_0 until squared with tank'
-#        print
-#        print 'at prompt enter the position adjustments in motor indices'
-#        print "enter 'done' when wing is square"
-#        self.zero_adj_loop(sixaxff, 'stroke_0')
-#
-#        print 
-#        print 'rotating system to -180 degrees'
-#        pos = scipy.zeros((len(motor_names),))
-#        pos[sixaxff.get_motor_num('yaw')] = -180.0 
-#        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
-#        print 
-#
-#        print '** Step 3: adjust position of stroke_1 until squared with tank'
-#        print 
-#        print 'at prompt enter position adjustments in indices'
-#        print "enter 'done' when wing is square"
-#        self.zero_adj_loop(sixaxff,'stroke_1')
-#
-#        print 
-#        print 'returning to zero position'
-#        pos = scipy.zeros((len(motor_names),))
-#        pos[sixaxff.get_motor_num('yaw')] = 90.0 
-#        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
-#
-#        print 'returning RC motors to zero index position'
-#        pos = libmove_motor.get_zero_indpos_deg(sixaxff.motor_maps)
-#        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
-#
-#    def zero_adj_loop(self, sixaxff, motor_name):
-#        """
-#        Yaw and stroke position zeroing routine adjustment loop
-#        """
-#        motor_names = sixaxff.get_motor_names()
-#        pos = scipy.zeros((len(motor_names),))
-#
-#        done = False
-#        while not done:
-#
-#            print
-#            ans = raw_input('adj> ')
-#            if ans.lower() == 'done':
-#                done = True
-#                continue
-#
-#            try:
-#                val = float(ans)
-#            except ValueError:
-#                print 'unable to convert entry to float - please try again'
-#                continue
-#
-#            print val
-#            pos[sixaxff.get_motor_num(motor_name)] = val
-#            sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
-#
-#        return 
-#
-#
-#
-#    def move_by_ind(self):
-#        self.args.remove('move-by-ind')
-#        motor_names, values = self.get_motors_and_value_from_args()
-#        if self.options_cmd['verbose'] == True:
-#            print 'moving motors %s by indices %s'%(motor_names,values)
-#        sixaxff = Yawff(self.run_params)
-#        sixaxff.move_by_ind(motor_names, values, noreturn = self.options_cmd['noreturn'])
-#
-#    def reset_pwm(self):
-#        """
-#        Reset pwm outputs to default values.
-#        """
-#        if self.options_cmd['verbose'] == True:
-#            print 'resetting pwms to default values' 
-#        clkdirpwm.set_pwm_to_default(None)
-#
-#    def motor_names(self):
-#        motor_names = self.get_motor_names()
-#        for name in motor_names:
-#            print ' ', name
-#
-#    def sensor_cal(self):
-#        print  'sensor_cal - not implemented yet'
-#
-#    def get_motor_names(self):
-#        sixaxff = Yawff(self.run_params)
-#        motor_names = sixaxff.motor_maps.keys()
-#        motor_names.sort()
-#        return motor_names
-#
-#    def get_motors_and_value_from_args(self):
-#        motor_names = self.get_motor_names()
-#        motors_in_args = [x for x in self.args if x in motor_names]
-#        others_in_args = [x for x in self.args if x not in motor_names]
-#        if len(others_in_args) == 1:
-#            v_str = others_in_args[0]
-#            v = self.get_float_value(v_str)
-#            values = [v]
-#        elif len(others_in_args) == len(motors_in_args):
-#            values = []
-#            for v_str in others_in_args:
-#                v = self.get_float_value(v_str)
-#                values.append(v)
-#        else:
-#            print 'ERROR: incorrect number of angle values -  must equal 1 or number of motor names given' 
-#            sys.exit(1)
-#        return motors_in_args, values
-#
-#    def get_float_value(self,v_str):
-#        if v_str[0] == 'n':
-#            v_str = v_str[1:]
-#            sign = -1
-#        else:
-#            sign = 1
-#        try:
-#            v = sign*float(v_str)
-#        except ValueError:
-#            print 'ERROR: cannot cast value to float'
-#            sys.exit(1)
-#        return v
-#
-#
-#def cmd_line_main():
-#    """
-#    Command line interface entry point.
-#    """
-#    cmd_line = sixaxff_cmd_line()
-#    cmd_line.run()
-#    pass
+# ------------------------------------------------------------------------------
+# Command line utilties
 
+class sixaxff_cmd_line:
 
-if __name__ == "__main__":
+    zero_help = """\
+command: zero 
 
-    dev = Sixaxff()
-    if 0:
-        dev.print_config()
-    else:
-        kine = scipy.zeros((1000,dev.num_motors()))
-        dev.run(kine)
+usage: %prog [options] zero 
 
 
+Zeroing routine for pitch and stroke position motors. Run and follow the
+instructions.
+"""
 
+    move2pos_help = """\
+command: move2pos 
+
+usage: %prog [options] move2pos motor_0, ..., motor_k, pos
+
+       or
+
+       %prog [options] move2pos motor_0, ..., motor_k, pos_0, ... pos_k
+
+Move motors, specified by name, to the given positions specified in degrees. If
+one position value is given then all motors specified are moved to that position. 
+Otherwise the number of positions specified must equal the number of motors and 
+each motor is moved to the corresponding position which is determined by the order.  
+By default, after the move, the routine will prompt the user press enter and then 
+return  the motors to the zero position unless the noreturn option is specified.  
+"""
+
+    move2zero_help = """\
+command: move2zero 
+
+usage: %prog [options] move2zero 
+
+Move motors to zero (degree) position. By default, after the move, the routine
+will prompt the user press enter and then return  the motors to the zero
+position unless the noreturn option is specified.  
+"""
+
+    move_by_ind_help = """\
+command: move-by-ind 
+
+usage: %prog [options] move-by-ind motor_0, ..., motor_k, pos
+ 
+       or
+
+       %prog [options] move-by-ind motor_0, ..., motor_k, pos_0, ..., pos_k
+
+Move motors, specified by name, by the given number of indices. If one value 
+is given then all motors specified are moved by that number of indices. Otherwise 
+the number of values specified must equal the number of motors and each motor
+is moved by the corresponding value which is determined by the order. By 
+default, after the move, the routine will prompt the user press enter and then 
+return  the motors to the zero position unless the noreturn option is specified.
+"""
+
+    reset_pwm_help = """\
+command: reset-pwm
+
+usage: %prog [options] reset-pwm 
+
+Resets all pwm signal to their default (start-up) positions.
+"""
+
+    motor_names_help = """\
+command: motor-names
+
+usage: %prog [options] motor-names
+
+Displays a list of all motors names.
+"""
+
+    help_help = """\
+command: help
+
+usage: %prog [options] help [COMMAND]
+
+Prints help information. If the optional argument COMMAND is not given
+then general usage information for the %prog is displayed. If a specific 
+command, COMMAND, is given then help for that command will be displayed.
+
+Examples:
+ %prog help         # prints general usage information
+ %prog help status  # prints help for the status command 
+"""
+
+    usage = """%prog [OPTION] command [arg0, ...] 
+
+%prog is a command line utility providing simple positioning and other useful 
+commands for working with the sixaxff controlled robot. This utitily is intended 
+for aid the experimenter with zeroing and calibration.
+
+Commands:
+
+    zero          - zero yaw and stroke position motors
+    move2pos      - move motors to specified positions (in degrees)
+    move2zero     - move motors to zero position (in degrees)
+    move-by-ind   - move motor by specified number of indices 
+    reset-pwm     - reset pwm output to their default positions
+    motor-names   - list motor names
+    help          - get help  
+     
+* To get help for a specific command type: %prog help COMMAND
+"""
+
+    def __init__(self):
+        self.cmd_table = {
+            'zero' : self.zero,
+            'move2pos': self.move2pos,
+            'move2zero' : self.move2zero,
+            'move-by-ind': self.move_by_ind,
+            'reset-pwm': self.reset_pwm, 
+            'motor-names': self.motor_names,
+            'help': self.help,
+        }
+        self.help_table = {
+            'zero' : sixaxff_cmd_line.zero_help,
+            'move2pos': sixaxff_cmd_line.move2pos_help,
+            'move2zero': sixaxff_cmd_line.move2zero_help,
+            'move-by-ind': sixaxff_cmd_line.move_by_ind_help,
+            'reset-pwm': sixaxff_cmd_line.reset_pwm_help,
+            'motor-names': sixaxff_cmd_line.motor_names_help,
+            'help': sixaxff_cmd_line.help_help,
+        }
+
+        self.run_params = {}
+        self.progname = os.path.split(sys.argv[0])[1]
+        self.options_cmd, self.args, self.parser = self.parse_cmd_options()
+        
+
+    def parse_cmd_options(self):
+        """
+        Parse command line options 
+        """
+
+        parser = optparse.OptionParser(usage=sixaxff_cmd_line.usage)
+
+        parser.add_option('-v', '--verbose',
+                               action='store_true',
+                               dest = 'verbose',
+                               help = 'verbose mode - print additional information',
+                               default = False)
+
+        parser.add_option('-n', '--noreturn',
+                               action='store_true',
+                               dest = 'noreturn',
+                               help = 'noreturn mode - do not return to starting positin after making move',
+                               default = False)
+
+        options, args = parser.parse_args()
+
+        # Convert options to dictionary
+        options = options.__dict__
+        return options, args, parser
+
+    def run(self):
+        """
+        Run command given on the command line
+        """
+        if len(self.args) == 0:
+            print "ERROR: no command given"
+            print 
+            self.parser.print_help()
+            sys.exit(0)
+        else:
+            cmd_str = self.args[0]
+            try:
+                cmd = self.cmd_table[cmd_str]
+            except KeyError:
+                print "ERROR: command, '%s', not found"%(cmd_str,)
+                print 
+                self.parser.print_help()
+                sys.exit(1)
+            # Run command
+            cmd()
+        return
+
+    def help(self):
+        """
+        Print help messages
+        """
+        if len(self.args)==1:
+            self.parser.print_help()
+        elif len(self.args)==2:
+            cmd_str = self.args[1].lower()
+            try:
+                help_str = self.help_table[cmd_str]
+            except KeyError:
+                print "ERROR: can't get help unkown command"
+                sys.exit(1)
+            print help_str.replace('%prog', self.progname)
+        else:
+            print "ERROR: too many arguments for command help"
+            sys.exit(1)
+
+    def move2pos(self):
+        self.args.remove('move2pos')
+        motor_names, values = self.get_motors_and_value_from_args()
+        if self.options_cmd['verbose'] == True:
+            if len(motor_names) == len(values):
+                print 'moving motors %s to positions %s'%(motor_names,values)
+            else:
+                print 'moving motors %s to position %s'%(motor_names,values)
+        sixaxff = Sixaxff(self.run_params)
+        sixaxff.move_to_pos(motor_names, values, noreturn = self.options_cmd['noreturn'])
+
+    def move2zero(self):
+        self.args.remove('move2zero')
+        if self.options_cmd['verbose'] == True:
+            if len(motor_names) == len(values):
+                print 'moving motors %s to positions %s'%(motor_names,values)
+            else:
+                print 'moving motors %s to position %s'%(motor_names,values)
+        sixaxff = Sixaxff(self.run_params)
+        motor_names = sixaxff.get_motor_names()
+        pos = scipy.zeros((len(motor_names),))
+        sixaxff.move_to_pos(motor_names, pos, noreturn = self.options_cmd['noreturn'])
+
+    def zero(self):
+
+        print 
+        print 'zeroing yaw and stroke position motors'
+        print '-'*60
+        print 
+
+        print '** Step 1: adjust yaw motor until system is squared with tank'
+        print 
+        print 'At the prompt enter the angle adjustments to yaw in degrees.'
+        print "Enter 'done' when system is squared."
+
+        sixaxff = Sixaxff(self.run_params)
+        motor_names = sixaxff.get_motor_names()
+        pos = scipy.zeros((len(motor_names),))
+        sixaxff.move_to_pos(motor_names, pos,noreturn=True,at_zero_ind=True)
+        self.zero_adj_loop(sixaxff,'yaw')
+
+        print
+        print 'Rotating system by 90 degrees'
+        pos = scipy.zeros((len(motor_names),))
+        pos[sixaxff.get_motor_num('yaw')] = 90.0
+        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
+        print 
+
+        print '** Step 2: adjust position of stroke_0 until squared with tank'
+        print
+        print 'at prompt enter the position adjustments in motor indices'
+        print "enter 'done' when wing is square"
+        self.zero_adj_loop(sixaxff, 'stroke_0')
+
+        print 
+        print 'rotating system to -180 degrees'
+        pos = scipy.zeros((len(motor_names),))
+        pos[sixaxff.get_motor_num('yaw')] = -180.0 
+        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
+        print 
+
+        print '** Step 3: adjust position of stroke_1 until squared with tank'
+        print 
+        print 'at prompt enter position adjustments in indices'
+        print "enter 'done' when wing is square"
+        self.zero_adj_loop(sixaxff,'stroke_1')
+
+        print 
+        print 'returning to zero position'
+        pos = scipy.zeros((len(motor_names),))
+        pos[sixaxff.get_motor_num('yaw')] = 90.0 
+        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
+
+        print 'returning RC motors to zero index position'
+        pos = libmove_motor.get_zero_indpos_deg(sixaxff.motor_maps)
+        sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
+
+    def zero_adj_loop(self, sixaxff, motor_name):
+        """
+        Pitch and stroke position zeroing routine adjustment loop
+        """
+        motor_names = sixaxff.get_motor_names()
+        pos = scipy.zeros((len(motor_names),))
+
+        done = False
+        while not done:
+
+            print
+            ans = raw_input('adj> ')
+            if ans.lower() == 'done':
+                done = True
+                continue
+
+            try:
+                val = float(ans)
+            except ValueError:
+                print 'unable to convert entry to float - please try again'
+                continue
+
+            print val
+            pos[sixaxff.get_motor_num(motor_name)] = val
+            sixaxff.move_to_pos(motor_names,pos,noreturn=True,at_zero_ind=False)
+
+        return 
+
+    def move_by_ind(self):
+        self.args.remove('move-by-ind')
+        motor_names, values = self.get_motors_and_value_from_args()
+        if self.options_cmd['verbose'] == True:
+            print 'moving motors %s by indices %s'%(motor_names,values)
+        sixaxff = Sixaxff(self.run_params)
+        sixaxff.move_by_ind(motor_names, values, noreturn = self.options_cmd['noreturn'])
+
+    def reset_pwm(self):
+        """
+        Reset pwm outputs to default values.
+        """
+        if self.options_cmd['verbose'] == True:
+            print 'resetting pwms to default values' 
+        clkdirpwm.set_pwm_to_default(None)
+
+    def motor_names(self):
+        motor_names = self.get_motor_names()
+        for name in motor_names:
+            print ' ', name
+
+    def get_motor_names(self):
+        sixaxff = Sixaxff(self.run_params)
+        motor_names = sixaxff.motor_maps.keys()
+        motor_names.sort()
+        return motor_names
+
+    def get_motors_and_value_from_args(self):
+        motor_names = self.get_motor_names()
+        motors_in_args = [x for x in self.args if x in motor_names]
+        others_in_args = [x for x in self.args if x not in motor_names]
+        if len(others_in_args) == 1:
+            v_str = others_in_args[0]
+            v = DEG2RAD*self.get_float_value(v_str)
+            values = [v]
+        elif len(others_in_args) == len(motors_in_args):
+            values = []
+            for v_str in others_in_args:
+                v = DEG2RAD*self.get_float_value(v_str)
+                values.append(v)
+        else:
+            print 'ERROR: incorrect number of angle values -  must equal 1 or number of motor names given' 
+            sys.exit(1)
+        return motors_in_args, values
+
+    def get_float_value(self,v_str):
+        if v_str[0] == 'n':
+            v_str = v_str[1:]
+            sign = -1
+        else:
+            sign = 1
+        try:
+            v = sign*float(v_str)
+        except ValueError:
+            print 'ERROR: cannot cast value to float'
+            sys.exit(1)
+        return v
+
+
+def cmd_line_main():
+    """
+    Command line interface entry point.
+    """
+    cmd_line = sixaxff_cmd_line()
+    cmd_line.run()
+

File python/notes.txt

+1.0 Either save current position on exit or make sure system always returns to
+zero position regardless of how program is terminated or both. 
+
+2.0 Add home-ing routines to libmove-motor similar. 
+
+3.0 Add home-ing routines to Sixaxff class and command line.
+
+4.0 Add routine for finding axis of rotation.