Sungwoo Kim
/
HydraulicControlBoard_LIGHT
For LIGHT
Diff: function_utilities/function_utilities.cpp
- Revision:
- 62:851cf7b7aa7a
- Parent:
- 59:0ad14153b58f
- Child:
- 63:a3c7f31742c9
diff -r 7572762e3837 -r 851cf7b7aa7a function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Fri Apr 10 02:00:46 2020 +0000 +++ b/function_utilities/function_utilities.cpp Tue May 12 06:23:22 2020 +0000 @@ -15,15 +15,13 @@ // 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) +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 SETTING_SWITCH = 0; -uint8_t SETTING_SWITCH_OLD = 0; uint8_t REFERENCE_MODE = 0; int16_t CAN_FREQ = 500; -int CAN_FREQUENCY = 500; int16_t DIR_JOINT_ENC = 0; int16_t DIR_VALVE = 0; int16_t DIR_VALVE_ENC = 0; @@ -38,11 +36,11 @@ 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; +float I_GAIN_JOINT_TORQUE = 0.0f; +float D_GAIN_JOINT_TORQUE = 0.0f; -int16_t K_SPRING = 0.0; -int16_t D_DAMPER = 0.0; +float K_SPRING = 0.0f; +float D_DAMPER = 0.0f; int16_t flag_delay_test = 0; //float P_GAIN_VALVE_POSITION_OPP = 0.0f; @@ -73,6 +71,7 @@ int16_t PISTON_AREA_A; int16_t PISTON_AREA_B; float PISTON_AREA_alpha; +float alpha3 = 1.0f; int16_t PRES_SUPPLY; @@ -84,8 +83,8 @@ int16_t STROKE; -int16_t VALVE_LIMIT_PLUS; -int16_t VALVE_LIMIT_MINUS; +//int16_t VALVE_LIMIT_PLUS; +//int16_t VALVE_LIMIT_MINUS; float ENC_PULSE_PER_POSITION; float TORQUE_SENSOR_PULSE_PER_TORQUE; @@ -126,7 +125,6 @@ float CUR_TORQUE; float CUR_PRES_A; float CUR_PRES_B; -int CUR_PWM; int CUR_VALVE_POSITION; unsigned int TMR2_COUNT_LED1; @@ -145,25 +143,20 @@ int MODE_POS_FT_TRANS = 0; -//////////////////////////////////////////////////////////////////////////////// -////////////////////////////// SEUNGHOON ADD /////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// - -float CUR_PRES_A_BAR; -float CUR_PRES_B_BAR; -float CUR_TORQUE_NM; -float CUR_TORQUE_NM_PRESS; +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; -float PRES_A_VREF_TEST; -float PRES_B_VREF; -float PRES_B_VREF_TEST; -float TORQUE_VREF; +float PRES_A_VREF = 0.0f; +float PRES_B_VREF = 0.0f; +float TORQUE_VREF = 0.0f; -float VALVE_PWM_RAW_FB; -float VALVE_PWM_RAW_FF; -int VALVE_PWM_VALVE_DZ; -int VALVE_INPUT_PWM; +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]; @@ -171,34 +164,25 @@ int VALVE_MAX_POS; int VALVE_MIN_POS; -float DDV_CENTER; int VALVE_POS_NUM; float VALVE_CENTER_OFFSET; float VALVE_DZ_MINUS_OFFSET; float VALVE_DZ_PLUS_OFFSET; -int VALVE_CENTER_OFFSET_times10; - -int TMR3_COUNT_FINDHOME; -int TMR3_COUNT_FLOWRATE; -int TMR3_COUNT_DEADZONE; -int TMR3_COUNT_PRES_NULL; -int TMR3_COUNT_TORQUE_NULL; -int TMR3_COUNT_PRES_CALIB; -int TMR3_COUNT_REFERENCE; -int TMR3_COUNT_JOINT; -int TMR3_COUNT_ROTARY_FRIC_TUNE; -bool FLAG_REFERENCE_VALVE_PWM; -bool FLAG_REFERENCE_VALVE_POSITION; -bool FLAG_REFERENCE_JOINT_POSITION; -bool FLAG_REFERENCE_JOINT_TORQUE; -bool FLAG_REFERENCE_PRES_DIFF; -bool FLAG_REFERENCE_CURRENT; +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; +float TUNING_TIME = 600.0f; // sec -float REFERENCE_FREQ; -float REFERENCE_MAG; +float REFERENCE_FREQ = 1.0f; +float REFERENCE_MAG = 0.0f; bool FLAG_FIND_HOME; @@ -218,8 +202,8 @@ float CUR_PRES_B_mean = 0.0f; float CUR_TORQUE_sum = 0.0f; float CUR_TORQUE_mean = 0.0f; -float PRES_A_NULL = 2048.0f; -float PRES_B_NULL = 1.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; @@ -239,7 +223,6 @@ 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; @@ -281,6 +264,8 @@ 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; @@ -299,11 +284,20 @@ float freq_fric_tune = 1.0f; -bool FLAG_VALVE_OUTPUT_CALIB = false; - uint32_t TMR3_COUNT_CAN_TX = 0; -float I_REF = 0.0f; +// 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}; @@ -314,6 +308,23 @@ 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; + +float K_LPF = 0.0f; +float D_LPF = 0.0f; + //int h1, h2, h3, h4, h5, h6; /******************************************************************************* @@ -363,7 +374,7 @@ { FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector if (!writer.ready()) writer.open(); - + //BNO = 1; 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); @@ -400,10 +411,10 @@ 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_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 * 100.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)); @@ -431,7 +442,7 @@ } 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_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); @@ -447,8 +458,10 @@ 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); + CURRENT_CONTROL_MODE = 0; FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE); CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ); + CAN_FREQ = 500; 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); @@ -475,16 +488,17 @@ 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); + //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.01f; + 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; @@ -515,7 +529,7 @@ } 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; + //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); @@ -552,24 +566,25 @@ ENC_pos_cur = spi_enc_read(); ENC_pos_diff = ENC_pos_cur - ENC_pos_old; -// if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) { -// //MOT_E_STOP(0); -// } - - 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; - // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur; - vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF; - - // CUR_POSITION = ENC_pos_cur; - // CUR_VELOCITY = ENC_VEL_KF; + //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; } @@ -579,6 +594,7 @@ spi_enc_set_clear(); CUR_POSITION = 0; + pos.sen = 0.0f; ENC_pos_old = ENC_pos_cur = 0; } @@ -588,9 +604,11 @@ spi_enc_set_clear(); enc_offset = value_e; CUR_POSITION = value_e; + pos.sen = (float) value_e; ENC_pos_old = ENC_pos_cur = value_e; } +