/* This is file hd_model.h. * It is the header file for program hd_model.c. * * Copyright 1993 Massachusetts Institute of Technology */ /* Define a bunch of constants. */ #define SHOULDER 0 #define ELBOW 1 #define WRIST 2 #define JOINT_TO_TEST SHOULDER #define NUM_OF_AXIS 3 #define STIFFNESS_POINTS 1000 #define TRUE (1) #define FALSE (0) #define TINY_VEL (1.0e-20) /* Constants specifying the position of the local variables * in the state vector. */ #define POS_IN_INDEX 1 #define POS_OUT_INDEX 2 #define VEL_IN_INDEX 3 #define VEL_OUT_INDEX 4 #define ENERGY 5 /* The order of the system (number of state variables). */ #define ORDER 4 /* Define index numbers for two different harmonic-drive models. */ #define ROTARY 0 #define GEAR_TOOTH 1 #define HD_MODELS 2 /* Define the degrees-to-radians conversion factor used in the equations. */ #define PI 3.14159265359 #define CONV (PI/180.0) /* Define mathematical functions to translate Mathematica's C-formatted * equations into a form understandable to the compiler. */ #define Power(x,y) pow((double) x, (double) y) #define Sin(x) sin((double) x) #define Cos(x) cos((double) x) /* Define the simulation accuracy variable. */ #define EPS 0.00001 /* Define a arbitrary intial step size and a minimum step size. */ #define INITIAL_STEP 0.0001 #define MINIMUM_STEP 0.0000000000000000001 /* Define constants describing the amplifier voltage-current curve. */ #define IMAX 20.0 #define VMAX 40.0 #define VI_SLOPE -1.0 #define AMP_DAMPING 0.5 /* Define the number of data runs to be made if the -plot option is selected. */ #define SHLD_NUM_OF_DATA_RUNS 10 #define ELB_NUM_OF_DATA_RUNS 8 #define WRIST_NUM_OF_DATA_RUNS 8 /* Define the indicies for the vector to hold all variables that can be * selected for output data plots. */ /* The harmonic drive positions, velocities, and torques. */ #define POS_WG 0 #define POS_FS 1 #define POS_CS 2 #define POS_TOOTH_BASE 3 #define POS_ERR 4 #define POS_TOOTH_TIP 5 #define POS_WG_SURFACE 6 #define POS_TOOTH_SURFACE 7 #define POS_WG_RELATIVE 8 #define POS_K 9 #define POS_N_WG 10 #define POS_N_FS 11 #define POS_N_CS 12 #define VEL_WG 13 #define VEL_FS 14 #define VEL_CS 15 #define VEL_TOOTH_BASE 16 #define VEL_ERR 17 #define VEL_TOOTH_TIP 18 #define VEL_WG_SURFACE 19 #define VEL_TOOTH_SURFACE 20 #define VEL_WG_RELATIVE 21 #define VEL_K 22 #define VEL_N_WG 23 #define VEL_N_FS 24 #define VEL_N_CS 25 #define VEL_HD_FRICTION 26 #define TORQUE_WG 27 #define TORQUE_FS 28 #define TORQUE_CS 29 #define TORQUE_INPUT_NORMAL 30 #define TORQUE_CYCLIC 31 #define TORQUE_WG_NORMAL 32 #define TORQUE_WG_FRICTION 33 #define TORQUE_ERR_IN 34 #define TORQUE_K 35 #define TORQUE_TOOTH_TIP_NORMAL 36 #define TORQUE_TOOTH_TIP_FRICTION 37 #define TORQUE_TOOTH_SURFACE_NORMAL 38 #define TORQUE_TOOTH_SURFACE_TOTAL_LOSS 39 #define TORQUE_TOOTH_SURFACE_COULOMB 40 #define TORQUE_TOOTH_SURFACE_FRICTION 41 #define TORQUE_CYCLIC_FRICTION 42 #define TORQUE_OUTPUT_NORMAL 43 #define TORQUE_N_WG 44 #define TORQUE_N_FS 45 #define TORQUE_N_CS 46 #define TORQUE_HD_FRICTION 47 /* The positions, velocities, and torques outside the harmonic drive. */ #define POS_IN 48 #define POS_OUT 49 #define VEL_IN 50 #define VEL_OUT 51 #define TORQUE_MOTOR 52 #define TORQUE_B_IN 53 #define TORQUE_B_OUT 54 /* Sensor variables. */ #define INPUT_POSITION_SENSOR 55 #define OUTPUT_POSITION_SENSOR 56 #define INPUT_VELOCITY_SENSOR 57 #define OUTPUT_VELOCITY_SENSOR 58 #define CURRENT_SENSOR 59 #define TORQUE_SENSOR 60 #define POSITION_ERROR 61 /* Parameter functions which can be plotted. */ #define K1 62 #define K2 63 #define ERFN 64 #define DERF 65 /* Energy Variables. */ #define TOTAL_ENERGY 66 #define TOTAL_KINETIC_ENERGY 67 #define TOTAL_POTENTIAL_ENERGY 68 #define TOTAL_INPUT_ENERGY 69 #define TOTAL_LOST_ENERGY 70 #define MOTOR_ENERGY 71 #define KINETIC_ENERGY_IN 72 #define KINETIC_ENERGY_OUT 73 #define SPRING_ENERGY 74 #define CYCLIC_ENERGY 75 #define INPUT_DAMPING_ENERGY 76 #define WG_FRICTION_ENERGY 77 #define TOOTH_TIP_FRICTION_ENERGY 78 #define TOOTH_SURFACE_TOTAL_LOSS_ENERGY 79 #define TOOTH_SURFACE_COULOMB_ENERGY 80 #define TOOTH_SURFACE_FRICTION_ENERGY 81 #define HD_FRICTION_ENERGY 82 #define OUTPUT_DAMPING_ENERGY 83 /* Define the number of variables in the output data vector. */ #define NUM_OF_OUTPUT_VAR 84 /* Declare the variables for the input parameters used in the harmonic drive * equations. */ double k1_output_constant, k1_output_amplitude, k1_output_phase; double k2_output_constant, k2_output_amplitude, k2_output_phase; double hysteresis_width; double error_amplitude0, error_phase0; double error_amplitude1, error_phase1; double error_amplitude2, error_phase2; double cyclic_amplitude, cyclic_phase; double b_wg_constant, b_wg1, b_wg2; double b_tooth_tip_constant, b_tooth_tip1, b_tooth_tip2; double cyclic_friction_amplitude, cyclic_friction_phase; double mu, mu_save; double b_total1, b_total2; double tooth_angle, N; double stiction_torque, stiction_vel; /* Declare the variables for the joint parameters. */ double inertia_in, inertia_out; double b_in, b_out; double motor_kt, motor_kb, amp_resistance; /* Requested step in motor current. */ double irequested; /* Initial and final time, the stepsize interval to save results */ double initial_time, final_time, step; /* Define the initial-condition vectors.*/ double xstart[ORDER+1], xstart_sav[ORDER+1]; /* Define a vector to hold all of the possible output data values for * each simulation time step. */ double v[NUM_OF_OUTPUT_VAR]; /* Define output vectors to be allocated dynamically. */ double **output_data, *time; /* Define the parameters that are calculated from the input parameters * for the rotary harmonic drive model. */ double k1_constant, k1_amplitude, k1_phase; double k2_constant, k2_amplitude, k2_phase; double stiction_torque_hd, stiction_vel_hd; double cyclic_friction_amp_hd, cyclic_friction_phase_hd; double b_hd_constant, b_hd1, b_hd2; /* Define the parameters that are calculated from the input parameters * for the tooth-rubbing harmonic drive model. */ double tan1, cos1, sin1, tan2, cos2, sin2; double wg_angle; double stiction_torque_tooth, stiction_vel_tooth; double cyclic_friction_amp_tooth, cyclic_friction_phase_tooth; double b_tooth_surface_constant, b_tooth_surface1, b_tooth_surface2;