Commits

Anonymous committed 65a464c

Added trimming bias to controller. Added new controller type CLTR_TYPE_KBIAS
which implements velocity control w/ zero setpt + time varying wing kinematics
bias. Added adjustable time delay to controller state estimates.

  • Participants
  • Parent commits 68fd7d4

Comments (0)

Files changed (11)

     if (config.ctlr_param.type == CTLR_TYPE_POS) {
       test = TRUE;
     }
+    if (config.ctlr_param.type == CTLR_TYPE_KBIAS) {
+      test = TRUE;
+    }
     if (test == FALSE) {
       PRINT_ERR_MSG("unknown controller type");
       flag = FAIL;
     }
+    // Check controll delay > 0
+    if (config.ctlr_param.delay < 0) {
+      PRINT_ERR_MSG("delay must be > 0");
+      flag = FAIL;
+    }
 
     // Check kinematics type
     test = FALSE;

File lib/test-util.c

   (config -> ctlr_param).type = CTLR_TYPE_VEL;
   (config -> ctlr_param).pgain = PGAIN;
   (config -> ctlr_param).dgain = DGAIN;
+  (config -> ctlr_param).bias = BIAS;
+  (config -> ctlr_param).delay = DELAY;
 
   for (i=0; i<config->num_motor; i++) {
 

File lib/test-util.h

 // Controller test paramters
 #define PGAIN 1.0
 #define DGAIN 1.0
+#define BIAS 1.0
+#define DELAY 5
 
 // Lookup table test parameters
 #define LOOKUP_TBL_NROW 50

File lib/test-yawff.c

   config_test.ctlr_flag = CTLR_ON;
   config_test.kine_param.period = KINE_MIN_PERIOD/2.0;
   CU_ASSERT_FALSE(check_config(config_test)==SUCCESS);
+
+  // Controller delay tests
+  config_test = config;
+  config_test.ctlr_flag = CTLR_ON;
+  config_test.ctlr_param.delay = -3;
+  CU_ASSERT_FALSE(check_config(config_test)==SUCCESS);
   
   // Lookup table tests
   config.ctlr_flag = CTLR_ON;
     else if (config.ctlr_param.type == CTLR_TYPE_POS) {
         printf("CTLR_TYPE_POS\n");
     }
+    else if (config.ctlr_param.type == CTLR_TYPE_KBIAS) {
+        printf("CTLR_TYPE_KBIAS\n");
+    }
     else {
         printf("unknown\n");
     }
     
     printf("  ctlr_pgian:          %1.2f\n", config.ctlr_param.pgain);
     printf("  ctlr_dgian:          %1.2f\n", config.ctlr_param.dgain);
+    printf("  ctlr_bias:           %1.2f\n", config.ctlr_param.bias);
+    printf("  ctlr_delay:          %d\n", config.ctlr_param.delay);
     printf("  stroke amp:          %1.2f\n", config.kine_param.stroke_amp);
     printf("  rotation amp:        %1.2f\n", config.kine_param.rotation_amp);
     printf("  stroke_k:            %1.2f\n", config.kine_param.stroke_k);
 int define_ctlr_off(void) {return CTLR_OFF;};
 int define_ctlr_type_vel(void) {return CTLR_TYPE_VEL;};
 int define_ctlr_type_pos(void) {return CTLR_TYPE_POS;};
+int define_ctlr_type_kbias(void) {return CTLR_TYPE_KBIAS;};
 int define_motor_caltype_tbl(void) {return MOTOR_CALTYPE_TBL;};
 int define_motor_caltype_mul(void) {return MOTOR_CALTYPE_MUL;};
 
 extern int define_ctlr_off(void);
 extern int define_ctlr_type_vel(void);
 extern int define_ctlr_type_pos(void);
+extern int define_ctlr_type_kbias(void);
 extern int define_motor_caltype_tbl(void);
 extern int define_motor_caltype_mul(void);
 
   data_t data;
   torq_info_t torq_info;
   state_t state[2];            // state[0] = previous, state[1] = current 
+  state_t state_est;           // estimated state, for handling delays
   int motor_ind[MAX_MOTOR][2]; // Motor index position [i][0] previous, [i][1] current
   int err_flag = 0;
   int i;
       break;
     }
 
+    // Get state estimate
+    state_est = get_state_est(state,data,i,config.ctlr_param.delay);
+
     // Update controller
     if (get_array_val(setpt,i,0,&curr_setpt) != SUCCESS) {
       PRINT_ERR_MSG("getting setpt value failed");
       err_flag |= RT_TASK_ERROR;
       break;
     }
-    if (get_ctlr_u(ctlr_err, &curr_u, i, curr_setpt, state, config) != SUCCESS) {
+    if (get_ctlr_u(ctlr_err, &curr_u, i, curr_setpt, state_est, config) != SUCCESS) {
       PRINT_ERR_MSG("getting control value failed");
       err_flag |= RT_TASK_ERROR;
       break;
 }
 
 // -------------------------------------------------------------------
+// Function: get_state_est
+//
+// Purpose: get state estimate. Adds any simumated sensor delay to the
+// state information.
+//
+// --------------------------------------------------------------------
+state_t get_state_est(
+    state_t *state,
+    data_t data,
+    int ind,
+    int delay
+    )
+{
+  state_t state_est;
+  int ind_delay;
+  float pos;
+  float vel;
+
+  if (delay == 0) {
+    // No delay
+    pos = state[1].pos;
+    vel = state[1].vel;
+  }
+  else {
+    // Delay is before start
+    ind_delay = ind - delay;
+    if (ind_delay < 0) {
+      pos = 0.0;
+      vel = 0.0;
+    }
+    else {
+      get_array_val(data.pos,ind_delay,0,&pos);
+      get_array_val(data.vel,ind_delay,0,&vel);
+    }
+  }
+  state_est.pos = pos;
+  state_est.vel = vel;
+
+  return state_est;
+}
+
+// -------------------------------------------------------------------
 // Function: get_ctlr_u 
 //
 // Purpose: get control signal value based on the current value of
     float *u,
     int ind,
     float setpt,  
-    state_t *state,
+    state_t state_est,
     config_t config
     )
 {
   float deriv_ctlr_err;
   float max_u;
 
-  // Get controller error based on controller type
-  ctlr_err[0] = ctlr_err[1];
 
-  switch(config.ctlr_param.type) { 
+  if (config.ctlr_param.type == CTLR_TYPE_KBIAS) {
+    *u = RAD2DEG*setpt - config.ctlr_param.pgain*state_est.vel;
+  }
+  else {
+    // Get controller error based on controller type
+    ctlr_err[0] = ctlr_err[1];
 
-    case CTLR_TYPE_POS:
-      ctlr_err[1] = DEG2RAD*setpt - state[1].pos;
-      break;
+    switch (config.ctlr_param.type) { 
 
-    case CTLR_TYPE_VEL:
-      ctlr_err[1] = DEG2RAD*setpt - state[1].vel;
-      break;
+      case CTLR_TYPE_POS:
+        ctlr_err[1] = DEG2RAD*setpt - state_est.pos;
+        break;
 
-    default:
-      PRINT_ERR_MSG("unknown controller type");
-      rtn_flag = FAIL;
-      break; 
+      case CTLR_TYPE_VEL:
+        ctlr_err[1] = DEG2RAD*setpt - state_est.vel;
+        break;
+
+      default:
+        PRINT_ERR_MSG("unknown controller type");
+        rtn_flag = FAIL;
+        break; 
+    }
+
+    // Get time rate of change of control error
+    if (ind != 0) {
+      deriv_ctlr_err = (ctlr_err[1] - ctlr_err[0])/(NS2S*(double) config.dt);
+    }
+    else {
+      // If it the first time through the loop set to zero to avoid
+      // discontinuities.
+      deriv_ctlr_err = 0.0;
+    }
+
+    // Compute control signal - simple PD controller 
+    *u = config.ctlr_param.pgain*ctlr_err[1] + config.ctlr_param.dgain*deriv_ctlr_err;
   }
 
-  // Get time rate of change of control error
-  if (ind != 0) {
-    deriv_ctlr_err = (ctlr_err[1] - ctlr_err[0])/(NS2S*(double) config.dt);
-  }
-  else {
-    // If it the first time through the loop set to zero to avoid
-    // discontinuities.
-    deriv_ctlr_err = 0.0;
-  }
-
-  // Compute control signal - simple PD controller 
-  *u = config.ctlr_param.pgain*ctlr_err[1] + config.ctlr_param.dgain*deriv_ctlr_err;
+  // Convert control signal to degrees
   *u = *u*RAD2DEG;
 
   // Clamp control signal
   float rot_amp;
   float str_k;
   float rot_k;
+  float bias;
   //float u_clamped;
 
   // Get kinematics parameters from configuration
 
     case DIFF_AOA_ID:
 
+      bias = config.ctlr_param.bias;
       vals[STROKE_0_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[STROKE_1_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
-      vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) - u;
-      vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) + u;
+      vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) - u - bias;
+      vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) + u + bias;
       vals[DEVIATION_0_ID] = 0.0;
       vals[DEVIATION_1_ID] = 0.0;
      break;
 
     case DIFF_DEV_ID:
 
+      bias = config.ctlr_param.bias;
       vals[STROKE_0_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[STROKE_1_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T)));
       vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T)));
-      vals[DEVIATION_0_ID] =  u;
-      vals[DEVIATION_1_ID] = -u; 
+      vals[DEVIATION_0_ID] =  u + bias;
+      vals[DEVIATION_1_ID] = -u - bias; 
      break; 
 
     default:
 // -----------------------------------------------------------------
 int get_start_pos(float setpt, array_t kine, config_t config)
 {
-  int i;
   int ind;
   float u;
   float ctlr_err[2] = {0.0, 0.0};
   double t;
-  state_t state[2];
+  state_t state;
 
   // Check configuration
   if (check_config(config) != SUCCESS) {
   }
 
   // Initialize dynamic state, time and  outscan index
-  for (i=0; i<2; i++) {
-    state[i].pos = 0.0;
-    state[i].vel = 0.0;
-  }  
+  state.pos = 0.0;
+  state.vel = 0.0;
   t = 0.0;
   ind = 0;
 
 #define RT_TASK_ERROR 2   // Mask used to detect if an error occured in the realtime thread
 #define RT_TASK_SIGINT 4  // Mask used to detect if an sigint stopped the realtime thread
 
-#define CTLR_TYPE_VEL 0   // Specifies velocity controller
-#define CTLR_TYPE_POS 1   // Specifies position controller
+#define CTLR_TYPE_VEL 0       // Specifies velocity controller
+#define CTLR_TYPE_POS 1       // Specifies position controller
+#define CTLR_TYPE_KBIAS 2     // Specifies velocity based damping w/ kinematics bias 
 
 #define MOTOR_CALTYPE_TBL 0  // Lookup table motor calibration
 #define MOTOR_CALTYPE_MUL 1  // Multiplicative motor calibration
   int type;
   float pgain;
   float dgain;
+  float bias;
+  int delay;
 } ctlr_param_t;
 
 // Wing kinematics parameter stucture
     config_t config
     );
 
+// Update state estimate
+extern state_t get_state_est(
+    state_t *state,
+    data_t data,
+    int ind,
+    int delay
+    );
+
 // Get control value 
 extern int get_ctlr_u(
     float ctlr_err[],
     float *u,
     int ind,
     float setpt,  
-    state_t *state,
+    state_t state_est,
     config_t config
     );
 
     config_t config
     );
 
-// Update feedback controller
-extern int update_ctlr(
-    int i,
-    state_t *state,
-    array_t setpt,
-    array_t u,
-    config_t config
-    );
-
 // Reassign sigint signal handler
 extern sighandler_t reassign_sigint(sighandler_t sigint_func);
 

File python/examples/yawff_ctlr_wrapper_test.py

     'ctlr_type'         : libyawff.CTLR_TYPE_VEL,
     'ctlr_pgain'        : 1.0,
     'ctlr_dgain'        : 0.1,
+    'ctlr_bias'         : 1.0,
+    'ctlr_delay'        : 2.0,
     'kine_type'         : 'diff_aoa',
     'kine_period'       : 6.0,
     'stroke_amp'        : 90.0,

File python/examples/yawff_w_ctlr_test.py

     'startup_t'         : 0.0,
     'ff_flag'           : libyawff.FF_ON,
     'ctlr_flag'         : libyawff.CTLR_ON,
-    'ctlr_type'         : libyawff.CTLR_TYPE_VEL,
+    #'ctlr_type'         : libyawff.CTLR_TYPE_VEL,
+    'ctlr_type'         : libyawff.CTLR_TYPE_KBIAS,
     'ctlr_pgain'        : 2.0,
     'ctlr_dgain'        : 0.0,
+    'ctlr_bias'         : 0.0,
+    'ctlr_delay'        : 2.0,
     'kine_type'         : 'diff_aoa',
     'kine_period'       : 6.0,
     'stroke_amp'        : 70.0,
 
 yawff = libyawff.Yawff_w_Ctlr(run_params, pause_t=1.0)
 
-T = 8.0*yawff.config_dict['kine_period'] 
+T = 1.0*yawff.config_dict['kine_period'] 
 N = int(T/dt) 
 tt = scipy.arange(0.0,N)*dt
 setpt = 5.0*scipy.sin(2.0*scipy.pi*tt/(2.0*yawff.config_dict['kine_period']))

File python/libyawff/yawff.py

             config_struct.ctlr_param.type = int(config['ctlr_type'])
             config_struct.ctlr_param.pgain = float(config['ctlr_pgain'])
             config_struct.ctlr_param.dgain = float(config['ctlr_dgain'])
+            config_struct.ctlr_param.bias = float(config['ctlr_bias'])
+            # Compute delay in indices
+            delay_ind = int(config['ctlr_delay']/config['dt'])
+            if delay_ind < 0:
+                raise ValueError, 'delay must be > 0'
+            config_struct.ctlr_param.delay = delay_ind
 
             config_struct.kine_param.type = int(kine_type_id_dict[config['kine_type']])
             config_struct.kine_param.period = float(config['kine_period'])
 CTLR_OFF = lib.define_ctlr_off()
 CTLR_TYPE_VEL = lib.define_ctlr_type_vel()
 CTLR_TYPE_POS = lib.define_ctlr_type_pos()
+CTLR_TYPE_KBIAS = lib.define_ctlr_type_kbias()
 MOTOR_CALTYPE_TBL = lib.define_motor_caltype_tbl()
 MOTOR_CALTYPE_MUL = lib.define_motor_caltype_mul()
 
         ('type', ctypes.c_int),
         ('pgain', ctypes.c_float),
         ('dgain', ctypes.c_float),
+        ('bias', ctypes.c_float),
+        ('delay', ctypes.c_int),
     ]
 
 class kine_param_t(ctypes.Structure):