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 int Rom_Sector = 6;
00009 //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
00010 
00011 /*******************************************************************************
00012  * VARIABLE
00013  ******************************************************************************/
00014 
00015 // Board Information
00016 uint8_t BNO = 0;
00017 uint8_t CONTROL_MODE = 0;
00018 uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin)
00019 uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure)
00020 uint8_t CONTROL_UTILITY_MODE = 0;
00021 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
00022 uint8_t FLAG_VALVE_DEADZONE = 0;
00023 uint8_t REFERENCE_MODE = 0;
00024 int16_t CAN_FREQ = 500;
00025 int16_t DIR_JOINT_ENC = 0;
00026 int16_t DIR_VALVE = 0;
00027 int16_t DIR_VALVE_ENC = 0;
00028 
00029 float SUPPLY_VOLTAGE = 12.0f;
00030 float VALVE_VOLTAGE_LIMIT = 12.0f;  //v
00031 
00032 float P_GAIN_VALVE_POSITION = 0.0f;
00033 float I_GAIN_VALVE_POSITION= 0.0f;
00034 float D_GAIN_VALVE_POSITION= 0.0f;
00035 float P_GAIN_JOINT_POSITION = 0.0f;
00036 float I_GAIN_JOINT_POSITION = 0.0f;
00037 float D_GAIN_JOINT_POSITION = 0.0f;
00038 float P_GAIN_JOINT_TORQUE = 0.0f;
00039 float I_GAIN_JOINT_TORQUE = 0.0f;
00040 float D_GAIN_JOINT_TORQUE = 0.0f;
00041 
00042 float K_SPRING = 0.0f;
00043 float D_DAMPER = 0.0f;
00044 int16_t flag_delay_test = 0;
00045 
00046 //float P_GAIN_VALVE_POSITION_OPP = 0.0f;
00047 //float I_GAIN_VALVE_POSITION_OPP= 0.0f;
00048 //float D_GAIN_VALVE_POSITION_OPP= 0.0f;
00049 //float P_GAIN_JOINT_POSITION_OPP = 0.0f;
00050 //float I_GAIN_JOINT_POSITION_OPP = 0.0f;
00051 //float D_GAIN_JOINT_POSITION_OPP = 0.0f;
00052 //float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
00053 //float I_GAIN_JOINT_TORQUE_OPP = 0.0;
00054 //float D_GAIN_JOINT_TORQUE_OPP = 0.0;
00055 
00056 float VALVE_DEADZONE_PLUS;
00057 float VALVE_DEADZONE_MINUS;
00058 
00059 int16_t VELOCITY_COMP_GAIN;
00060 int16_t COMPLIANCE_GAIN;
00061 
00062 int16_t VALVE_CENTER;
00063 
00064 int16_t VALVE_FF;
00065 
00066 int16_t BULK_MODULUS;
00067 
00068 int16_t CHAMBER_VOLUME_A;
00069 int16_t CHAMBER_VOLUME_B;
00070 
00071 int16_t PISTON_AREA_A;
00072 int16_t PISTON_AREA_B;
00073 float PISTON_AREA_alpha;
00074 float alpha3 = 1.0f;
00075 
00076 
00077 int16_t PRES_SUPPLY;
00078 int16_t PRES_RETURN;
00079 
00080 int16_t ENC_LIMIT_PLUS;
00081 int16_t ENC_LIMIT_MINUS;
00082 
00083 int16_t STROKE;
00084 
00085 
00086 //int16_t VALVE_LIMIT_PLUS;
00087 //int16_t VALVE_LIMIT_MINUS;
00088 
00089 float ENC_PULSE_PER_POSITION;
00090 float TORQUE_SENSOR_PULSE_PER_TORQUE;
00091 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
00092 float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
00093 
00094 int HOMEPOS_OFFSET;
00095 int HOMEPOS_VALVE_OPENING;
00096 
00097 float FRICTION;
00098 float REF_PERIOD;
00099 float REF_MAG;
00100 int REF_NUM;
00101 
00102 
00103 float DAC_REF;
00104 float DAC_RESOL;
00105 
00106 int REF_POSITION;
00107 int REF_VELOCITY;
00108 int16_t REF_TORQUE;
00109 int16_t REF_PRES_DIFF;
00110 int16_t REF_PWM;
00111 int16_t REF_VALVE_POSITION;
00112 int16_t REF_CURRENT;
00113 
00114 int REF_MOVE_TIME_5k;
00115 int INIT_REF_PWM;
00116 int INIT_REF_VALVE_POS;
00117 int INIT_REF_POS;
00118 int INIT_REF_VEL;
00119 int INIT_REF_TORQUE;
00120 int INIT_REF_PRES_DIFF;
00121 int INIT_REF_CURRENT;
00122 
00123 int CUR_POSITION;
00124 int CUR_VELOCITY;
00125 float CUR_TORQUE;
00126 float CUR_PRES_A;
00127 float CUR_PRES_B;
00128 int CUR_VALVE_POSITION;
00129 
00130 unsigned int    TMR2_COUNT_LED1;
00131 unsigned int    TMR2_COUNT_LED2;
00132 unsigned int    TMR2_COUNT_CAN_TX = 0;
00133 unsigned int    TMR3_COUNT_TEST = 0;
00134 
00135 int num_err;
00136 int flag_err[8];
00137 int flag_err_old[8];
00138 int flag_err_rt;
00139 
00140 int flag_ref_enable;
00141 
00142 int flag_data_request[5];
00143 
00144 int MODE_POS_FT_TRANS = 0;
00145 
00146 float CUR_CURRENT_mA = 0.0f;
00147 float CUR_PRES_A_BAR = 0.0f;
00148 float CUR_PRES_B_BAR = 0.0f;
00149 float CUR_TORQUE_NM = 0.0f;
00150 float CUR_TORQUE_NM_PRESS = 0.0f;
00151 
00152 float PRES_A_VREF = 0.0f;
00153 float PRES_B_VREF = 0.0f;
00154 float TORQUE_VREF = 0.0f;
00155 
00156 float VALVE_PWM_RAW_FB = 0.0f;
00157 float VALVE_PWM_RAW_FF = 0.0f;
00158 float VALVE_PWM_RAW = 0.0f;
00159 int VALVE_PWM_VALVE_DZ = 0;
00160 
00161 float VALVE_GAIN_LPM_PER_V[10];
00162 float VALVE_POS_VS_PWM[25];
00163 long JOINT_VEL[100];
00164 
00165 int VALVE_MAX_POS;
00166 int VALVE_MIN_POS;
00167 int VALVE_POS_NUM;
00168 float VALVE_CENTER_OFFSET;
00169 float VALVE_DZ_MINUS_OFFSET;
00170 float VALVE_DZ_PLUS_OFFSET;
00171 
00172 int TMR3_COUNT_FINDHOME = 0;
00173 int TMR3_COUNT_FLOWRATE = 0;
00174 int TMR3_COUNT_DEADZONE = 0;
00175 int TMR3_COUNT_PRES_NULL = 0;
00176 int TMR3_COUNT_TORQUE_NULL = 0;
00177 int TMR3_COUNT_PRES_CALIB = 0;
00178 int TMR3_COUNT_REFERENCE = 0;
00179 int TMR3_COUNT_JOINT = 0;
00180 int TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
00181 
00182 float TUNING_TIME = 600.0f;  // sec
00183 
00184 float REFERENCE_FREQ = 1.0f;
00185 float REFERENCE_MAG = 0.0f;
00186 
00187 bool FLAG_FIND_HOME;
00188 
00189 int MODE_JUMP_STATUS;
00190 enum _JUMP_STATUS {
00191     JUMP_NO_ACT = 0,                                //0
00192     JUMP_START,                                //1
00193     JUMP_TAKEOFF,                                  //2
00194     JUMP_FLYING,                                 //3
00195     JUMP_LANDING,                                  //4
00196 };
00197 
00198 float CUR_PRES_DIFF_BAR = 0.0f;
00199 float CUR_PRES_A_sum = 0.0f;
00200 float CUR_PRES_B_sum = 0.0f;
00201 float CUR_PRES_A_mean = 0.0f;
00202 float CUR_PRES_B_mean = 0.0f;
00203 float CUR_TORQUE_sum = 0.0f;
00204 float CUR_TORQUE_mean = 0.0f;
00205 float PRES_A_NULL = 300.0f;
00206 float PRES_B_NULL = 300.0f;
00207 float TORQUE_NULL = 3900.0f;
00208 
00209 float Ref_Valve_Pos_Old = 0.0f;
00210 
00211 int VALVE_ID_timer = 0;
00212 int VALVE_DZ_timer = 0;
00213 int VALVE_FR_timer = 0;
00214 //int VALVE_HPL_timer = 0;
00215 int VALVE_POS_TMP = 0;
00216 int JOINT_VEL_TMP = 0;
00217 int DDV_POS_AVG = 0;
00218 int VALVE_POS_AVG[50] = {0};
00219 int VALVE_POS_AVG_OLD = 0;
00220 int data_num = 0;
00221 int ID_index = 0;
00222 int DZ_index = 1;
00223 int ID_index_array[50] = {0};
00224 int first_check = 0;
00225 float init_time = 0.0f;
00226 int DZ_case = 0;
00227 int START_POS = 0;
00228 int FINAL_POS = 0;
00229 int DZ_DIRECTION = 0;
00230 int FIRST_DZ = 0;
00231 int SECOND_DZ = 0;
00232 int DZ_NUM = 0;
00233 int one_period_end = 0;
00234 float Ref_Vel_Test = 0.0f;
00235 long TMR2_FOR_SLOW_LOGGING = 0;
00236 //int velcount = 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 float Cur_Valve_Open_pulse = 0.0f;
00268 
00269 // find home
00270 int CUR_VELOCITY_OLD = 0;
00271 int cnt_findhome = 0;
00272 int cnt_vel_findhome = 0;
00273 int cnt_findhome_wait = 0;
00274 int FINDHOME_VELOCITY = 0;
00275 int FINDHOME_VELOCITY_OLD = 0;
00276 int FINDHOME_POSITION = 0;
00277 int FINDHOME_POSITION_OLD = 0;
00278 
00279 // valve gain
00280 int check_vel_pos_init = 0;
00281 int check_vel_pos_fin = 0;
00282 int check_vel_pos_interv = 0;
00283 int valve_gain_repeat_cnt = 0;
00284 float VALVE_VOLTAGE = 0.0f;
00285 
00286 float freq_fric_tune = 1.0f;
00287 
00288 uint32_t TMR3_COUNT_CAN_TX = 0;
00289 
00290 // Current Control Variables
00291 double I_REF = 0.0f;
00292 double I_REF_fil = 0.0f;
00293 double I_ERR = 0.0f;
00294 double I_ERR_INT = 0.0f;
00295 double I_REF_fil_old = 0.0f;
00296 double I_REF_fil_diff = 0.0f;
00297 
00298 // system id
00299 int cnt_sysid = 0;
00300 double freq_sysid_Iref = 0.0f;
00301 
00302 int TMR3_COUNT_IREF = 0;
00303 float CUR_CURRENT = 0.0f;
00304 float u_CUR[3] = {0.0f,0.0f,0.0f};
00305 int FINDHOME_STAGE = 0;
00306 int FINDHOME_INIT = 0;
00307 int FINDHOME_GOTOLIMIT = 1;
00308 int FINDHOME_ZEROPOSE = 2;
00309 int FINDHOME_WAIT     = 3;
00310 
00311 float alpha_trans = 0.0f;
00312 
00313 float V_out=0.0f;
00314 float V_rem=0.0f; // for anti-windup
00315 float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
00316 
00317 float PWM_out=0.0f;
00318 
00319 double K_v = 0.0f; // valve flowrate gain
00320 double mV_PER_mA = 600.0f; // current >> voltage
00321 double mV_PER_pulse = 0.6f; // pulse >> voltage
00322 double mA_PER_pulse = 0.001f; // pulse >> current
00323 
00324 int timer_while = 0;
00325 int while_index = 0;
00326 
00327 float K_LPF = 0.0f;
00328 float D_LPF = 0.0f;
00329 
00330 //int h1, h2, h3, h4, h5, h6;
00331 
00332 /*******************************************************************************
00333  * General math functions
00334  ******************************************************************************/
00335 
00336 
00337 float dabs(float tx)
00338 {
00339     if (tx >= 0.0f)
00340         return tx;
00341     else
00342         return -tx;
00343 }
00344 
00345 float change_int_to_efloat(int input)
00346 {
00347     int i = 0;
00348 
00349     float output = 0;
00350     int vn = (int) ((float) input / 10.0f);
00351     int en = input % 10;
00352 
00353     float temp = 1.;
00354     for (i = 0; i < en; i++)
00355         temp *= 0.1f;
00356 
00357     output = (float) vn*temp;
00358     return output;
00359 }
00360 
00361 void make_delay(void)
00362 {
00363     int i = 0;
00364 
00365     for (i = 0; i < 100000; i++) {
00366         ;
00367     }
00368 
00369 }
00370 
00371 
00372 /*******************************************************************************
00373  * ROM functions
00374  ******************************************************************************/
00375 void ROM_RESET_DATA(void)
00376 {
00377     FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
00378     if (!writer.ready()) writer.open();
00379 //    BNO = 11;
00380     writer.write(RID_BNO,(int) BNO);           // write at address, 쓸때도 4byte씩 씀
00381     writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
00382     writer.write(RID_SENSING_MODE, (int) SENSING_MODE);
00383     writer.write(RID_CURRENT_CONTROL_MODE, (int) CURRENT_CONTROL_MODE);
00384     writer.write(RID_FLAG_VALVE_DEADZONE, (int) FLAG_VALVE_DEADZONE);
00385     writer.write(RID_CAN_FREQ,(int) CAN_FREQ);
00386     writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
00387     writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
00388     writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
00389     writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f));
00390     writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f));
00391     writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
00392     writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
00393     writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
00394     writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION);
00395     writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION);
00396     writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION);
00397     writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE);
00398     writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE);
00399     writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE);
00400     writer.write(RID_VALVE_DEADZONE_PLUS,(int) (VALVE_DEADZONE_PLUS * 10.0f));
00401     writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f));
00402     writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
00403     writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
00404     writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
00405     writer.write(RID_VALVE_FF,(int) VALVE_FF);
00406     writer.write(RID_BULK_MODULUS,(int) BNO);
00407     writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A);
00408     writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B);
00409     writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A);
00410     writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B);
00411     writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY);
00412     writer.write(RID_PRES_RETURN,(int) PRES_RETURN);
00413     writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
00414     writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
00415     writer.write(RID_STROKE,(int) STROKE);
00416     //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
00417     //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
00418     writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f));
00419     writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f));
00420     writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
00421     writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
00422     writer.write(RID_FRICTION,(int) (FRICTION * 10.0f));
00423     writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET);
00424     writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING);
00425     writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f));
00426     writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f));
00427     writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f));
00428     writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
00429     writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
00430     writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
00431     writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
00432     writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
00433     writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
00434     writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
00435     writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
00436     writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
00437     writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
00438     for(int i=0; i<25; i++) {
00439         writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]);
00440     }
00441     for(int i=0; i<100; i++) {
00442         writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF));
00443         writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF));
00444     }
00445     writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
00446     writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
00447     //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f));
00448     writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM);
00449     
00450     writer.write(RID_K_SPRING, (int) K_SPRING);
00451     writer.write(RID_D_DAMPER, (int) D_DAMPER);
00452 
00453     writer.close();
00454 
00455 }
00456 
00457 void ROM_CALL_DATA(void)
00458 {
00459     BNO = flashReadInt(Rom_Sector, RID_BNO);
00460 //    BNO = 11;
00461     OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
00462     SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
00463     SENSING_MODE = 1;
00464     CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
00465     CURRENT_CONTROL_MODE = 1;
00466     FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE);
00467     CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
00468     CAN_FREQ = 500;
00469     DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
00470     DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
00471     DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
00472     SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
00473     VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
00474     P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
00475     I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
00476     D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
00477     P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
00478     I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
00479     D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
00480     P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
00481     I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
00482     D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
00483     VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
00484     VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
00485     VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
00486     COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
00487     VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
00488     VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
00489     BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
00490     CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
00491     CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
00492     PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
00493     PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
00494     PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
00495     alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
00496     PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
00497     PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
00498     ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
00499     ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
00500     STROKE = flashReadInt(Rom_Sector, RID_STROKE);
00501     //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
00502     //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
00503     ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f;
00504 //    ENC_PULSE_PER_POSITION = (float) 1024.0f;
00505     TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
00506     //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
00507 //    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
00508     PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
00509     PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00510     PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
00511     PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00512     FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
00513     HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
00514     HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
00515     TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
00516     PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
00517     PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
00518     VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
00519     VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
00520     VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
00521     VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
00522     VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
00523     VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
00524     VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
00525     VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
00526     VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
00527     VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
00528     for(int i=0; i<25; i++) {
00529         VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
00530     }
00531     for(int i=0; i<100; i++) {
00532         JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
00533     }
00534     VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
00535     VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
00536     //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
00537     VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
00538     
00539     K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING);
00540     D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER);
00541     
00542 //    ROM_RESET_DATA();
00543     
00544 }
00545 
00546 /*******************************************************************************
00547  * ENCODER functions
00548  ******************************************************************************/
00549 // A-KHA
00550 #define     KF_G1_11    0.083920206005350f
00551 #define     KF_G1_12    0.000013905329560f
00552 #define     KF_G1_21    -0.000575742328210f
00553 #define     KF_G1_22    0.799999939711725f
00554 // K
00555 #define     KF_G2_11    0.916079793994650f
00556 #define     KF_G2_12    0.000002878711641f
00557 #define     KF_G2_21    0.000575742328210f
00558 #define     KF_G2_22    0.199999945139809f
00559 
00560 float KF_Y_11 = 0.0f;
00561 float KF_Y_21 = 0.0f;
00562 float KF_X_11 = 0.0f;
00563 float KF_X_21 = 0.0f;
00564 
00565 long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
00566 long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
00567 long enc_offset = 0;
00568 
00569 void ENC_UPDATE(void)
00570 {
00571 
00572     ENC_pos_cur = spi_enc_read();
00573     ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
00574 
00575     //Kalman Filter
00576 //    ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k);
00577 //    KF_Y_11 = ENC_pos_cur;
00578 //    KF_Y_21 = ENC_VEL_RAW;
00579 //    KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
00580 //    KF_X_21 = KF_G1_21 * KF_X_11 + KF_G1_22 * KF_X_21 + KF_G2_21 * KF_Y_11 + KF_G2_22*KF_Y_21;
00581 //    ENC_VEL_KF = (int32_t) KF_X_21;
00582 //
00583 //    pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
00584 //    vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
00585     
00586     //Low Pass Filter
00587     
00588     double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset);
00589     double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k);
00590     
00591     double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f));
00592     pos.sen = NEW_POSITION;
00593     vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s
00594 
00595     ENC_pos_old = ENC_pos_cur;
00596 }
00597 
00598 void ENC_SET_ZERO(void)
00599 {
00600 
00601     spi_enc_set_clear();
00602     CUR_POSITION = 0;
00603     pos.sen = 0.0f;
00604     ENC_pos_old = ENC_pos_cur = 0;
00605 
00606 }
00607 
00608 void ENC_SET(int32_t value_e)
00609 {
00610     spi_enc_set_clear();
00611     enc_offset = value_e;
00612     CUR_POSITION = value_e;
00613     pos.sen = (float) value_e;
00614     ENC_pos_old = ENC_pos_cur = value_e;
00615 
00616 }
00617 
00618 
00619 
00620