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 CONTROL_UTILITY_MODE = 0;
00018 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
00019 uint8_t FLAG_VALVE_DEADZONE = 0;
00020 uint8_t REFERENCE_MODE = 0;
00021 int16_t CAN_FREQ = 500;
00022 int16_t DIR_JOINT_ENC = 0;
00023 int16_t DIR_VALVE = 0;
00024 int16_t DIR_VALVE_ENC = 0;
00025 
00026 float SUPPLY_VOLTAGE = 12.0f;
00027 float VALVE_VOLTAGE_LIMIT = 12.0f;  //v
00028 
00029 float P_GAIN_VALVE_POSITION = 0.0f;
00030 float I_GAIN_VALVE_POSITION= 0.0f;
00031 float D_GAIN_VALVE_POSITION= 0.0f;
00032 float P_GAIN_JOINT_POSITION = 0.0f;
00033 float I_GAIN_JOINT_POSITION = 0.0f;
00034 float D_GAIN_JOINT_POSITION = 0.0f;
00035 float P_GAIN_JOINT_TORQUE = 0.0f;
00036 float I_GAIN_JOINT_TORQUE = 0.0f;
00037 float D_GAIN_JOINT_TORQUE = 0.0f;
00038 float P_GAIN_JOINT_TORQUE_FF = 0.0f;
00039 float I_GAIN_JOINT_TORQUE_FF = 0.0f;
00040 float D_GAIN_JOINT_TORQUE_FF = 0.0f;
00041 
00042 int16_t K_SPRING = 0.0;
00043 int16_t D_DAMPER = 0.0;
00044 
00045 int16_t flag_delay_test = 0;
00046 
00047 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
00048 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
00049 //float D_GAIN_VALVE_POSITION_OPP= 0.0f;
00050 //float P_GAIN_JOINT_POSITION_OPP = 0.0f;
00051 //float I_GAIN_JOINT_POSITION_OPP = 0.0f;
00052 //float D_GAIN_JOINT_POSITION_OPP = 0.0f;
00053 //float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
00054 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
00055 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
00056 
00057 float VALVE_DEADZONE_PLUS;
00058 float VALVE_DEADZONE_MINUS;
00059 
00060 int16_t VELOCITY_COMP_GAIN;
00061 int16_t COMPLIANCE_GAIN;
00062 
00063 int16_t VALVE_CENTER;
00064 
00065 int16_t VALVE_FF;
00066 
00067 int16_t BULK_MODULUS;
00068 
00069 int16_t CHAMBER_VOLUME_A;
00070 int16_t CHAMBER_VOLUME_B;
00071 
00072 int16_t PISTON_AREA_A;
00073 int16_t PISTON_AREA_B;
00074 float PISTON_AREA_alpha;
00075 float alpha3 = 1.0f;
00076 
00077 
00078 int16_t PRES_SUPPLY;
00079 int16_t PRES_RETURN;
00080 
00081 int16_t ENC_LIMIT_PLUS;
00082 int16_t ENC_LIMIT_MINUS;
00083 
00084 int16_t STROKE;
00085 
00086 float Amm = 236.4f;
00087 float beta = 1300000000.0f;
00088 float Ps = 10000000.0f; //100bar = 100*10^5 Pa
00089 float Pt = 0.0f;    // 0bar = 0Pa
00090 //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
00091 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
00092 float a_hat = -13707631.7f;
00093 float V_adapt = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
00094 float x_4_des_old = 0.0f;
00095 
00096 //int16_t VALVE_LIMIT_PLUS;
00097 //int16_t VALVE_LIMIT_MINUS;
00098 
00099 float ENC_PULSE_PER_POSITION;
00100 float TORQUE_SENSOR_PULSE_PER_TORQUE;
00101 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
00102 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
00103 
00104 int HOMEPOS_OFFSET;
00105 int HOMEPOS_VALVE_OPENING;
00106 
00107 float FRICTION;
00108 float REF_PERIOD;
00109 float REF_MAG;
00110 int REF_NUM;
00111 
00112 
00113 float DAC_REF;
00114 float DAC_RESOL;
00115 
00116 int REF_POSITION;
00117 int REF_VELOCITY;
00118 int16_t REF_TORQUE;
00119 int16_t REF_PRES_DIFF;
00120 int16_t REF_PWM;
00121 int16_t REF_VALVE_POSITION;
00122 int16_t REF_CURRENT;
00123 
00124 int REF_MOVE_TIME_5k;
00125 int INIT_REF_PWM;
00126 int INIT_REF_VALVE_POS;
00127 int INIT_REF_POS;
00128 int INIT_REF_VEL;
00129 int INIT_REF_TORQUE;
00130 int INIT_REF_PRES_DIFF;
00131 int INIT_REF_CURRENT;
00132 
00133 int CUR_POSITION;
00134 int CUR_VELOCITY;
00135 float CUR_TORQUE;
00136 float CUR_PRES_A;
00137 float CUR_PRES_B;
00138 int CUR_VALVE_POSITION;
00139 
00140 unsigned int    TMR2_COUNT_LED1;
00141 unsigned int    TMR2_COUNT_LED2;
00142 unsigned int    TMR2_COUNT_CAN_TX = 0;
00143 unsigned int    TMR3_COUNT_TEST = 0;
00144 
00145 int num_err;
00146 int flag_err[8];
00147 int flag_err_old[8];
00148 int flag_err_rt;
00149 
00150 int flag_ref_enable;
00151 
00152 int flag_data_request[5];
00153 
00154 int MODE_POS_FT_TRANS = 0;
00155 int NN_Control_Flag = 0;
00156 
00157 int cnt_buffer = 0;
00158 
00159 float CUR_CURRENT_mA = 0.0f;
00160 float CUR_PRES_A_BAR = 0.0f;
00161 float CUR_PRES_B_BAR = 0.0f;
00162 float CUR_TORQUE_NM = 0.0f;
00163 float CUR_TORQUE_NM_PRESS = 0.0f;
00164 
00165 float PRES_A_VREF = 0.0f;
00166 float PRES_B_VREF = 0.0f;
00167 float TORQUE_VREF = 0.0f;
00168 
00169 float VALVE_PWM_RAW_FB = 0.0f;
00170 float VALVE_PWM_RAW_FF = 0.0f;
00171 float VALVE_PWM_RAW = 0.0f;
00172 int VALVE_PWM_VALVE_DZ = 0;
00173 
00174 float VALVE_GAIN_LPM_PER_V[10];
00175 float VALVE_POS_VS_PWM[25];
00176 long JOINT_VEL[100];
00177 
00178 int VALVE_MAX_POS;
00179 int VALVE_MIN_POS;
00180 int VALVE_POS_NUM;
00181 float VALVE_CENTER_OFFSET;
00182 float VALVE_DZ_MINUS_OFFSET;
00183 float VALVE_DZ_PLUS_OFFSET;
00184 
00185 int TMR3_COUNT_FINDHOME = 0;
00186 int TMR3_COUNT_FLOWRATE = 0;
00187 int TMR3_COUNT_DEADZONE = 0;
00188 int TMR3_COUNT_PRES_NULL = 0;
00189 int TMR3_COUNT_TORQUE_NULL = 0;
00190 int TMR3_COUNT_PRES_CALIB = 0;
00191 int TMR3_COUNT_REFERENCE = 0;
00192 int TMR3_COUNT_JOINT = 0;
00193 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
00194 
00195 float TUNING_TIME = 600.0f;  // sec
00196 
00197 float REFERENCE_FREQ = 1.0f;
00198 float REFERENCE_MAG = 0.0f;
00199 
00200 bool FLAG_FIND_HOME;
00201 
00202 int MODE_JUMP_STATUS;
00203 enum _JUMP_STATUS {
00204     JUMP_NO_ACT = 0,                                //0
00205     JUMP_START,                                //1
00206     JUMP_TAKEOFF,                                  //2
00207     JUMP_FLYING,                                 //3
00208     JUMP_LANDING,                                  //4
00209 };
00210 
00211 float CUR_PRES_DIFF_BAR = 0.0f;
00212 float CUR_PRES_A_sum = 0.0f;
00213 float CUR_PRES_B_sum = 0.0f;
00214 float CUR_PRES_A_mean = 0.0f;
00215 float CUR_PRES_B_mean = 0.0f;
00216 float CUR_TORQUE_sum = 0.0f;
00217 float CUR_TORQUE_mean = 0.0f;
00218 float PRES_A_NULL = 300.0f;
00219 float PRES_B_NULL = 300.0f;
00220 float TORQUE_NULL = 3900.0f;
00221 
00222 float Ref_Valve_Pos_Old = 0.0f;
00223 
00224 int VALVE_ID_timer = 0;
00225 int VALVE_DZ_timer = 0;
00226 int VALVE_FR_timer = 0;
00227 //int VALVE_HPL_timer = 0;
00228 int VALVE_POS_TMP = 0;
00229 int JOINT_VEL_TMP = 0;
00230 int DDV_POS_AVG = 0;
00231 int VALVE_POS_AVG[50] = {0};
00232 int VALVE_POS_AVG_OLD = 0;
00233 int data_num = 0;
00234 int ID_index = 0;
00235 int DZ_index = 1;
00236 int ID_index_array[50] = {0};
00237 int first_check = 0;
00238 float init_time = 0.0f;
00239 int DZ_case = 0;
00240 int START_POS = 0;
00241 int FINAL_POS = 0;
00242 int DZ_DIRECTION = 0;
00243 int FIRST_DZ = 0;
00244 int SECOND_DZ = 0;
00245 int DZ_NUM = 0;
00246 int one_period_end = 0;
00247 float Ref_Vel_Test = 0.0f;
00248 long TMR2_FOR_SLOW_LOGGING = 0;
00249 char max_check = 0;
00250 char min_check = 0;
00251 
00252 float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f;
00253 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;
00254 float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f;
00255 float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f;
00256 
00257 float CUR_FLOWRATE = 0.0f;
00258 float VALVE_FF_VOLTAGE = 0.0f;
00259 
00260 int pos_plus_end = 0;
00261 int pos_minus_end = 0;
00262 
00263 bool need_enc_init = false;
00264 
00265 int temp_time = 0;
00266 
00267 float CUR_VELOCITY_sum = 0.0f;
00268 float temp_vel_sum = 0.0f;
00269 
00270 int DZ_dir = 0;
00271 int DZ_temp_cnt = 0;
00272 int DZ_temp_cnt2 = 0;
00273 int DZ_end = 2;
00274 int flag_flowrate = 0;
00275 int fl_temp_cnt = 0;
00276 int fl_temp_cnt2 = 0;
00277 int cur_vel_sum = 0;
00278 
00279 float Cur_Valve_Open_pulse = 0.0f;
00280 
00281 // find home
00282 int CUR_VELOCITY_OLD = 0;
00283 int cnt_findhome = 0;
00284 int cnt_vel_findhome = 0;
00285 int FINDHOME_VELOCITY = 0;
00286 int FINDHOME_VELOCITY_OLD = 0;
00287 int FINDHOME_POSITION = 0;
00288 int FINDHOME_POSITION_OLD = 0;
00289 
00290 // valve gain
00291 int check_vel_pos_init = 0;
00292 int check_vel_pos_fin = 0;
00293 int check_vel_pos_interv = 0;
00294 int valve_gain_repeat_cnt = 0;
00295 float VALVE_VOLTAGE = 0.0f;
00296 
00297 float freq_fric_tune = 1.0f;
00298 
00299 uint32_t TMR3_COUNT_CAN_TX = 0;
00300 
00301 // Current Control Variables
00302 double I_REF = 0.0f;
00303 double I_REF_fil = 0.0f;
00304 double I_ERR = 0.0f;
00305 double I_ERR_INT = 0.0f;
00306 double I_REF_fil_old = 0.0f;
00307 double I_REF_fil_diff = 0.0f;
00308 
00309 // system id
00310 int cnt_sysid = 0;
00311 double freq_sysid_Iref = 0.0f;
00312 
00313 int cnt_freq_test = 0;
00314 int cnt_step_test = 0;
00315 int buffer_data_size = 0;
00316 int cnt_send_buffer = 0;
00317 float freq_test_valve_ref = 1.0f;
00318 float ref_array[10000];
00319 int pos_array[10000];
00320 int flag_every_reference = 0;
00321 
00322 int TMR3_COUNT_IREF = 0;
00323 float CUR_CURRENT = 0.0f;
00324 float u_CUR[3] = {0.0f,0.0f,0.0f};
00325 int FINDHOME_STAGE = 0;
00326 int FINDHOME_INIT = 0;
00327 int FINDHOME_GOTOLIMIT = 1;
00328 int FINDHOME_ZEROPOSE = 2;
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 = 0.0f; // valve flowrate gain
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 /*******************************************************************************
00396  * ROM functions
00397  ******************************************************************************/
00398 
00399 void ROM_CALL_DATA(void)
00400 {
00401     BNO = spi_eeprom_read(RID_BNO);
00402     BNO = 2;
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 = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
00423     VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
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 = spi_eeprom_read(RID_PRES_SUPPLY);
00436     PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
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.0001f;
00442     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
00443 //    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00444     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
00445 //    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00446     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
00447     HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
00448     HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
00449     TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
00450     PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
00451     PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
00452     VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
00453     VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
00454     VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
00455     VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
00456     VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
00457     VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
00458     VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
00459     VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
00460     VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
00461     VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
00462     for(int i=0; i<25; i++) {
00463         VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
00464     }
00465     for(int i=0; i<100; i++) {
00466         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 ) ;
00467     }
00468     VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
00469     VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
00470     VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
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 // A-KHA
00482 #define     KF_G1_11    0.083920206005350f
00483 #define     KF_G1_12    0.000013905329560f
00484 #define     KF_G1_21    -0.000575742328210f
00485 #define     KF_G1_22    0.799999939711725f
00486 // K
00487 #define     KF_G2_11    0.916079793994650f
00488 #define     KF_G2_12    0.000002878711641f
00489 #define     KF_G2_21    0.000575742328210f
00490 #define     KF_G2_22    0.199999945139809f
00491 
00492 float KF_Y_11 = 0.0f;
00493 float KF_Y_21 = 0.0f;
00494 float KF_X_11 = 0.0f;
00495 float KF_X_21 = 0.0f;
00496 
00497 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
00498 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
00499 long enc_offset = 0;
00500 
00501 void ENC_UPDATE(void)
00502 {
00503 
00504     ENC_pos_cur = spi_enc_read();
00505     ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
00506 
00507     //Low Pass Filter
00508 
00509     double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
00510     double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
00511 
00512     double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
00513     pos.sen = NEW_POSITION;
00514     vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
00515 
00516     ENC_pos_old = ENC_pos_cur;
00517 }
00518 
00519 void ENC_SET_ZERO(void)
00520 {
00521 
00522     spi_enc_set_clear();
00523     CUR_POSITION = 0;
00524     ENC_pos_old = ENC_pos_cur = 0;
00525 
00526 }
00527 
00528 void ENC_SET(int32_t value_e)
00529 {
00530     spi_enc_set_clear();
00531     enc_offset = value_e;
00532     CUR_POSITION = value_e;
00533     ENC_pos_old = ENC_pos_cur = value_e;
00534 
00535 }
00536 
00537 
00538 
00539