function_utilities.cpp

00001 #include "setting.h"
00002 #include "SPI_EEP_ENC.h"
00003 #include "function_utilities.h"
00004 #include "function_CAN.h"
00005 #include "stm32f4xx_flash.h"
00006 #include "FlashWriter.h"
00007 
00008 /*******************************************************************************
00009  * VARIABLE
00010  ******************************************************************************/
00011 
00012 // Board Information
00013 uint8_t BNO = 0;
00014 uint8_t CONTROL_MODE = 0;
00015 uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
00016 uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
00017 uint8_t SUPPLY_PRESSURE_UPDATE = 0; // (0 : Update Off (constant Ps) , 1 : Update On (variable Ps))
00018 
00019 uint8_t CONTROL_UTILITY_MODE = 0;
00020 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
00021 uint8_t FLAG_VALVE_DEADZONE = 0;
00022 uint8_t REFERENCE_MODE = 1;
00023 int16_t CAN_FREQ = 500;
00024 int16_t DIR_JOINT_ENC = 0;
00025 int16_t DIR_VALVE = 0;
00026 int16_t DIR_VALVE_ENC = 0;
00027 
00028 float SUPPLY_VOLTAGE = 12.0f;
00029 float VALVE_VOLTAGE_LIMIT = 12.0f;  //v
00030 
00031 float P_GAIN_VALVE_POSITION = 0.0f;
00032 float I_GAIN_VALVE_POSITION= 0.0f;
00033 float D_GAIN_VALVE_POSITION= 0.0f;
00034 float P_GAIN_JOINT_POSITION = 0.0f;
00035 float I_GAIN_JOINT_POSITION = 0.0f;
00036 float D_GAIN_JOINT_POSITION = 0.0f;
00037 float P_GAIN_JOINT_TORQUE = 0.0f;
00038 float I_GAIN_JOINT_TORQUE = 0.0f;
00039 float D_GAIN_JOINT_TORQUE = 0.0f;
00040 float P_GAIN_JOINT_TORQUE_FF = 0.0f;
00041 float I_GAIN_JOINT_TORQUE_FF = 0.0f;
00042 float D_GAIN_JOINT_TORQUE_FF = 0.0f;
00043 
00044 int16_t K_SPRING = 0;
00045 int16_t D_DAMPER = 0;
00046 
00047 int16_t flag_delay_test = 0;
00048 
00049 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
00050 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
00051 //float D_GAIN_VALVE_POSITION_OPP= 0.0f;
00052 //float P_GAIN_JOINT_POSITION_OPP = 0.0f;
00053 //float I_GAIN_JOINT_POSITION_OPP = 0.0f;
00054 //float D_GAIN_JOINT_POSITION_OPP = 0.0f;
00055 //float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
00056 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
00057 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
00058 
00059 int16_t VALVE_DEADZONE_PLUS;
00060 int16_t VALVE_DEADZONE_MINUS;
00061 
00062 int16_t VELOCITY_COMP_GAIN;
00063 //int16_t COMPLIANCE_GAIN;
00064 
00065 int16_t VALVE_CENTER;
00066 
00067 int16_t VALVE_FF;
00068 
00069 int16_t BULK_MODULUS;
00070 
00071 int16_t CHAMBER_VOLUME_A;
00072 int16_t CHAMBER_VOLUME_B;
00073 
00074 int16_t PISTON_AREA_A;
00075 int16_t PISTON_AREA_B;
00076 float PISTON_AREA_alpha;
00077 float alpha3 = 1.0f;
00078 
00079 float PRES_SUPPLY_NOM = 70.0f;
00080 float PRES_SUPPLY = 70.0f;
00081 
00082 int16_t ENC_LIMIT_PLUS;
00083 int16_t ENC_LIMIT_MINUS;
00084 
00085 int16_t STROKE;
00086 
00087 float Amm = 236.4f;
00088 float beta = 1300000000.0f;
00089 float Ps = 10000000.0f; //100bar = 100*10^5 Pa
00090 float Pt = 0.0f;    // 0bar = 0Pa
00091 //float Kv = 0.00000002635f;          // Q = Kv*xv*sqrt(Ps-Pa)    => 100bar full opening 5LPM    (full opening : xv = 1)  [unit] m^3.5/kg^0.5
00092 float gamma_hat = 1075.0f; // Kv*beta*A/(sqrt(2)*V)   0.00000002635f * 1300000000.0f *  / (sqrt(2.0f)*(1256.6f + 236.4f * 39.75f) * 0.000000001f / 2)     [unit] m^3.5/kg^0.5
00093 float a_hat = -13707631.7f;
00094 float V_adapt = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
00095 float x_4_des_old = 0.0f;
00096 
00097 //int16_t VALVE_LIMIT_PLUS;
00098 //int16_t VALVE_LIMIT_MINUS;
00099 
00100 float ENC_PULSE_PER_POSITION = 1.0f;
00101 float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f;
00102 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
00103 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
00104 
00105 int16_t HOMEPOS_OFFSET;
00106 int HOMEPOS_VALVE_OPENING;
00107 
00108 float FRICTION;
00109 float REF_PERIOD;
00110 float REF_MAG;
00111 int REF_NUM;
00112 
00113 
00114 float DAC_REF;
00115 float DAC_RESOL;
00116 
00117 float REF_FORCE = 0.0;
00118 float REF_TORQUE = 0.0;
00119 float REF_POSITION = 0.0;
00120 float REF_VELOCITY = 0.0;
00121 
00122 float REF_POSITION_FINDHOME = 0.0;
00123 
00124 int16_t REF_PWM;
00125 int16_t REF_VALVE_POSITION;
00126 int16_t REF_CURRENT;
00127 
00128 int REF_MOVE_TIME_5k;
00129 int INIT_REF_PWM;
00130 int INIT_REF_VALVE_POS;
00131 int INIT_REF_VEL;
00132 int INIT_REF_TORQUE;
00133 int INIT_REF_PRES_DIFF;
00134 int INIT_REF_CURRENT;
00135 
00136 unsigned int    TMR2_COUNT_LED1;
00137 unsigned int    TMR2_COUNT_LED2;
00138 unsigned int    TMR2_COUNT_CAN_TX = 0;
00139 unsigned int    TMR3_COUNT_TEST = 0;
00140 
00141 int num_err;
00142 int flag_err[8];
00143 int flag_err_old[8];
00144 int flag_err_rt;
00145 
00146 int flag_ref_enable;
00147 
00148 int flag_data_request[5];
00149 
00150 int MODE_POS_FT_TRANS = 0;
00151 int NN_Control_Flag = 0;
00152 
00153 int cnt_buffer = 0;
00154 
00155 float CUR_CURRENT_mA = 0.0f;
00156 float CUR_TORQUE_NM = 0.0f;
00157 float CUR_TORQUE_NM_PRESS = 0.0f;
00158 
00159 float FORCE_VREF = 0.0f;
00160 float PRES_A_VREF = 0.0f;
00161 float PRES_B_VREF = 0.0f;
00162 
00163 float VALVE_PWM_RAW_FB = 0.0f;
00164 float VALVE_PWM_RAW_FF = 0.0f;
00165 float VALVE_PWM_RAW = 0.0f;
00166 int VALVE_PWM_VALVE_DZ = 0;
00167 
00168 float VALVE_GAIN_LPM_PER_V[10];
00169 float VALVE_POS_VS_PWM[25];
00170 long JOINT_VEL[100];
00171 
00172 int VALVE_MAX_POS;
00173 int VALVE_MIN_POS;
00174 int VALVE_ELECTRIC_CENTER;
00175 int VALVE_POS_NUM;
00176 float VALVE_CENTER_OFFSET;
00177 float VALVE_DZ_MINUS_OFFSET;
00178 float VALVE_DZ_PLUS_OFFSET;
00179 
00180 int TMR3_COUNT_FINDHOME = 0;
00181 int TMR3_COUNT_FLOWRATE = 0;
00182 int TMR3_COUNT_DEADZONE = 0;
00183 int TMR3_COUNT_PRES_NULL = 0;
00184 int TMR3_COUNT_TORQUE_NULL = 0;
00185 int TMR3_COUNT_PRES_CALIB = 0;
00186 int TMR3_COUNT_REFERENCE = 0;
00187 int TMR3_COUNT_JOINT = 0;
00188 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
00189 
00190 float TUNING_TIME = 600.0f;  // sec
00191 
00192 float REFERENCE_FREQ = 1.0f;
00193 float REFERENCE_MAG = 0.0f;
00194 
00195 bool FLAG_FIND_HOME;
00196 
00197 int MODE_JUMP_STATUS;
00198 enum _JUMP_STATUS {
00199     JUMP_NO_ACT = 0,                                //0
00200     JUMP_START,                                //1
00201     JUMP_TAKEOFF,                                  //2
00202     JUMP_FLYING,                                 //3
00203     JUMP_LANDING,                                  //4
00204 };
00205 
00206 
00207 float PRES_A_NULL_pulse = 300.0f;
00208 float PRES_B_NULL_pulse = 300.0f;
00209 float FORCE_NULL_pulse = 3900.0f;
00210 
00211 float Ref_Valve_Pos_Old = 0.0f;
00212 
00213 int VALVE_ID_timer = 0;
00214 int VALVE_DZ_timer = 0;
00215 int VALVE_FR_timer = 0;
00216 //int VALVE_HPL_timer = 0;
00217 int VALVE_POS_TMP = 0;
00218 int JOINT_VEL_TMP = 0;
00219 int DDV_POS_AVG = 0;
00220 int VALVE_POS_AVG[50] = {0};
00221 int VALVE_POS_AVG_OLD = 0;
00222 int data_num = 0;
00223 int ID_index = 0;
00224 int DZ_index = 1;
00225 int ID_index_array[50] = {0};
00226 int first_check = 0;
00227 float init_time = 0.0f;
00228 int DZ_case = 0;
00229 int START_POS = 0;
00230 int FINAL_POS = 0;
00231 int DZ_DIRECTION = 0;
00232 int FIRST_DZ = 0;
00233 int SECOND_DZ = 0;
00234 int DZ_NUM = 0;
00235 int one_period_end = 0;
00236 float Ref_Vel_Test = 0.0f;
00237 long TMR2_FOR_SLOW_LOGGING = 0;
00238 char max_check = 0;
00239 char min_check = 0;
00240 
00241 float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
00242 float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f;
00243 float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
00244 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
00245 float force_ref_filter = 0.0f;
00246 
00247 float CUR_FLOWRATE = 0.0f;
00248 float VALVE_FF_VOLTAGE = 0.0f;
00249 
00250 int pos_plus_end = 0;
00251 int pos_minus_end = 0;
00252 
00253 bool need_enc_init = false;
00254 
00255 int temp_time = 0;
00256 
00257 float CUR_VELOCITY_sum = 0.0f;
00258 float temp_vel_sum = 0.0f;
00259 
00260 int DZ_dir = 0;
00261 int DZ_temp_cnt = 0;
00262 int DZ_temp_cnt2 = 0;
00263 int DZ_end = 2;
00264 int flag_flowrate = 0;
00265 int fl_temp_cnt = 0;
00266 int fl_temp_cnt2 = 0;
00267 int cur_vel_sum = 0;
00268 
00269 int cnt_finddz = 0;
00270 int cnt_vel_finddz = 0;
00271 int flag_finddz = 0;
00272 int FINDDZ_VELOCITY = 0;
00273 int FINDDZ_VELOCITY_OLD = 0;
00274 int FINDDZ_POSITION = 0;
00275 int FINDDZ_POSITION_OLD = 0;
00276 
00277 double temp_VALVE_DEADZONE_PLUS = 0.0f;
00278 double temp_VALVE_DEADZONE_MINUS = 0.0f;
00279 float temp_pos_ref = 0.0f;
00280 float temp_pos_ref_offset = 0.0f;
00281 
00282 
00283 // valve gain
00284 int check_vel_pos_init = 0;
00285 int check_vel_pos_fin = 0;
00286 int check_vel_pos_interv = 0;
00287 int valve_gain_repeat_cnt = 0;
00288 float VALVE_VOLTAGE = 0.0f;
00289 
00290 float freq_fric_tune = 1.0f;
00291 
00292 uint32_t TMR3_COUNT_CAN_TX = 0;
00293 
00294 // Current Control Variables
00295 double I_REF = 0.0f;
00296 double I_REF_fil = 0.0f;
00297 double I_REF_fil_DZ = 0.0f;
00298 double I_ERR = 0.0f;
00299 double I_ERR_INT = 0.0f;
00300 double I_REF_fil_old = 0.0f;
00301 double I_REF_fil_diff = 0.0f;
00302 
00303 // system id
00304 int cnt_sysid = 0;
00305 double freq_sysid_Iref = 0.0f;
00306 
00307 int cnt_freq_test = 0;
00308 int cnt_step_test = 0;
00309 int buffer_data_size = 0;
00310 int cnt_send_buffer = 0;
00311 float freq_test_valve_ref = 1.0f;
00312 float ref_array[10000];
00313 int pos_array[10000];
00314 int flag_every_reference = 0;
00315 
00316 int TMR3_COUNT_IREF = 0;
00317 float CUR_CURRENT = 0.0f;
00318 float u_CUR[3] = {0.0f,0.0f,0.0f};
00319 int FINDHOME_STAGE = 0;
00320 int FINDHOME_INIT = 0;
00321 int FINDHOME_GOTOLIMIT = 1;
00322 int FINDHOME_ZEROPOSE = 2;
00323 
00324 int FINDDZ_STAGE = 0;
00325 int FINDDZ_INIT = 0;
00326 int FINDDZ_START1 = 1;
00327 int FINDDZ_START2 = 2;
00328 int FINDDZ_STOP = 3;
00329 
00330 float alpha_trans = 0.0f;
00331 
00332 float V_out=0.0f;
00333 float V_rem=0.0f; // for anti-windup
00334 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
00335 
00336 float PWM_out=0.0f;
00337 
00338 double K_v = 1.0f; // valve flowrate gain 1
00339 double C_d = 0.16f; // valve flowrate gain 2
00340 
00341 double mV_PER_mA = 600.0f; // current >> voltage
00342 double mV_PER_pulse = 0.6f; // pulse >> voltage
00343 double mA_PER_pulse = 0.001f; // pulse >> current
00344 
00345 int timer_while = 0;
00346 int while_index = 0;
00347 int RL_timer = 0;
00348 
00349 float K_LPF = 0.0f;
00350 float D_LPF = 0.0f;
00351 
00352 float torq_sen_past = 0.0f;
00353 float torq_ref_past = 0.0f;
00354 float output_normalized = 0.0f;
00355 
00356 
00357 /*******************************************************************************
00358  * General math functions
00359  ******************************************************************************/
00360 
00361 
00362 float dabs(float tx)
00363 {
00364     if (tx >= 0.0f)
00365         return tx;
00366     else
00367         return -tx;
00368 }
00369 
00370 float change_int_to_efloat(int input)
00371 {
00372     int i = 0;
00373 
00374     float output = 0;
00375     int vn = (int) ((float) input / 10.0f);
00376     int en = input % 10;
00377 
00378     float temp = 1.;
00379     for (i = 0; i < en; i++)
00380         temp *= 0.1f;
00381 
00382     output = (float) vn*temp;
00383     return output;
00384 }
00385 
00386 void make_delay(void)
00387 {
00388     int i = 0;
00389 
00390     for (i = 0; i < 1000000; i++) {
00391         ;
00392     }
00393 }
00394 
00395 
00396 /*******************************************************************************
00397  * ROM functions
00398  ******************************************************************************/
00399 
00400 void ROM_CALL_DATA(void)
00401 {
00402     BNO = spi_eeprom_read(RID_BNO);
00403     OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
00404     SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
00405     CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
00406     FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
00407     CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
00408     DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
00409     DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
00410     DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
00411     SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
00412     VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
00413     P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
00414     I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
00415     D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
00416     P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
00417     I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
00418     D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
00419     P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
00420     I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
00421     D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
00422     VALVE_DEADZONE_PLUS = (spi_eeprom_read(RID_VALVE_DEADZONE_PLUS));
00423     VALVE_DEADZONE_MINUS = (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS));
00424     VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
00425 //    COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
00426     VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
00427     VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
00428     BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
00429     CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
00430     CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
00431     PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
00432     PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
00433     PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
00434     alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
00435     PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY);
00436     PRES_SUPPLY = PRES_SUPPLY_NOM;
00437     ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
00438     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
00439     STROKE = spi_eeprom_read(RID_STROKE);
00440     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
00441     TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.001f;
00442     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
00443     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
00444     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
00445     HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
00446     HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
00447     FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f;
00448 //    FORCE_VREF = 1.6f;
00449     PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
00450     PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
00451     VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
00452     VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
00453     VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
00454     VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
00455     VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
00456     VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
00457     VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
00458     VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
00459     VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
00460     VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
00461     for(int i=0; i<25; i++) {
00462         VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
00463     }
00464     for(int i=0; i<100; i++) {
00465         JOINT_VEL[i] = ( ((spi_eeprom_read( RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
00466     }
00467     VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
00468     VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
00469     VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
00470     VALVE_ELECTRIC_CENTER = spi_eeprom_read(RID_VALVE_ELECTRIC_CENTER);
00471 
00472 //    K_SPRING = spi_eeprom_read(RID_K_SPRING);
00473 //    D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
00474 
00475 }
00476 
00477 /*******************************************************************************
00478  * ENCODER functions
00479 
00480  ******************************************************************************/
00481 long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0;
00482 long ENC_pulse_offset = 0;
00483 
00484 void ENC_UPDATE(void)
00485 {
00486     ENC_pulse = spi_enc_read(); // Unit : pulse
00487     ENC_pulse_diff = ENC_pulse - ENC_pulse_old;
00488 
00489     pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm
00490     vel.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse_diff * (long)FREQ_10k)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg/s or mm/s
00491 
00492     ENC_pulse_old = ENC_pulse;
00493 }
00494 
00495 void ENC_SET_ZERO(void)
00496 {
00497     spi_enc_set_clear();
00498     pos.Reset();
00499     ENC_pulse_offset = 0;
00500     ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0;
00501 }
00502 
00503 void ENC_SET(long value_e)
00504 {
00505     spi_enc_set_clear();
00506     ENC_pulse_offset = value_e;
00507     pos.Reset();
00508     pos.sen = value_e/ENC_PULSE_PER_POSITION;
00509     ENC_pulse = ENC_pulse_old = value_e;
00510     ENC_pulse_diff = 0;
00511 }
00512 
00513 
00514 
00515