![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: function_utilities/function_utilities.cpp
- Revision:
- 54:647072f5307a
- Parent:
- 52:8ea76864368a
- Child:
- 55:b25725257569
diff -r 8ea76864368a -r 647072f5307a function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Wed Feb 19 05:48:57 2020 +0000 +++ b/function_utilities/function_utilities.cpp Tue Feb 25 12:56:39 2020 +0000 @@ -15,14 +15,14 @@ // 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; -uint16_t CAN_FREQ = 500; +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; @@ -42,6 +42,7 @@ 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; @@ -124,12 +125,12 @@ float CUR_TORQUE; float CUR_PRES_A; float CUR_PRES_B; -int CUR_PWM; int CUR_VALVE_POSITION; unsigned int TMR2_COUNT_LED1; unsigned int TMR2_COUNT_LED2; -unsigned int TMR2_COUNT_CAN_TX; +unsigned int TMR2_COUNT_CAN_TX = 0; +unsigned int TMR3_COUNT_TEST = 0; int num_err; int flag_err[8]; @@ -142,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]; @@ -175,27 +171,20 @@ 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; +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; -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; +float TUNING_TIME = 600.0f; // sec -float TUNING_TIME; - -float REFERENCE_FREQ; -float REFERENCE_MAG; +float REFERENCE_FREQ = 1.0f; +float REFERENCE_MAG = 0.0f; bool FLAG_FIND_HOME; @@ -278,6 +267,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; @@ -296,11 +287,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}; @@ -311,6 +311,20 @@ 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; /******************************************************************************* @@ -549,24 +563,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; }