Anonymous avatar Anonymous committed e84b27b

Modified the update_ind function to handle the new data structures.

Comments (0)

Files changed (4)

 
 int sixax_init_cal(Calibration **cal, char *cal_file_path, float tool_trans[])
 {
-	short rtn_flag;    
+    short rtn_flag;    
     unsigned short index = 1;
 
-	// create Calibration struct
+    // create Calibration struct
     *cal=createCalibration(cal_file_path,index);
     if (cal==NULL) {
         PRINT_ERR_MSG("Specified calibration could not be loaded.\n");
         return FAIL;
     }
 
-	// Set force units.
-	rtn_flag=SetForceUnits(*cal,"N");
-	switch (rtn_flag) {
-		case 0: 
+    // Set force units.
+    rtn_flag=SetForceUnits(*cal,"N");
+    switch (rtn_flag) {
+        case 0: 
             // successful completion
-            break;	
-		case 1: 
+            break;  
+        case 1: 
                 PRINT_ERR_MSG("Invalid Calibration struct\n"); 
                 return FAIL;
-		case 2: 
+        case 2: 
                 PRINT_ERR_MSG("Invalid force units\n"); 
                 return FAIL;
-		default: 
+        default: 
                 PRINT_ERR_MSG("Unknown error\n"); 
                 return FAIL;
-	}
+    }
 
-	// Set torque units.
-	rtn_flag=SetTorqueUnits(*cal,"N-m");
-	switch (rtn_flag) {
-		case 0: 
+    // Set torque units.
+    rtn_flag=SetTorqueUnits(*cal,"N-m");
+    switch (rtn_flag) {
+        case 0: 
             // successful completion
-            break;	
-		case 1: 
+            break;  
+        case 1: 
             PRINT_ERR_MSG("Invalid Calibration struct\n"); 
             return FAIL;
-		case 2: 
+        case 2: 
             PRINT_ERR_MSG("Invalid torque units\n"); 
             return FAIL;
-		default: 
+        default: 
             PRINT_ERR_MSG("Unknown error\n"); 
             return FAIL;
-	}
+    }
 
-	// Set tool transform.
-	// This line is only required if you want to move or rotate the sensor's coordinate system.
-	// This example tool transform translates the coordinate system 20 mm along the Z-axis 
-	// and rotates it 45 degrees about the X-axis.
+    // Set tool transform.
+    // This line is only required if you want to move or rotate the sensor's coordinate system.
+    // This example tool transform translates the coordinate system 20 mm along the Z-axis 
+    // and rotates it 45 degrees about the X-axis.
     
-	// This sample transform includes a translation along the Z-axis and a rotation about the X-axis.
-	// float tool_trans[6]={0,0,20,45,0,0};
+    // This sample transform includes a translation along the Z-axis and a rotation about the X-axis.
+    // float tool_trans[6]={0,0,20,45,0,0};
     
-	rtn_flag=SetToolTransform(*cal,tool_trans,"mm","degrees");
-	switch (rtn_flag) {
-		case 0: 
+    rtn_flag=SetToolTransform(*cal,tool_trans,"mm","degrees");
+    switch (rtn_flag) {
+        case 0: 
             // successful completion
-            break;	
-		case 1: 
+            break;  
+        case 1: 
             PRINT_ERR_MSG("Invalid Calibration struct\n"); 
             return FAIL;
-		case 2: 
+        case 2: 
             PRINT_ERR_MSG("Invalid distance units\n"); 
             return FAIL;
-		case 3: 
+        case 3: 
             PRINT_ERR_MSG("Invalid angle units\n"); 
             return FAIL;
-		default: 
+        default: 
             PRINT_ERR_MSG("Unknown error\n"); 
             return FAIL;
-	}
+    }
 
-	// Temperature compensation is on by default if it is available.
-	// To explicitly disable temperature compensation, uncomment the following code
-	rtn_flag = SetTempComp(*cal,FALSE);                   // disable temperature compensation
-	switch (rtn_flag) {
-		case 0: 
+    // Temperature compensation is on by default if it is available.
+    // To explicitly disable temperature compensation, uncomment the following code
+    rtn_flag = SetTempComp(*cal,FALSE);                   // disable temperature compensation
+    switch (rtn_flag) {
+        case 0: 
             // successful completion
-            break;	
-		case 1: 
+            break;  
+        case 1: 
             PRINT_ERR_MSG("Invalid Calibration struct\n"); 
             return FAIL;
-		case 2: 
+        case 2: 
             PRINT_ERR_MSG("Temperature Compensation not available on this transducer\n"); 
             return FAIL;
-		default: 
+        default: 
             PRINT_ERR_MSG("Unknown error\n"); 
             return FAIL;
-	}
+    }
     return SUCCESS;
 } 
 
 void sixax_free_cal(Calibration *cal)
 {
-	// free memory allocated to Calibration structure
-	destroyCalibration(cal);
+    // free memory allocated to Calibration structure
+    destroyCalibration(cal);
     return;
 }
 
 int sixax_set_bias(Calibration *cal, float bias[])
 {
-	// store an unloaded measurement; this removes the effect of tooling weight
-	Bias(cal,bias);
+    // store an unloaded measurement; this removes the effect of tooling weight
+    Bias(cal,bias);
     return SUCCESS;
 } 
 
 int sixax_sample2ft(Calibration *cal, float sample[], float ft[])
 {
-	// convert a loaded measurement into forces and torques
-	ConvertToFT(cal,sample,ft);
+    // convert a loaded measurement into forces and torques
+    ConvertToFT(cal,sample,ft);
     return SUCCESS;
 }
 
     int i;
     int j;
 
-	// display info from calibration file
+    // display info from calibration file
     printf("\n");
     printf("Six-axis sensor calibration information \n");
     printf("\n");
-	printf("                  Serial: %s\n",cal->Serial);
-	printf("              Body Style: %s\n",cal->BodyStyle);
-	printf("             Calibration: %s\n",cal->PartNumber);
-	printf("        Calibration Date: %s\n",cal->CalDate);
-	printf("                  Family: %s\n",cal->Family);
-	printf("              # Channels: %i\n",cal->rt.NumChannels);
-	printf("                  # Axes: %i\n",cal->rt.NumAxes);
-	printf("             Force Units: %s\n",cal->ForceUnits);
-	printf("            Torque Units: %s\n",cal->TorqueUnits);
-	printf("Temperature Compensation: %s\n",(cal->TempCompAvailable ? "Yes" : "No"));
+    printf("                  Serial: %s\n",cal->Serial);
+    printf("              Body Style: %s\n",cal->BodyStyle);
+    printf("             Calibration: %s\n",cal->PartNumber);
+    printf("        Calibration Date: %s\n",cal->CalDate);
+    printf("                  Family: %s\n",cal->Family);
+    printf("              # Channels: %i\n",cal->rt.NumChannels);
+    printf("                  # Axes: %i\n",cal->rt.NumAxes);
+    printf("             Force Units: %s\n",cal->ForceUnits);
+    printf("            Torque Units: %s\n",cal->TorqueUnits);
+    printf("Temperature Compensation: %s\n",(cal->TempCompAvailable ? "Yes" : "No"));
     printf("\n");
-	
-	// print maximum loads of axes
-	printf("\nRated Loads\n");
-	for (i=0;i<cal->rt.NumAxes;i++) {
-		char *units;
-		if ((cal->AxisNames[i])[0]=='F') {
-			units=cal->ForceUnits;
-		} else units=cal->TorqueUnits;
-		printf("%s: %f %s\n",cal->AxisNames[i],cal->MaxLoads[i],units);
-	}
+    
+    // print maximum loads of axes
+    printf("\nRated Loads\n");
+    for (i=0;i<cal->rt.NumAxes;i++) {
+        char *units;
+        if ((cal->AxisNames[i])[0]=='F') {
+            units=cal->ForceUnits;
+        } else units=cal->TorqueUnits;
+        printf("%s: %f %s\n",cal->AxisNames[i],cal->MaxLoads[i],units);
+    }
 
-	// print working calibration matrix
-	printf("\nWorking Calibration Matrix\n");
-	printf("     ");
-	for (i=0;i<cal->rt.NumChannels-1;i++)
-		printf("G%i            ",i);
-	printf("\n");
-	for (i=0;i<cal->rt.NumAxes;i++) {
-		printf("%s: ",cal->AxisNames[i]);
-		for (j=0;j<cal->rt.NumChannels-1;j++)
-			printf("%13.5e ",cal->rt.working_matrix[i][j]);
-		printf("\n");
-	}
+    // print working calibration matrix
+    printf("\nWorking Calibration Matrix\n");
+    printf("     ");
+    for (i=0;i<cal->rt.NumChannels-1;i++)
+        printf("G%i            ",i);
+    printf("\n");
+    for (i=0;i<cal->rt.NumAxes;i++) {
+        printf("%s: ",cal->AxisNames[i]);
+        for (j=0;j<cal->rt.NumChannels-1;j++)
+            printf("%13.5e ",cal->rt.working_matrix[i][j]);
+        printf("\n");
+    }
 
-	// print temperature compensation information, if available
-	if (cal->TempCompAvailable) {
-		printf("\nTemperature Compensation Information\n");
-		printf("BS: ");
-		for (i=0;i<cal->rt.NumChannels-1;i++) {
-			printf("%13.5e ",cal->rt.bias_slopes[i]);
-		}
-		printf("\nGS: ");
-		for (i=0;i<cal->rt.NumChannels-1;i++) {
-			printf("%13.5e ",cal->rt.gain_slopes[i]);
-		}
-		printf("\nTherm: %f\n",cal->rt.thermistor);
-	}
+    // print temperature compensation information, if available
+    if (cal->TempCompAvailable) {
+        printf("\nTemperature Compensation Information\n");
+        printf("BS: ");
+        for (i=0;i<cal->rt.NumChannels-1;i++) {
+            printf("%13.5e ",cal->rt.bias_slopes[i]);
+        }
+        printf("\nGS: ");
+        for (i=0;i<cal->rt.NumChannels-1;i++) {
+            printf("%13.5e ",cal->rt.gain_slopes[i]);
+        }
+        printf("\nTherm: %f\n",cal->rt.thermistor);
+    }
 
     // Print bias vector
     printf("\nBias vector\n");
 //
 // ---------------------------------------------------------------------
 
-/*
 int update_ind(
-    int motor_ind[][2],
-    array_t kine, 
-    int kine_ind,
-    state_t *state, 
-    config_t config
-    )
+        motor_ind_t motor_ind[],
+        array_t kine, 
+        int kine_ind,
+        state_t state[], 
+        config_t config
+        )
 {
-  int i;
-  int ind;
-  int kine_num;
-  int motor_num; 
+    int i;
+    int ind;
+    int kine_num=0;
+    int motor_num; 
+    int ff_test;
+    int ff_index;
 
-  // Set current indices to previous indices 
-  for (i=0; i<config.num_motor; i++) {
-    motor_ind[i][0] = motor_ind[i][1];
-  }
+    // Set previous indices to current indices 
+    for (i=0; i<config.num_motor; i++) {
+        motor_ind[i].prev = motor_ind[i].curr;
+    }
 
-  // Set current index
-  kine_num = 0;
-  for (i=0; i<config.num_motor; i++) {
-    if (i == config.yaw_motor)  {
-      // This is the yaw motor 
-      if (config.ff_flag == FF_ON) {
-        // Force-feedback is on - get index from current state
-        ind = (int)((RAD2DEG/config.yaw_ind2deg)*state[1].pos);
-        motor_num = config.yaw_motor;
-      }
-      else {
-        // Force-feedback is off - get index from kinematics
-        motor_num = config.yaw_motor;
-        if (get_array_val(kine,kine_ind,motor_num,&ind) != SUCCESS) {
-          PRINT_ERR_MSG("problem accessing kine array");
-          return FAIL;
+    // Loop over motors
+    for (i=0; i<config.num_motor; i++) {
+        ff_test = is_ff_motor(i,config,&ff_index);
+        if (ff_test == TRUE) {
+            // This is a force feedback motor
+            motor_num = config.ff_motor[ff_index];
+            if (config.ff_flag[ff_index]==FF_ON) {
+                // Force-feedback is on - get index from current state
+                ind = (int) (state[ff_index].pos/config.ff_ind2unit[ff_index]);
+            }
+            else {
+                // Force-feedback is off - get index from kinematics
+                if (get_array_val(kine, kine_ind, motor_num, &ind) != SUCCESS) {
+                    PRINT_ERR_MSG("problem accessing kine array");
+                    return FAIL;
+                } 
+                // Set position in state structure
+                state[ff_index].pos_prev = state[ff_index].pos;
+                state[ff_index].pos = config.ff_ind2unit[ff_index]*ind;
+            }
+        }
+        else { 
+            // This is not a force feedback motor 
+            motor_num = config.kine_map[kine_num];
+            // Get index from kine array
+            if (get_array_val(kine,kine_ind,motor_num,&ind) != SUCCESS) {
+                PRINT_ERR_MSG("problem accessing kine array");
+                return FAIL;
+            }
         } 
-        // Set position in state
-        state[0].pos = config.yaw_ind2deg*DEG2RAD*ind;
-        state[1].pos = config.yaw_ind2deg*DEG2RAD*ind;
-      }
+        motor_ind[motor_num].curr = ind;
+        kine_num += 1;
     }
-    else { 
-      // This is a wing motor
-      motor_num = config.kine_map[kine_num];
+    return SUCCESS;
+}
 
-      // Get index from kine array
-      if (get_array_val(kine,kine_ind,motor_num,&ind) != SUCCESS) {
-        PRINT_ERR_MSG("problem accessing kine array");
-        return FAIL;
-      }
-      kine_num += 1;
+// --------------------------------------------------------------------
+// Function: is_ff_motor
+//
+// Tests whether or not the given index corresponds to a force feed back
+// motor.
+// --------------------------------------------------------------------
+int is_ff_motor(int index, config_t config, int *ff_index)
+{
+    int i;
+    int ff_test = FALSE;
+    for (i=0; i<NUM_FF; i++) {
+        if (config.ff_motor[i] == index) {
+            ff_test = TRUE;
+            *ff_index = i;
+            break;
+        }
     } 
-    motor_ind[motor_num][1] = ind;
-  }
-  return SUCCESS;
+    return ff_test;
 }
-*/
-
 
 // ---------------------------------------------------------------------
 // Function: init_ind
         return FAIL;
     }
 
-  ///////////////////////////////////////////////////////////////////
-  // Constant torque test  - set torque to some known value.
-  //for (i=0; i<NUM_FF; i++) {
-  //    ff_ind = config.ff_ft[i];
-  //    ft_raw[ff_ind] = 0.001;
-  //}
-  ///////////////////////////////////////////////////////////////////
-  
+    ///////////////////////////////////////////////////////////////////
+    // Constant torque test  - set torque to some known value.
+    //for (i=0; i<NUM_FF; i++) {
+    //    ff_ind = config.ff_ft[i];
+    //    ft_raw[ff_ind] = 0.001;
+    //}
+    ///////////////////////////////////////////////////////////////////
 
-  // If we are inside start up wff_indow set the force feedback forces/torques to zero
-  if (t < (double)config.startup_t) {
-      for (i=0; i<NUM_FF; i++) {
-          ff_ind = config.ff_ft[i];
-          ft_raw[ff_ind] = 0.0;
-      }
-  }
 
-  // Lowpass filter forces and torques 
-  for (i=0; i<6; i++) {
-      ft_filt[i] = lowpass_filt1(
-              ft_raw[i], 
-              (ft_info->ft_last)[i], 
-              config.ain_filt_lpcut,
-              dt
-              );
-  }
+    // If we are inside start up wff_indow set the force feedback forces/torques to zero
+    if (t < (double)config.startup_t) {
+        for (i=0; i<NUM_FF; i++) {
+            ff_ind = config.ff_ft[i];
+            ft_raw[ff_ind] = 0.0;
+        }
+    }
 
-  // Update ft_info structure
-  for (i=0; i<6; i++) {
-      (ft_info -> ft_last)[i] = ft_filt[i];
-      (ft_info -> ft_raw)[i] = ft_raw[i];
-  }
+    // Lowpass filter forces and torques 
+    for (i=0; i<6; i++) {
+        ft_filt[i] = lowpass_filt1(
+                ft_raw[i], 
+                (ft_info->ft_last)[i], 
+                config.ain_filt_lpcut,
+                dt
+                );
+    }
 
+    // Update ft_info structure
+    for (i=0; i<6; i++) {
+        (ft_info -> ft_last)[i] = ft_filt[i];
+        (ft_info -> ft_raw)[i] = ft_raw[i];
+    }
 
-  // Update dynamic state - only if force-feedback is turned on
-  if (config.ff_flag == FF_ON) {
-
-      for (i=0; i<NUM_FF; i++) {
-
-          // Set previous state to current state
-          state[i].pos_prev = state[i].pos;
-          state[i].vel_prev = state[i].vel;
-
-          // Integrate one time step
-          ff_ind = config.ff_ft[i];
-          rval = integrator(
-                  &state[i],
-                  ft_filt[ff_ind],
-                  config.ff_mass[i], 
-                  config.ff_damping[i], 
-                  dt,
-                  config.ff_integ_type
-                  );
-          if (rval != SUCCESS ) {
-              PRINT_ERR_MSG("integrator failed");
-              return FAIL;
-          }
-      }
-  }
-
-  return SUCCESS;
+    // Update dynamic state - only if force-feedback is turned on
+    if (config.ff_flag == FF_ON) {
+        for (i=0; i<NUM_FF; i++) {
+            // Integrate one time step
+            ff_ind = config.ff_ft[i];
+            rval = integrator(
+                    &state[i],
+                    ft_filt[ff_ind],
+                    config.ff_mass[i], 
+                    config.ff_damping[i], 
+                    dt,
+                    config.ff_integ_type
+                    );
+            if (rval != SUCCESS ) {
+                PRINT_ERR_MSG("integrator failed");
+                return FAIL;
+            }
+        }
+    }
+    return SUCCESS;
 }
 
 // -----------------------------------------------------------------------
         motor_ind_t motor_ind[],
         array_t kine, 
         int kine_ind, 
-        state_t *state, 
+        state_t state[], 
         config_t config
         );
 
         config_t config
         );
 
+// Test if motor index corresponds to force feedback motor
+extern int is_ff_motor(int index, config_t config, int *ff_index);
+
 // Reassign sigint signal handler
 extern sighandler_t reassign_sigint(sighandler_t sigint_func);
 
     state_t k0,k1,k2,k3;
     int ret_flag = SUCCESS;
 
+    // Set previous state to current state
+    state -> pos_prev = state -> pos;
+    state -> vel_prev = state -> vel;
+
     switch (method) {
 
         case INTEG_RKUTTA: // Integrate using Runge-Kutta method
 {
     state_t state_out;
     state_out.pos = state_in.vel;
-    // Linear drag
     state_out.vel = force/mass - (damping/mass)*state_in.vel;
+    state_out.pos_prev = state_in.pos; 
+    state_out.vel_prev = state_in.vel;
     return state_out;
 }
 
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.