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.0;
00045 int16_t D_DAMPER = 0.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_POS_NUM;
00175 float VALVE_CENTER_OFFSET;
00176 float VALVE_DZ_MINUS_OFFSET;
00177 float VALVE_DZ_PLUS_OFFSET;
00178 
00179 int TMR3_COUNT_FINDHOME = 0;
00180 int TMR3_COUNT_FLOWRATE = 0;
00181 int TMR3_COUNT_DEADZONE = 0;
00182 int TMR3_COUNT_PRES_NULL = 0;
00183 int TMR3_COUNT_TORQUE_NULL = 0;
00184 int TMR3_COUNT_PRES_CALIB = 0;
00185 int TMR3_COUNT_REFERENCE = 0;
00186 int TMR3_COUNT_JOINT = 0;
00187 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
00188 
00189 float TUNING_TIME = 600.0f;  // sec
00190 
00191 float REFERENCE_FREQ = 1.0f;
00192 float REFERENCE_MAG = 0.0f;
00193 
00194 bool FLAG_FIND_HOME;
00195 
00196 int MODE_JUMP_STATUS;
00197 enum _JUMP_STATUS {
00198     JUMP_NO_ACT = 0,                                //0
00199     JUMP_START,                                //1
00200     JUMP_TAKEOFF,                                  //2
00201     JUMP_FLYING,                                 //3
00202     JUMP_LANDING,                                  //4
00203 };
00204 
00205 
00206 float PRES_A_NULL_pulse = 300.0f;
00207 float PRES_B_NULL_pulse = 300.0f;
00208 float FORCE_NULL_pulse = 3900.0f;
00209 
00210 float Ref_Valve_Pos_Old = 0.0f;
00211 
00212 int VALVE_ID_timer = 0;
00213 int VALVE_DZ_timer = 0;
00214 int VALVE_FR_timer = 0;
00215 //int VALVE_HPL_timer = 0;
00216 int VALVE_POS_TMP = 0;
00217 int JOINT_VEL_TMP = 0;
00218 int DDV_POS_AVG = 0;
00219 int VALVE_POS_AVG[50] = {0};
00220 int VALVE_POS_AVG_OLD = 0;
00221 int data_num = 0;
00222 int ID_index = 0;
00223 int DZ_index = 1;
00224 int ID_index_array[50] = {0};
00225 int first_check = 0;
00226 float init_time = 0.0f;
00227 int DZ_case = 0;
00228 int START_POS = 0;
00229 int FINAL_POS = 0;
00230 int DZ_DIRECTION = 0;
00231 int FIRST_DZ = 0;
00232 int SECOND_DZ = 0;
00233 int DZ_NUM = 0;
00234 int one_period_end = 0;
00235 float Ref_Vel_Test = 0.0f;
00236 long TMR2_FOR_SLOW_LOGGING = 0;
00237 char max_check = 0;
00238 char min_check = 0;
00239 
00240 float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
00241 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;
00242 float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
00243 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
00244 
00245 float CUR_FLOWRATE = 0.0f;
00246 float VALVE_FF_VOLTAGE = 0.0f;
00247 
00248 int pos_plus_end = 0;
00249 int pos_minus_end = 0;
00250 
00251 bool need_enc_init = false;
00252 
00253 int temp_time = 0;
00254 
00255 float CUR_VELOCITY_sum = 0.0f;
00256 float temp_vel_sum = 0.0f;
00257 
00258 int DZ_dir = 0;
00259 int DZ_temp_cnt = 0;
00260 int DZ_temp_cnt2 = 0;
00261 int DZ_end = 2;
00262 int flag_flowrate = 0;
00263 int fl_temp_cnt = 0;
00264 int fl_temp_cnt2 = 0;
00265 int cur_vel_sum = 0;
00266 
00267 int cnt_finddz = 0;
00268 int cnt_vel_finddz = 0;
00269 int flag_finddz = 0;
00270 int FINDDZ_VELOCITY = 0;
00271 int FINDDZ_VELOCITY_OLD = 0;
00272 int FINDDZ_POSITION = 0;
00273 int FINDDZ_POSITION_OLD = 0;
00274 
00275 double temp_VALVE_DEADZONE_PLUS = 0.0f;
00276 double temp_VALVE_DEADZONE_MINUS = 0.0f;
00277 float temp_pos_ref = 0.0f;
00278 float temp_pos_ref_offset = 0.0f;
00279 
00280 
00281 // valve gain
00282 int check_vel_pos_init = 0;
00283 int check_vel_pos_fin = 0;
00284 int check_vel_pos_interv = 0;
00285 int valve_gain_repeat_cnt = 0;
00286 float VALVE_VOLTAGE = 0.0f;
00287 
00288 float freq_fric_tune = 1.0f;
00289 
00290 uint32_t TMR3_COUNT_CAN_TX = 0;
00291 
00292 // Current Control Variables
00293 double I_REF = 0.0f;
00294 double I_REF_fil = 0.0f;
00295 double I_REF_fil_DZ = 0.0f;
00296 double I_ERR = 0.0f;
00297 double I_ERR_INT = 0.0f;
00298 double I_REF_fil_old = 0.0f;
00299 double I_REF_fil_diff = 0.0f;
00300 
00301 // system id
00302 int cnt_sysid = 0;
00303 double freq_sysid_Iref = 0.0f;
00304 
00305 int cnt_freq_test = 0;
00306 int cnt_step_test = 0;
00307 int buffer_data_size = 0;
00308 int cnt_send_buffer = 0;
00309 float freq_test_valve_ref = 1.0f;
00310 float ref_array[10000];
00311 int pos_array[10000];
00312 int flag_every_reference = 0;
00313 
00314 int TMR3_COUNT_IREF = 0;
00315 float CUR_CURRENT = 0.0f;
00316 float u_CUR[3] = {0.0f,0.0f,0.0f};
00317 int FINDHOME_STAGE = 0;
00318 int FINDHOME_INIT = 0;
00319 int FINDHOME_GOTOLIMIT = 1;
00320 int FINDHOME_ZEROPOSE = 2;
00321 
00322 int FINDDZ_STAGE = 0;
00323 int FINDDZ_INIT = 0;
00324 int FINDDZ_START1 = 1;
00325 int FINDDZ_START2 = 2;
00326 int FINDDZ_STOP = 3;
00327 
00328 float alpha_trans = 0.0f;
00329 
00330 float V_out=0.0f;
00331 float V_rem=0.0f; // for anti-windup
00332 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
00333 
00334 float PWM_out=0.0f;
00335 
00336 double K_v = 1.0f; // valve flowrate gain 1
00337 double C_d = 0.16f; // valve flowrate gain 2
00338 
00339 double mV_PER_mA = 600.0f; // current >> voltage
00340 double mV_PER_pulse = 0.6f; // pulse >> voltage
00341 double mA_PER_pulse = 0.001f; // pulse >> current
00342 
00343 int timer_while = 0;
00344 int while_index = 0;
00345 int RL_timer = 0;
00346 
00347 float K_LPF = 0.0f;
00348 float D_LPF = 0.0f;
00349 
00350 float torq_sen_past = 0.0f;
00351 float torq_ref_past = 0.0f;
00352 float output_normalized = 0.0f;
00353 
00354 
00355 /*******************************************************************************
00356  * General math functions
00357  ******************************************************************************/
00358 
00359 
00360 float dabs(float tx)
00361 {
00362     if (tx >= 0.0f)
00363         return tx;
00364     else
00365         return -tx;
00366 }
00367 
00368 float change_int_to_efloat(int input)
00369 {
00370     int i = 0;
00371 
00372     float output = 0;
00373     int vn = (int) ((float) input / 10.0f);
00374     int en = input % 10;
00375 
00376     float temp = 1.;
00377     for (i = 0; i < en; i++)
00378         temp *= 0.1f;
00379 
00380     output = (float) vn*temp;
00381     return output;
00382 }
00383 
00384 void make_delay(void)
00385 {
00386     int i = 0;
00387 
00388     for (i = 0; i < 1000000; i++) {
00389         ;
00390     }
00391 }
00392 
00393 
00394 /*******************************************************************************
00395  * ROM functions
00396  ******************************************************************************/
00397 
00398 void ROM_CALL_DATA(void)
00399 {
00400     BNO = spi_eeprom_read(RID_BNO);
00401     BNO = 11;
00402     OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
00403     SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
00404     CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
00405     FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
00406     CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
00407     DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
00408     DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
00409     DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
00410     SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
00411     VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
00412     P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
00413     I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
00414     D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
00415     P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
00416     I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
00417     D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
00418     P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
00419     I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
00420     D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
00421     VALVE_DEADZONE_PLUS = (spi_eeprom_read(RID_VALVE_DEADZONE_PLUS));
00422     VALVE_DEADZONE_MINUS = (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS));
00423     VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
00424     COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
00425     VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
00426     VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
00427     BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
00428     CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
00429     CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
00430     PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
00431     PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
00432     PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
00433     alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
00434     PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY);
00435     PRES_SUPPLY = PRES_SUPPLY_NOM;
00436     ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
00437     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
00438     STROKE = spi_eeprom_read(RID_STROKE);
00439     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
00440     TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
00441     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
00442 //    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00443     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
00444 //    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00445     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
00446     HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
00447     HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
00448     FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f;
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 
00471 //    K_SPRING = spi_eeprom_read(RID_K_SPRING);
00472 //    D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
00473 
00474 }
00475 
00476 /*******************************************************************************
00477  * ENCODER functions
00478 
00479  ******************************************************************************/
00480 long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0;
00481 long ENC_pulse_offset = 0;
00482 
00483 void ENC_UPDATE(void)
00484 {
00485     ENC_pulse = spi_enc_read(); // Unit : pulse
00486     ENC_pulse_diff = ENC_pulse - ENC_pulse_old;
00487 
00488     pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm
00489     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
00490 
00491     ENC_pulse_old = ENC_pulse;
00492 }
00493 
00494 void ENC_SET_ZERO(void)
00495 {
00496     spi_enc_set_clear();
00497     pos.Reset();
00498     ENC_pulse_offset = 0;
00499     ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0;
00500 }
00501 
00502 void ENC_SET(long value_e)
00503 {
00504     spi_enc_set_clear();
00505     ENC_pulse_offset = value_e;
00506     pos.Reset();
00507     pos.sen = value_e/ENC_PULSE_PER_POSITION;
00508     ENC_pulse = ENC_pulse_old = value_e;
00509     ENC_pulse_diff = 0;
00510 }
00511 
00512 
00513 
00514