![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
rainbow
Diff: function_utilities/function_utilities.cpp
- Revision:
- 230:2c3e5ecbe7e1
- Parent:
- 227:699c3e572283
- Child:
- 231:e00e71ca3e80
--- a/function_utilities/function_utilities.cpp Tue Apr 20 00:42:45 2021 +0000 +++ b/function_utilities/function_utilities.cpp Wed Apr 21 04:20:39 2021 +0000 @@ -14,6 +14,8 @@ 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 SUPPLY_PRESSURE_UPDATE = 0; // (0 : Update Off (constant Ps) , 1 : Update On (variable Ps)) + uint8_t CONTROL_UTILITY_MODE = 0; uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control) uint8_t FLAG_VALVE_DEADZONE = 0; @@ -74,9 +76,8 @@ float PISTON_AREA_alpha; float alpha3 = 1.0f; - -int16_t PRES_SUPPLY; -int16_t PRES_RETURN; +float PRES_SUPPLY_NOM = 70.0f; +float PRES_SUPPLY = 70.0f; int16_t ENC_LIMIT_PLUS; int16_t ENC_LIMIT_MINUS; @@ -96,8 +97,8 @@ //int16_t VALVE_LIMIT_PLUS; //int16_t VALVE_LIMIT_MINUS; -float ENC_PULSE_PER_POSITION; -float TORQUE_SENSOR_PULSE_PER_TORQUE; +float ENC_PULSE_PER_POSITION = 1.0f; +float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f; float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; @@ -113,11 +114,13 @@ float DAC_REF; float DAC_RESOL; -float REF_POSITION; -float REF_VELOCITY; -float REF_TORQUE; -float REF_TORQUE_OLD; -float REF_PRES_DIFF; +float REF_FORCE = 0.0; +float REF_TORQUE = 0.0; +float REF_POSITION = 0.0; +float REF_VELOCITY = 0.0; + +float REF_POSITION_FINDHOME = 0.0; + int16_t REF_PWM; int16_t REF_VALVE_POSITION; int16_t REF_CURRENT; @@ -125,19 +128,11 @@ int REF_MOVE_TIME_5k; int INIT_REF_PWM; int INIT_REF_VALVE_POS; -int32_t 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; @@ -158,14 +153,12 @@ int cnt_buffer = 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 FORCE_VREF = 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; @@ -209,16 +202,10 @@ 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 PRES_A_NULL_pulse = 300.0f; +float PRES_B_NULL_pulse = 300.0f; +float FORCE_NULL_pulse = 3900.0f; float Ref_Valve_Pos_Old = 0.0f; @@ -277,17 +264,6 @@ 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; - int cnt_finddz = 0; int cnt_vel_finddz = 0; int flag_finddz = 0; @@ -357,7 +333,9 @@ float PWM_out=0.0f; -double K_v = 0.0f; // valve flowrate gain +double K_v = 1.0f; // valve flowrate gain 1 +double C_d = 0.16f; // valve flowrate gain 2 + double mV_PER_mA = 600.0f; // current >> voltage double mV_PER_pulse = 0.6f; // pulse >> voltage double mA_PER_pulse = 0.001f; // pulse >> current @@ -453,8 +431,8 @@ PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B); PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B; alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha; - PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY); - PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN); + PRES_SUPPLY_NOM = spi_eeprom_read(RID_PRES_SUPPLY); + PRES_SUPPLY = PRES_SUPPLY_NOM; ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS); ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS); STROKE = spi_eeprom_read(RID_STROKE); @@ -467,7 +445,7 @@ FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f; HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET); HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING); - TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f; + FORCE_VREF = (float) (spi_eeprom_read(RID_FORCE_SENSOR_VREF)) *0.001f; PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f; PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f; VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f; @@ -497,62 +475,38 @@ /******************************************************************************* * 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; +long ENC_pulse = 0, ENC_pulse_old = 0, ENC_pulse_diff = 0; +long ENC_pulse_offset = 0; void ENC_UPDATE(void) { - - ENC_pos_cur = spi_enc_read(); - ENC_pos_diff = ENC_pos_cur - ENC_pos_old; - - //Low Pass Filter + ENC_pulse = spi_enc_read(); // Unit : pulse + ENC_pulse_diff = ENC_pulse - ENC_pulse_old; - 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); + pos.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse + ENC_pulse_offset)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg or mm + vel.UpdateSen((float)((long)DIR_JOINT_ENC * ENC_pulse_diff * (long)FREQ_10k)/ENC_PULSE_PER_POSITION, FREQ_10k, 100.0f); // Unit : deg/s or mm/s - 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; + ENC_pulse_old = ENC_pulse; } void ENC_SET_ZERO(void) { - spi_enc_set_clear(); - CUR_POSITION = 0; - ENC_pos_old = ENC_pos_cur = 0; - + pos.Reset(); + ENC_pulse_offset = 0; + ENC_pulse = ENC_pulse_old = ENC_pulse_diff = 0; } -void ENC_SET(int32_t value_e) +void ENC_SET(long value_e) { spi_enc_set_clear(); - enc_offset = value_e; - CUR_POSITION = value_e; - ENC_pos_old = ENC_pos_cur = value_e; - + ENC_pulse_offset = value_e; + pos.Reset(); + pos.sen = value_e/ENC_PULSE_PER_POSITION; + ENC_pulse = ENC_pulse_old = value_e; + ENC_pulse_diff = 0; }