![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For LIGHT
function_utilities/function_utilities.cpp
- Committer:
- Lightvalve
- Date:
- 2020-03-09
- Revision:
- 58:2eade98630e2
- Parent:
- 57:f4819de54e7a
- Child:
- 59:0ad14153b58f
File content as of revision 58:2eade98630e2:
#include "setting.h" #include "SPI_EEP_ENC.h" #include "function_utilities.h" #include "function_CAN.h" #include "stm32f4xx_flash.h" #include "FlashWriter.h" int Rom_Sector = 6; //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector /******************************************************************************* * VARIABLE ******************************************************************************/ // Board Information uint8_t BNO = 0; uint8_t CONTROL_MODE = 0; uint8_t OPERATING_MODE = 0; // (00 : Moog & Rot, 01 : Moog & Lin, 10 : KNR & Rot, 11 : KNR & Lin, 101 : SW & Lin) uint8_t SENSING_MODE = 0; // (0 : torque, 1: pressure) uint8_t CONTROL_UTILITY_MODE = 0; uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control) uint8_t FLAG_VALVE_DEADZONE = 0; uint8_t REFERENCE_MODE = 0; int16_t CAN_FREQ = 500; int16_t DIR_JOINT_ENC = 0; int16_t DIR_VALVE = 0; int16_t DIR_VALVE_ENC = 0; float SUPPLY_VOLTAGE = 12.0f; float VALVE_VOLTAGE_LIMIT = 12.0f; //v float P_GAIN_VALVE_POSITION = 0.0f; float I_GAIN_VALVE_POSITION= 0.0f; float D_GAIN_VALVE_POSITION= 0.0f; float P_GAIN_JOINT_POSITION = 0.0f; float I_GAIN_JOINT_POSITION = 0.0f; float D_GAIN_JOINT_POSITION = 0.0f; float P_GAIN_JOINT_TORQUE = 0.0f; float I_GAIN_JOINT_TORQUE = 0.0; float D_GAIN_JOINT_TORQUE = 0.0; int16_t K_SPRING = 0.0; int16_t D_DAMPER = 0.0; int16_t flag_delay_test = 0; //float P_GAIN_VALVE_POSITION_OPP = 0.0f; //float I_GAIN_VALVE_POSITION_OPP= 0.0f; //float D_GAIN_VALVE_POSITION_OPP= 0.0f; //float P_GAIN_JOINT_POSITION_OPP = 0.0f; //float I_GAIN_JOINT_POSITION_OPP = 0.0f; //float D_GAIN_JOINT_POSITION_OPP = 0.0f; //float P_GAIN_JOINT_TORQUE_OPP = 0.0f; //float I_GAIN_JOINT_TORQUE_OPP = 0.0; //float D_GAIN_JOINT_TORQUE_OPP = 0.0; float VALVE_DEADZONE_PLUS; float VALVE_DEADZONE_MINUS; int16_t VELOCITY_COMP_GAIN; int16_t COMPLIANCE_GAIN; int16_t VALVE_CENTER; int16_t VALVE_FF; int16_t BULK_MODULUS; int16_t CHAMBER_VOLUME_A; int16_t CHAMBER_VOLUME_B; int16_t PISTON_AREA_A; int16_t PISTON_AREA_B; float PISTON_AREA_alpha; float alpha3 = 1.0f; int16_t PRES_SUPPLY; int16_t PRES_RETURN; int16_t ENC_LIMIT_PLUS; int16_t ENC_LIMIT_MINUS; int16_t STROKE; //int16_t VALVE_LIMIT_PLUS; //int16_t VALVE_LIMIT_MINUS; float ENC_PULSE_PER_POSITION; float TORQUE_SENSOR_PULSE_PER_TORQUE; float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; int HOMEPOS_OFFSET; int HOMEPOS_VALVE_OPENING; float FRICTION; float REF_PERIOD; float REF_MAG; int REF_NUM; float DAC_REF; float DAC_RESOL; int REF_POSITION; int REF_VELOCITY; int16_t REF_TORQUE; int16_t REF_PRES_DIFF; int16_t REF_PWM; int16_t REF_VALVE_POSITION; int16_t REF_CURRENT; int REF_MOVE_TIME_5k; int INIT_REF_PWM; int INIT_REF_VALVE_POS; int INIT_REF_POS; int INIT_REF_VEL; int INIT_REF_TORQUE; int INIT_REF_PRES_DIFF; int INIT_REF_CURRENT; int CUR_POSITION; int CUR_VELOCITY; float CUR_TORQUE; float CUR_PRES_A; float CUR_PRES_B; int CUR_VALVE_POSITION; unsigned int TMR2_COUNT_LED1; unsigned int TMR2_COUNT_LED2; unsigned int TMR2_COUNT_CAN_TX = 0; unsigned int TMR3_COUNT_TEST = 0; int num_err; int flag_err[8]; int flag_err_old[8]; int flag_err_rt; int flag_ref_enable; int flag_data_request[5]; int MODE_POS_FT_TRANS = 0; float CUR_CURRENT_mA = 0.0f; float CUR_PRES_A_BAR = 0.0f; float CUR_PRES_B_BAR = 0.0f; float CUR_TORQUE_NM = 0.0f; float CUR_TORQUE_NM_PRESS = 0.0f; float PRES_A_VREF = 0.0f; float PRES_B_VREF = 0.0f; float TORQUE_VREF = 0.0f; float VALVE_PWM_RAW_FB = 0.0f; float VALVE_PWM_RAW_FF = 0.0f; float VALVE_PWM_RAW = 0.0f; int VALVE_PWM_VALVE_DZ = 0; float VALVE_GAIN_LPM_PER_V[10]; float VALVE_POS_VS_PWM[25]; long JOINT_VEL[100]; int VALVE_MAX_POS; int VALVE_MIN_POS; int VALVE_POS_NUM; float VALVE_CENTER_OFFSET; float VALVE_DZ_MINUS_OFFSET; float VALVE_DZ_PLUS_OFFSET; int TMR3_COUNT_FINDHOME = 0; int TMR3_COUNT_FLOWRATE = 0; int TMR3_COUNT_DEADZONE = 0; int TMR3_COUNT_PRES_NULL = 0; int TMR3_COUNT_TORQUE_NULL = 0; int TMR3_COUNT_PRES_CALIB = 0; int TMR3_COUNT_REFERENCE = 0; int TMR3_COUNT_JOINT = 0; int TMR3_COUNT_ROTARY_FRIC_TUNE = 0; float TUNING_TIME = 600.0f; // sec float REFERENCE_FREQ = 1.0f; float REFERENCE_MAG = 0.0f; bool FLAG_FIND_HOME; int MODE_JUMP_STATUS; enum _JUMP_STATUS { JUMP_NO_ACT = 0, //0 JUMP_START, //1 JUMP_TAKEOFF, //2 JUMP_FLYING, //3 JUMP_LANDING, //4 }; float CUR_PRES_DIFF_BAR = 0.0f; float CUR_PRES_A_sum = 0.0f; float CUR_PRES_B_sum = 0.0f; float CUR_PRES_A_mean = 0.0f; float CUR_PRES_B_mean = 0.0f; float CUR_TORQUE_sum = 0.0f; float CUR_TORQUE_mean = 0.0f; float PRES_A_NULL = 300.0f; float PRES_B_NULL = 300.0f; float TORQUE_NULL = 3900.0f; float Ref_Valve_Pos_Old = 0.0f; int VALVE_ID_timer = 0; int VALVE_DZ_timer = 0; int VALVE_FR_timer = 0; //int VALVE_HPL_timer = 0; int VALVE_POS_TMP = 0; int JOINT_VEL_TMP = 0; int DDV_POS_AVG = 0; int VALVE_POS_AVG[50] = {0}; int VALVE_POS_AVG_OLD = 0; int data_num = 0; int ID_index = 0; int DZ_index = 1; int ID_index_array[50] = {0}; int first_check = 0; float init_time = 0.0f; int VEL_POINT = 0; int DZ_case = 0; int START_POS = 0; int FINAL_POS = 0; int DZ_DIRECTION = 0; int FIRST_DZ = 0; int SECOND_DZ = 0; int DZ_NUM = 0; int one_period_end = 0; float Ref_Vel_Test = 0.0f; long TMR2_FOR_SLOW_LOGGING = 0; //int velcount = 0; char max_check = 0; char min_check = 0; float valve_pos_err = 0.0f, valve_pos_err_old = 0.0f, valve_pos_err_diff = 0.0f, valve_pos_err_sum = 0.0f; 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; float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f; float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f; float CUR_FLOWRATE = 0.0f; float VALVE_FF_VOLTAGE = 0.0f; int pos_plus_end = 0; int pos_minus_end = 0; bool need_enc_init = false; int temp_time = 0; float CUR_VELOCITY_sum = 0.0f; float temp_vel_sum = 0.0f; int DZ_dir = 0; int DZ_temp_cnt = 0; int DZ_temp_cnt2 = 0; int DZ_end = 2; int flag_flowrate = 0; int fl_temp_cnt = 0; int fl_temp_cnt2 = 0; int cur_vel_sum = 0; float Cur_Valve_Open_pulse = 0.0f; // find home int CUR_VELOCITY_OLD = 0; int cnt_findhome = 0; int cnt_vel_findhome = 0; int FINDHOME_VELOCITY = 0; int FINDHOME_VELOCITY_OLD = 0; int FINDHOME_POSITION = 0; int FINDHOME_POSITION_OLD = 0; // valve gain int check_vel_pos_init = 0; int check_vel_pos_fin = 0; int check_vel_pos_interv = 0; int valve_gain_repeat_cnt = 0; float VALVE_VOLTAGE = 0.0f; float freq_fric_tune = 1.0f; uint32_t TMR3_COUNT_CAN_TX = 0; // Current Control Variables double I_REF = 0.0f; double I_REF_fil = 0.0f; double I_ERR = 0.0f; double I_ERR_INT = 0.0f; double I_REF_fil_old = 0.0f; double I_REF_fil_diff = 0.0f; // system id int cnt_sysid = 0; double freq_sysid_Iref = 0.0f; int TMR3_COUNT_IREF = 0; float CUR_CURRENT = 0.0f; float u_CUR[3] = {0.0f,0.0f,0.0f}; int FINDHOME_STAGE = 0; int FINDHOME_INIT = 0; int FINDHOME_GOTOLIMIT = 1; int FINDHOME_ZEROPOSE = 2; float alpha_trans = 0.0f; float V_out=0.0f; float V_rem=0.0f; // for anti-windup float V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV float PWM_out=0.0f; double K_v = 0.0f; // valve flowrate gain double mV_PER_mA = 600.0f; // current >> voltage double mV_PER_pulse = 0.6f; // pulse >> voltage double mA_PER_pulse = 0.001f; // pulse >> current int timer_while = 0; int while_index = 0; //int h1, h2, h3, h4, h5, h6; /******************************************************************************* * General math functions ******************************************************************************/ float dabs(float tx) { if (tx >= 0.0f) return tx; else return -tx; } float change_int_to_efloat(int input) { int i = 0; float output = 0; int vn = (int) ((float) input / 10.0f); int en = input % 10; float temp = 1.; for (i = 0; i < en; i++) temp *= 0.1f; output = (float) vn*temp; return output; } void make_delay(void) { int i = 0; for (i = 0; i < 100000; i++) { ; } } /******************************************************************************* * ROM functions ******************************************************************************/ void ROM_RESET_DATA(void) { FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector if (!writer.ready()) writer.open(); writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀 writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE); writer.write(RID_SENSING_MODE, (int) SENSING_MODE); writer.write(RID_CURRENT_CONTROL_MODE, (int) CURRENT_CONTROL_MODE); writer.write(RID_FLAG_VALVE_DEADZONE, (int) FLAG_VALVE_DEADZONE); writer.write(RID_CAN_FREQ,(int) CAN_FREQ); writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC); writer.write(RID_VALVE_DIR,(int) DIR_VALVE); writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC); writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0f)); writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0f)); writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION); writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION); writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION); writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION); writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION); writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION); writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE); writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE); writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE); writer.write(RID_VALVE_DEADZONE_PLUS,(int) (VALVE_DEADZONE_PLUS * 10.0f)); writer.write(RID_VALVE_DEADZONE_MINUS,(int) (VALVE_DEADZONE_MINUS * 10.0f)); writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN); writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN); writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER); writer.write(RID_VALVE_FF,(int) VALVE_FF); writer.write(RID_BULK_MODULUS,(int) BNO); writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A); writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B); writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A); writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B); writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY); writer.write(RID_PRES_RETURN,(int) PRES_RETURN); writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS); writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS); writer.write(RID_STROKE,(int) STROKE); //writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS); //writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS); writer.write(RID_ENC_PULSE_PER_POSITION,(int) (ENC_PULSE_PER_POSITION*10.0f)); writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f)); writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); writer.write(RID_FRICTION,(int) (FRICTION * 10.0f)); writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET); writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING); writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0f)); writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0f)); writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0f)); writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0f)); writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0f)); writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0f)); writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0f)); writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0f)); writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0f)); for(int i=0; i<25; i++) { writer.write(RID_VALVE_POS_VS_PWM_0 + i, (int) VALVE_POS_VS_PWM[i]); } for(int i=0; i<100; i++) { writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF)); writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF)); } writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS); writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS); //writer.write(RID_DDV_CENTER, (int) (DDV_CENTER * 10.0f)); writer.write(RID_VALVE_POS_NUM, (int) VALVE_POS_NUM); writer.write(RID_K_SPRING, (int) K_SPRING); writer.write(RID_D_DAMPER, (int) D_DAMPER); writer.close(); } void ROM_CALL_DATA(void) { BNO = flashReadInt(Rom_Sector, RID_BNO); BNO = 1; OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE); SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE); CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE); FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE); CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ); DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR); DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR); DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR); SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f; VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f; P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION); I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION); D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION); P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION); I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION); D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION); P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE); I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE); D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE); VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f; VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f; VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN); COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN); VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER); VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF); BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS); CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A); CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B); PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A); PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B); PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A; alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha; PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY); PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN); ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS); ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS); STROKE = flashReadInt(Rom_Sector, RID_STROKE); //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS); //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS); ENC_PULSE_PER_POSITION = (float) (flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION)) * 0.1f; // ENC_PULSE_PER_POSITION = (float) 1024.0f; TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f; //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle // TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f; // PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f; // PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f; HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET); HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING); TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f; PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f; PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f; VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f; VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f; VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f; VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f; VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f; VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f; VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f; VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f; VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f; VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f; for(int i=0; i<25; i++) { VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i)); } for(int i=0; i<100; i++) { 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 ) ; } VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS); VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS); //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f; VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM); K_SPRING = flashReadInt(Rom_Sector, RID_K_SPRING); D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER); } /******************************************************************************* * ENCODER functions ******************************************************************************/ // A-KHA #define KF_G1_11 0.083920206005350f #define KF_G1_12 0.000013905329560f #define KF_G1_21 -0.000575742328210f #define KF_G1_22 0.799999939711725f // K #define KF_G2_11 0.916079793994650f #define KF_G2_12 0.000002878711641f #define KF_G2_21 0.000575742328210f #define KF_G2_22 0.199999945139809f float KF_Y_11 = 0.0f; float KF_Y_21 = 0.0f; float KF_X_11 = 0.0f; float KF_X_21 = 0.0f; long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0; long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0; long enc_offset = 0; void ENC_UPDATE(void) { ENC_pos_cur = spi_enc_read(); ENC_pos_diff = ENC_pos_cur - ENC_pos_old; //Kalman Filter // ENC_VEL_RAW = (int32_t) (ENC_pos_diff * TMR_FREQ_5k); // KF_Y_11 = ENC_pos_cur; // KF_Y_21 = ENC_VEL_RAW; // 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; // 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; // ENC_VEL_KF = (int32_t) KF_X_21; // // pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset; // vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF; //Low Pass Filter double NEW_POSITION = (double) ((DIR_JOINT_ENC) * ENC_pos_cur + enc_offset); double NEW_VELOCITY = (double) ((DIR_JOINT_ENC) * ENC_pos_diff * (int) FREQ_10k); double alpha_update_pos = 1.0f/(1.0f + FREQ_10k/(2.0f*3.14f*100.0f)); pos.sen = NEW_POSITION; vel.sen = (1.0f - alpha_update_pos) * vel.sen + alpha_update_pos * NEW_VELOCITY; // pulse/s ENC_pos_old = ENC_pos_cur; } void ENC_SET_ZERO(void) { spi_enc_set_clear(); CUR_POSITION = 0; ENC_pos_old = ENC_pos_cur = 0; } void ENC_SET(int32_t value_e) { spi_enc_set_clear(); enc_offset = value_e; CUR_POSITION = value_e; ENC_pos_old = ENC_pos_cur = value_e; }