Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 230:2c3e5ecbe7e1, committed 2021-04-21
- Comitter:
- jobuuu
- Date:
- Wed Apr 21 04:20:39 2021 +0000
- Parent:
- 229:7a1c46b9b471
- Child:
- 231:e00e71ca3e80
- Commit message:
- Synchronize to HCB_210420 (MZ-board)
Changed in this revision
--- a/CAN/function_CAN.cpp Tue Apr 20 00:42:45 2021 +0000 +++ b/CAN/function_CAN.cpp Wed Apr 21 04:20:39 2021 +0000 @@ -14,10 +14,11 @@ int CID_TX_INFO = 1100; int CID_TX_POS_VEL_TORQ = 1200; int CID_TX_PWM = 1300; +int CID_TX_SOMETHING = 1400; + int CID_TX_CURRENT = 1400; int CID_TX_VOUT = 1500; int CID_TX_VALVE_POSITION = 1600; -int CID_TX_SOMETHING = 1700; // variables uint8_t can_index = 0; @@ -32,6 +33,31 @@ extern float input_NN[]; /******************************************************************************* + * State Class functions + ******************************************************************************/ +void State::UpdateSen(float sen_new, float Freq_update, float f_cut) { + if(f_cut<=0.0f) f_cut=0.001f; + + this->sen_diff = (sen_new-this->sen)*Freq_update; + float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz + this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new; +} + +void State::UpdateRef(float ref_new) { + this->ref = ref_new; +} + +void State::Reset() { + this->sen = 0.0f; + this->sen_diff = 0.0f; + this->ref = 0.0f; + this->err = 0.0f; + this->err_int = 0.0f; + this->err_old = 0.0f; + this->err_diff = 0.0f; +} + +/******************************************************************************* * CAN functions ******************************************************************************/ void CAN_ID_INIT(void) @@ -232,6 +258,18 @@ CONTROL_UTILITY_MODE = 22; break; } + + case CRX_ASK_VARIABLE_SUPPLY: + { + CAN_TX_VARIABLE_SUPPLY_ONOFF(); + break; + } + + case CRX_SET_VARIABLE_SUPPLY: + { + SUPPLY_PRESSURE_UPDATE = msg.data[1]; + break; + } case CRX_ASK_PID_GAIN: { CAN_TX_PID_GAIN(msg.data[1]); @@ -392,22 +430,16 @@ break; } - case CRX_ASK_PRES: { - CAN_TX_PRES_A_AND_B(); -// SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); - //dac_1 = PRES_A_VREF; - //dac_2 = PRES_B_VREF; - + case CRX_ASK_SUP_PRES: { + CAN_TX_SUP_PRES(); break; } - case CRX_SET_PRES: { - PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8); - PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8); - spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY); - spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN); - - + case CRX_SET_SUP_PRES: { + int16_t temp = (int16_t) (msg.data[1] | msg.data[2] << 8); + spi_eeprom_write(RID_PRES_SUPPLY, temp); + PRES_SUPPLY_NOM = (float)temp; + PRES_SUPPLY = PRES_SUPPLY_NOM; break; } @@ -634,65 +666,58 @@ } else if(address==CID_RX_REF_POSITION) { - int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); + int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8); int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8); if((OPERATING_MODE&0b001)==0) { // Rotary Actuator - REF_POSITION = (double)temp_pos * 10.0f; - REF_VELOCITY = (double)temp_vel * 256.0f; + REF_POSITION = (float)temp_pos * 1.0f / ENC_PULSE_PER_POSITION; // pulse >> deg + REF_VELOCITY = (float)temp_vel * 10.0f / ENC_PULSE_PER_POSITION; // pulse/s >> deg/s + REF_TORQUE = (float)temp_torq * 0.01f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> Nm } else { //Linear Actuator - REF_POSITION = (double)temp_pos * 10.0f; - REF_VELOCITY = (double)temp_vel * 256.0f; + REF_POSITION = (float)temp_pos * 4.0f / ENC_PULSE_PER_POSITION; // pulse >> mm + REF_VELOCITY = (float)temp_vel * 100.0f / ENC_PULSE_PER_POSITION; // pulse/s >> mm/s + REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; // pulse >> N } - REF_TORQUE = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N - torq.ref_diff = REF_TORQUE - REF_TORQUE_OLD; - REF_TORQUE_OLD = REF_TORQUE; + if(SUPPLY_PRESSURE_UPDATE == 1) { + int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8); + PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0; + if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f; + else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f; + } else { + PRES_SUPPLY = PRES_SUPPLY_NOM; + } if(CAN_FREQ == -1) { + // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } } - // PWM, ID:1300 + // Valve Position (ID:1300) if (flag_data_request[1] == HIGH) { - CAN_TX_PWM((int16_t) (V_out)); //1300 + CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); } - // Reference Current, Current Current (ID:1400) + // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) if (flag_data_request[2] == HIGH) { - CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400 + CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (0), (int16_t) (0)); } - - // Reference Current, Current Current (ID:1700) - if (flag_data_request[3] == HIGH) { - CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700 - } - } - } else if(address==CID_RX_REF_OPENLOOP) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve - valve_pos.ref = (double) temp_ref_valve_pos; + valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~10000) } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve - valve_pos.ref = (double) temp_ref_valve_pos; + valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000) } else { //SW Valve if(temp_ref_valve_pos >= 0) { valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f; @@ -851,6 +876,16 @@ can.write(temp_msg); } +void CAN_TX_VARIABLE_SUPPLY_ONOFF(void) +{ + CANMessage temp_msg; + temp_msg.id = CID_TX_INFO; + temp_msg.len = 2; + temp_msg.data[0] = (uint8_t) CTX_SEND_VARIABLE_SUPPLY; + temp_msg.data[1] = (uint8_t) SUPPLY_PRESSURE_UPDATE; + + can.write(temp_msg); +} void CAN_TX_PID_GAIN(int t_type) { @@ -992,17 +1027,18 @@ can.write(temp_msg); } -void CAN_TX_PRES_A_AND_B(void) +void CAN_TX_SUP_PRES(void) { CANMessage temp_msg; + int16_t temp_PRES_SUPPLY = (int16_t) (PRES_SUPPLY); temp_msg.id = CID_TX_INFO; temp_msg.len = 5; - temp_msg.data[0] = (uint8_t) CTX_SEND_PRES; - temp_msg.data[1] = (uint8_t) PRES_SUPPLY; - temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8); - temp_msg.data[3] = (uint8_t) PRES_RETURN; - temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8); + temp_msg.data[0] = (uint8_t) CTX_SEND_SUP_PRES; + temp_msg.data[1] = (uint8_t) temp_PRES_SUPPLY; + temp_msg.data[2] = (uint8_t) (temp_PRES_SUPPLY >> 8); + temp_msg.data[3] = 0; + temp_msg.data[4] = 0; can.write(temp_msg); }
--- a/CAN/function_CAN.h Tue Apr 20 00:42:45 2021 +0000 +++ b/CAN/function_CAN.h Wed Apr 21 04:20:39 2021 +0000 @@ -45,7 +45,10 @@ #define CRX_SET_VOLTAGE_SUPPLY 112 #define CRX_ASK_VOLTAGE_VALVE 13 #define CRX_SET_VOLTAGE_VALVE 113 -#define CRX_SET_HOMEPOS 114 +#define CRX_SET_HOMEPOS 114 +#define CRX_ASK_VARIABLE_SUPPLY 15 +#define CRX_SET_VARIABLE_SUPPLY 115 + #define CRX_ASK_PID_GAIN 20 #define CRX_SET_PID_GAIN 120 #define CRX_ASK_VALVE_DEADZONE 21 @@ -62,8 +65,8 @@ #define CRX_SET_CHAMBER_VOLUME 127 #define CRX_ASK_PISTON_AREA 28 #define CRX_SET_PISTON_AREA 128 -#define CRX_ASK_PRES 29 -#define CRX_SET_PRES 129 +#define CRX_ASK_SUP_PRES 29 +#define CRX_SET_SUP_PRES 129 #define CRX_ASK_ENC_LIMIT 30 #define CRX_SET_ENC_LIMIT 130 #define CRX_ASK_STROKE 31 @@ -119,6 +122,7 @@ #define CTX_SEND_VALVE_ENC_DIR 11 #define CTX_SEND_VOLTAGE_SUPPLY 12 #define CTX_SEND_VOLTAGE_VALVE 13 +#define CTX_SEND_VARIABLE_SUPPLY 15 #define CTX_SEND_PID_GAIN 20 #define CTX_SEND_VALVE_DEADZONE 21 #define CTX_SEND_VELOCITY_COMP_GAIN 22 @@ -127,7 +131,7 @@ #define CTX_SEND_BULK_MODULUS 26 #define CTX_SEND_CHAMBER_VOLUME 27 #define CTX_SEND_PISTON_AREA 28 -#define CTX_SEND_PRES 29 +#define CTX_SEND_SUP_PRES 29 #define CTX_SEND_ENC_LIMIT 30 #define CTX_SEND_STROKE 31 #define CTX_SEND_VALVE_LIMIT 32 @@ -168,6 +172,8 @@ void CAN_TX_VALVE_ENC_DIR(void); void CAN_TX_VOLTAGE_SUPPLY(void); void CAN_TX_VOLTAGE_VALVE(void); +void CAN_TX_VARIABLE_SUPPLY_ONOFF(void); + void CAN_TX_PID_GAIN(int t_type); void CAN_TX_VALVE_DEADZONE(void); void CAN_TX_VELOCITY_COMP_GAIN(void); @@ -176,7 +182,7 @@ void CAN_TX_BULK_MODULUS(void); void CAN_TX_CHAMBER_VOLUME(void); void CAN_TX_PISTON_AREA(void); -void CAN_TX_PRES_A_AND_B(void); +void CAN_TX_SUP_PRES(void); void CAN_TX_ENC_LIMIT(void); void CAN_TX_STROKE(void); void CAN_TX_VALVE_LIMIT(void); @@ -199,10 +205,8 @@ { public: float sen; + float sen_diff; float ref; - float ref_old; - float ref_diff; - float ref_home_pos; float err; float err_int; float err_old; @@ -211,20 +215,23 @@ public: State(){ sen = 0.0f; + sen_diff = 0.0f; ref = 0.0f; - ref_old = 0.0f; - ref_diff = 0.0f; - ref_home_pos = 0.0f; err = 0.0f; err_int = 0.0f; err_old = 0.0f; err_diff = 0.0f; } + + void UpdateSen(float sen_new, float Freq_update, float f_cut = 1000.0f); + void UpdateRef(float ref_new); + void Reset(); }; extern State pos; extern State vel; extern State Vout; +extern State force; extern State torq; extern State torq_dot; extern State pres_A;
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h Tue Apr 20 00:42:45 2021 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.h Wed Apr 21 04:20:39 2021 +0000 @@ -80,7 +80,7 @@ #define RID_HOMEPOS_OFFSET 41 #define RID_HOMEPOS_VALVE_OPENING 42 -#define RID_TORQUE_SENSOR_VREF 45 +#define RID_FORCE_SENSOR_VREF 45 #define RID_PRES_A_SENSOR_VREF 50 #define RID_PRES_B_SENSOR_VREF 51
--- 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; }
--- a/function_utilities/function_utilities.h Tue Apr 20 00:42:45 2021 +0000 +++ b/function_utilities/function_utilities.h Wed Apr 21 04:20:39 2021 +0000 @@ -28,7 +28,7 @@ void ENC_UPDATE(void); void ENC_SET_ZERO(void); -void ENC_SET(int32_t value); +void ENC_SET(long value); void VALVE_POSITION_INIT(void); void VALVE_PWM(int pwm, float vol_max, float vol_in);
--- a/main.cpp Tue Apr 20 00:42:45 2021 +0000 +++ b/main.cpp Wed Apr 21 04:20:39 2021 +0000 @@ -29,8 +29,8 @@ // dac & check /////////////////////////////////////////// DigitalOut check(PC_2); DigitalOut check_2(PC_3); -AnalogOut dac_1(PA_4); -AnalogOut dac_2(PA_5); +AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f +AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f AnalogIn adc1(PC_4); //pressure_1 AnalogIn adc2(PB_0); //pressure_2 AnalogIn adc3(PC_1); //current @@ -66,11 +66,12 @@ State pos; State vel; State Vout; -State torq; +State force; +State torq; // unit : N State torq_dot; -State pres_A; +State pres_A; // unit : bar State pres_B; -State cur; +State cur; // unit : mA State valve_pos; State INIT_Vout; @@ -91,26 +92,24 @@ extern int CID_TX_VALVE_POSITION; extern int CID_TX_SOMETHING; -float pres_A_new = 0.0f; -float pres_B_new = 0.0f; - - - float temp_P_GAIN = 0.0f; float temp_I_GAIN = 0.0f; int temp_VELOCITY_COMP_GAIN = 0; +inline float tanh_inv(float y) { + if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f; + if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f; + return log(sqrt((1.0f+y)/(1.0f-y))); +} + /******************************************************************************* * REFERENCE MODE ******************************************************************************/ enum _REFERENCE_MODE { - MODE_REF_NO_ACT = 0, //0 - MODE_REF_DIRECT, //1 - MODE_REF_COS_INC, //2 - MODE_REF_LINE_INC, //3 - MODE_REF_SIN_WAVE, //4 - MODE_REF_SQUARE_WAVE, //5 + MODE_REF_NO_ACT = 0, + MODE_REF_DIRECT, + MODE_REF_FINDHOME }; /******************************************************************************* @@ -234,7 +233,6 @@ make_delay(); - // spi _ enc spi_enc_set_init(); make_delay(); @@ -270,19 +268,14 @@ TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); - - //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM4_IRQn, 3); - - - //DAC init if (SENSING_MODE == 0) { - dac_1 = TORQUE_VREF / 3.3f; + dac_1 = FORCE_VREF / 3.3f; dac_2 = 0.0f; } else if (SENSING_MODE == 1) { if (DIR_VALVE_ENC > 0) { @@ -435,6 +428,9 @@ TIMER INTERRUPT *******************************************************************************/ +//------------------------------------------------ +// TMR4 : Sensor Read & Data Handling +//----------------------------------------------- float FREQ_TMR4 = (float)FREQ_20k; float DT_TMR4 = (float)DT_20k; long CNT_TMR4 = 0; @@ -442,52 +438,42 @@ extern "C" void TIM4_IRQHandler(void) { if (TIM4->SR & TIM_SR_UIF ) { - /******************************************************* - *** Sensor Read & Data Handling - ********************************************************/ - //Encoder + // Current =================================================== + //ADC3->CR2 |= 0x40000000; // adc _ 12bit + + cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA + + // Encoder =================================================== if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { ENC_UPDATE(); } + // Force or Pressure Transducer ============================================= ADC1->CR2 |= 0x40000000; - // Torque Sensing ============================================= - if (SENSING_MODE == 0) { - float pres_A_new = (((float) ADC1->DR) - 2047.5f); - double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 100Hz - pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new; - torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE; - - // Pressure Sensing (0~210)bar ============================================= - } else if (SENSING_MODE == 1) { - + if (SENSING_MODE == 0) { // Force sensing + force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N + } else if (SENSING_MODE == 1) { // Pressure sensing + float pres_A_new, pres_B_new; if (DIR_VALVE_ENC > 0) { - pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); - pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar + pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; } else { - pres_A_new = (((float)ADC2->DR) - PRES_A_NULL); - pres_B_new = (((float)ADC1->DR) - PRES_B_NULL); + pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar + pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; } - double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz - pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new; - pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new; - CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR; - CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR; + pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); + pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator - torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm + float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm + torq.UpdateSen(torq_new,FREQ_TMR4,1000.0); // unit : Nm } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator - torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N + float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N + force.UpdateSen(force_new,FREQ_TMR4,1000.0); // unit : N } } - //Current - //ADC3->CR2 |= 0x40000000; // adc _ 12bit - float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz - float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA - cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur); - CNT_TMR4++; } TIM4->SR = 0x0; // reset the status register @@ -505,26 +491,15 @@ { if (TIM3->SR & TIM_SR_UIF ) { - - if (((OPERATING_MODE&0b110)>>1) == 0) { - K_v = 0.4f; // Moog (LPM >> mA) , 100bar - mV_PER_mA = 500.0f; // 5000mV/10mA - mV_PER_pulse = 0.5f; // 5000mV/10000pulse - mA_PER_pulse = 0.001f; // 10mA/10000pulse - } else if (((OPERATING_MODE&0b110)>>1) == 1) { - K_v = 0.5f; // KNR (LPM >> mA) , 100bar - mV_PER_mA = 166.6666f; // 5000mV/30mA - mV_PER_pulse = 0.5f; // 5000mV/10000pulse - mA_PER_pulse = 0.003f; // 30mA/10000pulse - } - if(MODE_POS_FT_TRANS == 1) { + if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; torq.err_sum = 0; if((float)cnt_trans * DT_TMR3 > 3.0f) MODE_POS_FT_TRANS = 2; } else if(MODE_POS_FT_TRANS == 3) { + if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0; alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; torq.err_sum = 0; @@ -539,22 +514,45 @@ } -// Reference Loop ========================================================== + // Reference Update ========================================================== switch (REFERENCE_MODE) { case MODE_REF_NO_ACT: { break; } - case MODE_REF_DIRECT: { pos.ref = REF_POSITION; vel.ref = REF_VELOCITY; torq.ref = REF_TORQUE; + force.ref = REF_FORCE; + break; + } + case MODE_REF_FINDHOME: { + pos.ref = REF_POSITION_FINDHOME; + vel.ref = 0.0f; + torq.ref = 0.0f; + force.ref = 0.0f; break; } default: break; } + if (((OPERATING_MODE&0b010)>>1) == 0) { + K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv); + K_v = 0.16f; + mV_PER_mA = 500.0f; // 5000mV/10mA + mV_PER_pulse = 0.5f; // 5000mV/10000pulse + mA_PER_pulse = 0.001f; // 10mA/10000pulse + } else if (((OPERATING_MODE&0b010)>>1) == 1) { + K_v = 0.5f; // KNR (LPM >> mA) , 100bar + mV_PER_mA = 166.6666f; // 5000mV/30mA + mV_PER_pulse = 0.5f; // 5000mV/10000pulse + mA_PER_pulse = 0.003f; // 30mA/10000pulse + } + + // ===================================================================== + // CONTROL LOOP -------------------------------------------------------- + // ===================================================================== int UTILITY_MODE = 0; int CONTROL_MODE = 0; @@ -565,350 +563,145 @@ CONTROL_MODE = CONTROL_UTILITY_MODE; UTILITY_MODE = MODE_NO_ACT; } - - - // UTILITY MODE ------------------------------------------------------------ switch (UTILITY_MODE) { case MODE_NO_ACT: { break; } - case MODE_TORQUE_SENSOR_NULLING: { + case MODE_TORQUE_SENSOR_NULLING: + { + static float FORCE_pulse_sum = 0.0; + static float PresA_pulse_sum = 0.0; + static float PresB_pulse_sum = 0.0; + // DAC Voltage reference set - if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { - CUR_TORQUE_sum += torq.sen; + float VREF_TuningGain = 0.000003f; + if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) { + LED = 1; + if(SENSING_MODE == 0) { // Force Sensor (Loadcell) + FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE; + if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { + float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f; + FORCE_pulse_sum = 0.0f; - if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { - CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f; - CUR_TORQUE_sum = 0; - - TORQUE_VREF += 0.000003f * (0.0f - CUR_TORQUE_mean); + FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean); + if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f; + if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f; + dac_1 = FORCE_VREF / 3.3f; + } + } else if (SENSING_MODE == 1) { // Pressure Sensor + PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR; + PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR; + if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { + float PresA_pluse_mean = PresA_pulse_sum / 10.0f; + float PresB_pluse_mean = PresB_pulse_sum / 10.0f; + PresA_pulse_sum = 0.0f; + PresB_pulse_sum = 0.0f; - if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f; - if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f; - dac_1 = TORQUE_VREF / 3.3f; + PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean); + if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; + if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; + dac_1 = PRES_A_VREF / 3.3f; + PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean); + if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; + if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; + dac_2 = PRES_B_VREF / 3.3f; + } } + TMR3_COUNT_TORQUE_NULL++; } else { + if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell) + FORCE_pulse_sum = 0.0f; + dac_1 = FORCE_VREF / 3.3f; + spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f)); + } else if (SENSING_MODE == 1) { + PresA_pulse_sum = 0.0f; + PresB_pulse_sum = 0.0f; + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f)); + spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f)); + } CONTROL_UTILITY_MODE = MODE_NO_ACT; TMR3_COUNT_TORQUE_NULL = 0; - CUR_TORQUE_sum = 0; - CUR_TORQUE_mean = 0; - - spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0f)); - - dac_1 = TORQUE_VREF / 3.3f; - } - TMR3_COUNT_TORQUE_NULL++; break; } - case MODE_FIND_HOME: { + case MODE_FIND_HOME: + { + static int cnt_findhome = 0; + static int cnt_terminate_findhome = 0; + static float FINDHOME_POSITION_pulse = 0.0f; + static float FINDHOME_POSITION_pulse_OLD = 0.0f; + static float FINDHOME_VELOCITY_pulse = 0.0f; + static float REF_POSITION_FINDHOME_INIT = 0.0f; + if (FINDHOME_STAGE == FINDHOME_INIT) { - REFERENCE_MODE=MODE_REF_NO_ACT; + REFERENCE_MODE = MODE_REF_FINDHOME; cnt_findhome = 0; - cnt_vel_findhome = 0; + cnt_terminate_findhome = 0; pos.ref = pos.sen; vel.ref = 0.0f; + REF_POSITION_FINDHOME = pos.ref; FINDHOME_STAGE = FINDHOME_GOTOLIMIT; } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) { - int cnt_check_enc = (TMR_FREQ_5k/20); + int cnt_check_enc = (TMR_FREQ_5k/20); // 5000/20 = 250tic = 50msec if(cnt_findhome%cnt_check_enc == 0) { - FINDHOME_POSITION = pos.sen; - FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD; - FINDHOME_POSITION_OLD = FINDHOME_POSITION; + FINDHOME_POSITION_pulse = pos.sen*ENC_PULSE_PER_POSITION; + FINDHOME_VELOCITY_pulse = FINDHOME_POSITION_pulse - FINDHOME_POSITION_pulse_OLD; + FINDHOME_POSITION_pulse_OLD = FINDHOME_POSITION_pulse; } cnt_findhome++; - if (abs(FINDHOME_VELOCITY) <= 1) { - cnt_vel_findhome = cnt_vel_findhome + 1; + if (fabs(FINDHOME_VELOCITY_pulse) <= 1) { + cnt_terminate_findhome = cnt_terminate_findhome + 1; } else { - cnt_vel_findhome = 0; + cnt_terminate_findhome = 0; } - if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec + if ((cnt_terminate_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec + double GOTOHOME_SPEED = 10.0f; // 20mm/s or 20deg/s if (HOMEPOS_OFFSET > 0) { - pos.ref = pos.ref + 12.0f; + REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k; } else { - pos.ref = pos.ref - 12.0f; + REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k; } - - CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; - } else { - ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10)); - INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10); - REF_POSITION = 0; - REF_VELOCITY = 0; - FINDHOME_POSITION = 0; - FINDHOME_POSITION_OLD = 0; - FINDHOME_VELOCITY = 0; + ENC_SET((long)((long)HOMEPOS_OFFSET*10)); + REF_POSITION_FINDHOME_INIT = (float)((long)HOMEPOS_OFFSET*10); + FINDHOME_POSITION_pulse = 0; + FINDHOME_POSITION_pulse_OLD = 0; + FINDHOME_VELOCITY_pulse = 0; + cnt_findhome = 0; - cnt_vel_findhome = 0; + cnt_terminate_findhome = 0; + pos.ref = 0.0f; FINDHOME_STAGE = FINDHOME_ZEROPOSE; - - - cnt_findhome = 0; - pos.ref = 0.0f; - vel.ref = 0.0f; - pos.ref_home_pos = 0.0f; - vel.ref_home_pos = 0.0f; } } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) { + int T_move = 2*TMR_FREQ_5k; - pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS; - vel.ref = 0.0f; - alpha_trans = 0.0f; - - pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] - vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] - pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] - CONTROL_MODE = MODE_JOINT_CONTROL; + REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT; cnt_findhome++; if (cnt_findhome >= T_move) { cnt_findhome = 0; pos.ref = 0.0f; - vel.ref = 0.0f; - pos.ref_home_pos = 0.0f; - vel.ref_home_pos = 0.0f; FINDHOME_STAGE = FINDHOME_INIT; CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; - REFERENCE_MODE=MODE_REF_DIRECT; - } - } - - break; - } - - case MODE_PRESSURE_SENSOR_NULLING: { - // DAC Voltage reference set - if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { - CUR_PRES_A_sum += pres_A.sen; - CUR_PRES_B_sum += pres_B.sen; - - if (TMR3_COUNT_PRES_NULL % 10 == 0) { - CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f; - CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f; - CUR_PRES_A_sum = 0; - CUR_PRES_B_sum = 0; - - float VREF_NullingGain = 0.0003f; - PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean; - PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean; - - if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f; - if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f; - if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; - if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; - - if (DIR_VALVE_ENC > 0) { - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; - } else { - dac_1 = PRES_B_VREF / 3.3f; - dac_2 = PRES_A_VREF / 3.3f; - } - } - } else { - CONTROL_UTILITY_MODE = MODE_NO_ACT; - TMR3_COUNT_PRES_NULL = 0; - CUR_PRES_A_sum = 0; - CUR_PRES_B_sum = 0; - CUR_PRES_A_mean = 0; - CUR_PRES_B_mean = 0; - - spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f)); - spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f)); - - if (DIR_VALVE_ENC > 0) { - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; - } else { - dac_1 = PRES_B_VREF / 3.3f; - dac_2 = PRES_A_VREF / 3.3f; + REFERENCE_MODE = MODE_REF_DIRECT; } } - TMR3_COUNT_PRES_NULL++; - break; - } - - case MODE_DDV_DEADZONE_AND_CENTER: { - if (FINDDZ_STAGE == FINDDZ_INIT) { - cnt_finddz = 0; - cnt_vel_finddz = 0; - temp_pos_ref = pos.sen; - temp_pos_ref_offset = 0.0f; - vel.ref = 0.0f; - FINDDZ_STAGE = FINDDZ_START1; - flag_finddz = 1; - FINDDZ_POSITION = pos.sen; - FINDDZ_POSITION_OLD = FINDDZ_POSITION; - - temp_P_GAIN = P_GAIN_JOINT_POSITION; - temp_I_GAIN = I_GAIN_JOINT_POSITION; - temp_VELOCITY_COMP_GAIN = VELOCITY_COMP_GAIN; - - P_GAIN_JOINT_POSITION = 1.0f; - I_GAIN_JOINT_POSITION = 0.0f; - VELOCITY_COMP_GAIN = 0; - FLAG_VALVE_DEADZONE = 0; - alpha_trans = 0.0f; - REFERENCE_MODE=MODE_REF_NO_ACT; - - } else if (FINDDZ_STAGE == FINDDZ_START1) { - cnt_finddz++; - CONTROL_MODE = MODE_JOINT_CONTROL; - FINDDZ_POSITION = pos.sen; - FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD; - FINDDZ_POSITION_OLD = FINDDZ_POSITION; - - if (temp_P_GAIN > 0.0f) temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*temp_P_GAIN; - else temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*20.0f; - - pos.ref = temp_pos_ref + temp_pos_ref_offset; - - if (cnt_finddz > 10000) FINDDZ_STAGE = FINDDZ_START2; - - - } else if (FINDDZ_STAGE == FINDDZ_START2) { - CONTROL_MODE = MODE_JOINT_CONTROL; - FINDDZ_POSITION = pos.sen; - FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD; - FINDDZ_POSITION_OLD = FINDDZ_POSITION; - - if (flag_finddz > 0) { - temp_pos_ref = temp_pos_ref + 1.0f; - if (FINDDZ_VELOCITY > 0) { - cnt_vel_finddz = cnt_vel_finddz + 1; - } - if (cnt_vel_finddz > 5) { - VALVE_DEADZONE_PLUS = (int)(I_REF_fil/mA_PER_pulse); -// temp_VALVE_DEADZONE_PLUS = temp_VALVE_DEADZONE_PLUS + I_REF_fil/mA_PER_pulse; - cnt_vel_finddz = 0; - flag_finddz = - flag_finddz - 1; - } - - } else if (flag_finddz < 0) { - temp_pos_ref = temp_pos_ref - 1.0f; - if (FINDDZ_VELOCITY < 0) { - cnt_vel_finddz = cnt_vel_finddz + 1; - } - if (cnt_vel_finddz > 5) { - VALVE_DEADZONE_MINUS = (int)(I_REF_fil/mA_PER_pulse); -// temp_VALVE_DEADZONE_MINUS = temp_VALVE_DEADZONE_MINUS + I_REF_fil/mA_PER_pulse; - cnt_vel_finddz = 0; - flag_finddz = - flag_finddz + 1; - } - } - if (abs(flag_finddz) >= 6) { - FINDDZ_STAGE = FINDDZ_STOP; -// VALVE_DEADZONE_PLUS = (int)(temp_VALVE_DEADZONE_PLUS / (double)flag_finddz * 2.0f); -// VALVE_DEADZONE_MINUS = (int)(temp_VALVE_DEADZONE_MINUS / (double)flag_finddz * 2.0f); - spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); - } - pos.ref = temp_pos_ref + temp_pos_ref_offset; - - } else if (FINDDZ_STAGE == FINDDZ_STOP) { - FINDDZ_STAGE = FINDDZ_INIT; - CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL; - P_GAIN_JOINT_POSITION = temp_P_GAIN; - I_GAIN_JOINT_POSITION = temp_I_GAIN; - VELOCITY_COMP_GAIN = temp_VELOCITY_COMP_GAIN; - flag_finddz = 0; - FLAG_VALVE_DEADZONE = 1; - REFERENCE_MODE=MODE_REF_DIRECT; - } break; } - - case MODE_SYSTEM_ID: { - freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.0f; - valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3); - CONTROL_MODE = MODE_VALVE_OPEN_LOOP; - cnt_sysid++; - if (freq_sysid_Iref >= 300) { - cnt_sysid = 0; - CONTROL_UTILITY_MODE = MODE_NO_ACT; - } - break; - } - - case MODE_FREQ_TEST: { - float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3); - if(valve_pos_ref >= 0) { - valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f; - } else { - valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f; - } - ref_array[cnt_freq_test] = valve_pos_ref; - if(value>=(float) VALVE_CENTER) { - pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - } else { - pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - - CONTROL_MODE = MODE_VALVE_POSITION_CONTROL; - cnt_freq_test++; - if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) { - buffer_data_size = cnt_freq_test; - cnt_freq_test = 0; - cnt_send_buffer = 0; - freq_test_valve_ref = freq_test_valve_ref * 1.05f; - if (freq_test_valve_ref >= 400) { - CONTROL_UTILITY_MODE = MODE_NO_ACT; - CONTROL_MODE = MODE_NO_ACT; - CAN_TX_PWM((int16_t) (1)); //1300 - } - CONTROL_MODE = MODE_NO_ACT; - CONTROL_UTILITY_MODE = MODE_SEND_OVER; - - } - break; - } - - case MODE_STEP_TEST: { - float valve_pos_ref = 0.0f; - if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) { - valve_pos_ref = 0.0f; - } else { - valve_pos_ref = 10000.0f; - } - if(valve_pos_ref >= 0) { - valve_pos.ref = (double)VALVE_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f; - } else { - valve_pos.ref = (double)VALVE_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f; - } - ref_array[cnt_step_test] = valve_pos_ref; - if(value>=(float) VALVE_CENTER) { - pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - } else { - pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - - CONTROL_MODE = MODE_VALVE_POSITION_CONTROL; - cnt_step_test++; - if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) { - buffer_data_size = cnt_step_test; - cnt_step_test = 0; - cnt_send_buffer = 0; - CONTROL_UTILITY_MODE = MODE_SEND_OVER; - CONTROL_MODE = MODE_NO_ACT; - } - - break; - } - default: break; } - - // CONTROL MODE ------------------------------------------------------------ switch (CONTROL_MODE) { case MODE_NO_ACT: { @@ -921,125 +714,93 @@ VALVE_POS_CONTROL(valve_pos.ref); V_out = Vout.ref; } else if (CURRENT_CONTROL_MODE == 0) { //PWM - V_out = valve_pos.ref; + V_out = valve_pos.ref; } else { - I_REF = valve_pos.ref * 0.001f; + I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA + float I_MAX = 10.0f; // Max : 10mA + if (I_REF > I_MAX) { + I_REF = I_MAX; + } else if (I_REF < -I_MAX) { + I_REF = -I_MAX; + } } break; } - case MODE_JOINT_CONTROL: { - - double torq_ref = 0.0f; - pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] - vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] - pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] - - //K & D Low Pass Filter - float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz - K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D); - D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D); - - torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] + case MODE_JOINT_CONTROL: + { - // torque feedback - torq.err = torq_ref - torq.sen; //[N] - torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] - - if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { - - double I_REF_POS = 0.0f; - double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback - double I_REF_VC = 0.0f; // I_REF for velocity compensation - - double temp_vel_pos = 0.0f; - double temp_vel_torq = 0.0f; - double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control + float temp_vel_pos = 0.0f; // desired velocity for position control + float temp_vel_FT = 0.0f; // desired velocity for force/torque control + float temp_vel_ff = 0.0f; // desired velocity for feedforward control + float temp_vel = 0.0f; - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode - temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s - // L when P-gain = 100, f_cut = 10Hz L feedforward velocity - } else if ((OPERATING_MODE & 0x01) == 1) { - temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s - // L when P-gain = 100, f_cut = 10Hz L feedforward velocity - } - if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); - else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); - - // velocity compensation for torque control - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode - I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); - // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s - temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s - // L feedforward velocity - } else if ((OPERATING_MODE & 0x01) == 1) { - I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive. - // temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s - temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s - // L feedforward velocity - } - if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f)))); - else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f)))); - // L velocity(rad/s or mm/s) >> I_ref(mA) - // Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k; - // Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm; + float wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control + + pos.err = pos.ref - pos.sen; // Unit : mm or deg + vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s + + // position control command =============================================================================================================================================== + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s + // L when P-gain = 100, f_cut = 10Hz + } else { + temp_vel_pos = 0.01f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s + // L when P-gain = 100, f_cut = 10Hz + } - I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB); - - // Anti-windup for FT - if (I_GAIN_JOINT_TORQUE != 0) { - double I_MAX = 10.0f; // Maximum Current : 10mV - double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f); - if (I_REF > I_MAX) { - double I_rem = I_REF - I_MAX; - I_rem = Ka*I_rem; - I_REF = I_MAX; - torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k; - } else if (I_REF < -I_MAX) { - double I_rem = I_REF - (-I_MAX); - I_rem = Ka*I_rem; - I_REF = -I_MAX; - torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k; - } - } - + // torque control command =============================================================================================================================================== + float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f)); + K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING; + D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER; + + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm + torq.err = torq_ref_act - torq.sen; + torq.err_int += torq.err/((float)TMR_FREQ_5k); + temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s } else { - float VALVE_POS_RAW_FORCE_FB = 0.0f; - float VALVE_POS_RAW_FORCE_FF = 0.0f; - float VALVE_POS_RAW = 0.0f; - - VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen)) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); - - VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f; + float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N + force.err = force_ref_act - force.sen; + force.err_int += force.err/((float)TMR_FREQ_5k); + temp_vel_FT = 0.0001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s + } + + // velocity feedforward command ======================================================================================================================================== + if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s + } else { + temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s + } - VALVE_POS_RAW = VALVE_POS_RAW_FORCE_FB + VALVE_POS_RAW_FORCE_FF; - - - if (VALVE_POS_RAW >= 0) { - valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_PLUS; - } else { - valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_MINUS; + // command integration ================================================================================================================================================= + temp_vel = (1.0f - alpha_trans) * temp_vel_pos + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward + + float Qact = 0.0f; // required flow rate + if( temp_vel > 0.0f ) { + Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM + I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d; + } else { + Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM + I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d; + } + + // Anti-windup for FT + if (I_GAIN_JOINT_TORQUE != 0.0f) { + float I_MAX = 10.0f; // Maximum Current : 10mA + float Ka = 2.0f; + if (I_REF > I_MAX) { + float I_rem = I_REF - I_MAX; + I_REF = I_MAX; + float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] + torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! + } else if (I_REF < -I_MAX) { + double I_rem = I_REF - (-I_MAX); + I_REF = -I_MAX; + float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] + torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! } - - if(I_GAIN_JOINT_TORQUE != 0) { - double Ka = 2.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; - if(valve_pos.ref>VALVE_MAX_POS) { - double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; - valve_pos_rem = valve_pos_rem * Ka; - valve_pos.ref = VALVE_MAX_POS; - torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; - } else if(valve_pos.ref < VALVE_MIN_POS) { - double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS; - valve_pos_rem = valve_pos_rem * Ka; - valve_pos.ref = VALVE_MIN_POS; - torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; - } - } - - VALVE_POS_CONTROL(valve_pos.ref); - V_out = (float) Vout.ref; } - torq_ref_past = torq_ref; break; } @@ -1048,79 +809,78 @@ break; } - case MODE_JOINT_ADAPTIVE_BACKSTEPPING: { - - - float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 - float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 - - V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f - - //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 - float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 - - float g3_prime = 0.0f; - if (torq.sen > Amm*(Ps-Pt)*0.000001f) { - g3_prime = 1.0f; - } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { - g3_prime = -1.0f; - } else { - if ((value-VALVE_CENTER) > 0) { - g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); -// g3_prime = sqrt(Ps-Pt); - } else { - g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); -// g3_prime = sqrt(Ps-Pt); - } - } - float tau = 0.01f; - float K_valve = 0.0004f; - - float x_v = 0.0f; //x_v : -1~1 - if(value>=VALVE_CENTER) { - x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - } else { - x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - float f4 = -x_v/tau; - float g4 = K_valve/tau; - - float torq_ref_dot = torq.ref_diff * 500.0f; - - pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] - vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] - pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] - - torq.err = torq.ref - torq.sen; //[N] - torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] - - float k3 = 2000.0f; //2000 //20000 - float k4 = 10.0f; - float rho3 = 3.2f; - float rho4 = 10000000.0f; //25000000.0f; - float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime); - if (x_4_des > 1) x_4_des = 1; - else if (x_4_des < -1) x_4_des = -1; - - if (x_4_des > 0) { - valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER; - } else { - valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER; - } - - float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k; - x_4_des_old = x_4_des; - V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4; - - float rho_a = 0.00001f; - float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err); - a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k; - - if(a_hat > -3000000.0f) a_hat = -3000000.0f; - else if(a_hat < -30000000.0f) a_hat = -30000000.0f; - - break; - } +// case MODE_JOINT_ADAPTIVE_BACKSTEPPING: { +// +// float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 +// float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 +// +// V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f +// +// //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 +// float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 +// +// float g3_prime = 0.0f; +// if (torq.sen > Amm*(Ps-Pt)*0.000001f) { +// g3_prime = 1.0f; +// } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { +// g3_prime = -1.0f; +// } else { +// if ((value-VALVE_CENTER) > 0) { +// g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); +//// g3_prime = sqrt(Ps-Pt); +// } else { +// g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); +//// g3_prime = sqrt(Ps-Pt); +// } +// } +// float tau = 0.01f; +// float K_valve = 0.0004f; +// +// float x_v = 0.0f; //x_v : -1~1 +// if(value>=VALVE_CENTER) { +// x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); +// } else { +// x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); +// } +// float f4 = -x_v/tau; +// float g4 = K_valve/tau; +// +// float torq_ref_dot = torq.ref_diff * 500.0f; +// +// pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] +// vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] +// pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] +// +// torq.err = torq.ref - torq.sen; //[N] +// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] +// +// float k3 = 2000.0f; //2000 //20000 +// float k4 = 10.0f; +// float rho3 = 3.2f; +// float rho4 = 10000000.0f; //25000000.0f; +// float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime); +// if (x_4_des > 1) x_4_des = 1; +// else if (x_4_des < -1) x_4_des = -1; +// +// if (x_4_des > 0) { +// valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER; +// } else { +// valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER; +// } +// +// float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k; +// x_4_des_old = x_4_des; +// V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4; +// +// float rho_a = 0.00001f; +// float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err); +// a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k; +// +// if(a_hat > -3000000.0f) a_hat = -3000000.0f; +// else if(a_hat < -30000000.0f) a_hat = -30000000.0f; +// +// break; +// } default: break; @@ -1136,13 +896,10 @@ double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; - if (FLAG_VALVE_DEADZONE) { - if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA - else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA - else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA - } else { - I_REF_fil_DZ = I_REF_fil; - } + if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA + else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA + else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA + I_ERR = I_REF_fil_DZ - (double)cur.sen; I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; @@ -1184,10 +941,8 @@ VALVE_PWM_RAW = -V_MAX; I_ERR_INT = I_ERR_INT - V_rem * 0.0002f; } - Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse; } else { VALVE_PWM_RAW = I_REF * mV_PER_mA; - Cur_Valve_Open_pulse = I_REF / mA_PER_pulse; } //////////////////////////////////////////////////////////////////////////// @@ -1202,6 +957,7 @@ if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f); else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f); else V_out = (float) (CUR_PWM_lin); + } else { //////////////////////////sw valve // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f; @@ -1209,9 +965,9 @@ else V_out = 0.0f; } - /******************************************************* - *** PWM - ********************************************************/ + //////////////////////////////////////////////////////////////////// + /////////////////// PWM Command /////////////////////////////////// + //////////////////////////////////////////////////////////////////// if(DIR_VALVE<0) { V_out = -V_out; } @@ -1239,38 +995,29 @@ TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); - + //////////////////////////////////////////////////////////////////////////// + ////////////////////// Data transmission through CAN ////////////////////// + //////////////////////////////////////////////////////////////////////////// + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { + // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.1f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } + CAN_TX_POSITION_FT((int16_t) (pos.sen*ENC_PULSE_PER_POSITION*0.25f), (int16_t) (vel.sen*ENC_PULSE_PER_POSITION*0.01f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } } - // PWM, ID:1300 + // Valve Position (ID:1300) if (flag_data_request[1] == HIGH) { - CAN_TX_PWM((int16_t) (V_out)); //1300 + CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); } - // Reference Current, Current Current (ID:1400) + // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) if (flag_data_request[2] == HIGH) { - CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400 - } - - // Reference Current, Current Current (ID:1700) - if (flag_data_request[3] == HIGH) { - CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700 + CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM)); } TMR2_COUNT_CAN_TX = 0;
--- a/setting.h Tue Apr 20 00:42:45 2021 +0000 +++ b/setting.h Wed Apr 21 04:20:39 2021 +0000 @@ -19,12 +19,15 @@ #define FREQ_20k 20000.0f #define FREQ_40k 40000.0f #define DT_500 0.002f -#define DT_1k 0.0005f +#define DT_1k 0.001f #define DT_5k 0.0002f #define DT_10k 0.0001f #define DT_20k 0.00005f #define DT_40k 0.000025f +//#define TMR_FREQ_10k 10000 +#define TMR_FREQ_5k 5000 + extern DigitalOut check; extern DigitalOut check_2; extern AnalogOut dac_1; @@ -79,9 +82,6 @@ #define SYSFREQ 200000000 #define PBCLK 100000000 -//#define TMR_FREQ_10k 10000 -#define TMR_FREQ_5k 5000 - #define FALSE 0 #define TRUE 1 #define OUTPUT 0 @@ -109,6 +109,8 @@ extern uint8_t CONTROL_MODE; extern uint8_t OPERATING_MODE; extern uint8_t SENSING_MODE; +extern uint8_t SUPPLY_PRESSURE_UPDATE; + extern uint8_t CONTROL_UTILITY_MODE; extern uint8_t CURRENT_CONTROL_MODE; extern uint8_t FLAG_VALVE_DEADZONE; @@ -169,16 +171,14 @@ extern float PISTON_AREA_alpha; extern float alpha3; - -extern int16_t PRES_SUPPLY; -extern int16_t PRES_RETURN; +extern float PRES_SUPPLY_NOM; +extern float PRES_SUPPLY; extern int16_t ENC_LIMIT_PLUS; extern int16_t ENC_LIMIT_MINUS; extern int16_t STROKE; - extern float Amm; extern float beta; extern float Ps; @@ -209,11 +209,13 @@ extern float DAC_REF; extern float DAC_RESOL; +extern float REF_FORCE; +extern float REF_TORQUE; extern float REF_POSITION; extern float REF_VELOCITY; -extern float REF_TORQUE; -extern float REF_TORQUE_OLD; -extern float REF_PRES_DIFF; + +extern float REF_POSITION_FINDHOME; + extern int16_t REF_PWM; extern int16_t REF_VALVE_POSITION; extern int16_t REF_CURRENT; @@ -221,19 +223,11 @@ extern int REF_MOVE_TIME_5k; extern int INIT_REF_PWM; extern int INIT_REF_VALVE_POS; -extern int32_t INIT_REF_POS; extern int INIT_REF_VEL; extern int INIT_REF_TORQUE; extern int INIT_REF_PRES_DIFF; extern int INIT_REF_CURRENT; -extern int CUR_POSITION; -extern int CUR_VELOCITY; -extern float CUR_TORQUE; -extern float CUR_PRES_A; -extern float CUR_PRES_B; -extern int CUR_VALVE_POSITION; - extern unsigned int TMR2_COUNT_LED1; extern unsigned int TMR2_COUNT_LED2; extern unsigned int TMR2_COUNT_CAN_TX; @@ -254,14 +248,12 @@ extern int cnt_buffer; extern float CUR_CURRENT_mA; -extern float CUR_PRES_A_BAR; -extern float CUR_PRES_B_BAR; extern float CUR_TORQUE_NM; extern float CUR_TORQUE_NM_PRESS; +extern float FORCE_VREF; extern float PRES_A_VREF; extern float PRES_B_VREF; -extern float TORQUE_VREF; extern float VALVE_PWM_RAW_FB; extern float VALVE_PWM_RAW_FF; @@ -303,11 +295,9 @@ extern float CUR_PRES_B_sum; extern float CUR_PRES_A_mean; extern float CUR_PRES_B_mean; -extern float CUR_TORQUE_sum; -extern float CUR_TORQUE_mean; -extern float PRES_A_NULL; -extern float PRES_B_NULL; -extern float TORQUE_NULL; +extern float PRES_A_NULL_pulse; +extern float PRES_B_NULL_pulse; +extern float FORCE_NULL_pulse; extern float Ref_Valve_Pos_Old; @@ -367,17 +357,6 @@ extern int fl_temp_cnt2; extern int cur_vel_sum; -extern float Cur_Valve_Open_pulse; - -// find home -extern int CUR_VELOCITY_OLD; -extern int cnt_findhome; -extern int cnt_vel_findhome; -extern int FINDHOME_VELOCITY; -extern int FINDHOME_VELOCITY_OLD; -extern int FINDHOME_POSITION; -extern int FINDHOME_POSITION_OLD; - extern int cnt_finddz; extern int cnt_vel_finddz; extern int flag_finddz; @@ -449,6 +428,7 @@ extern float PWM_out; extern double K_v; +extern double C_d; extern double mV_PER_mA; extern double mV_PER_pulse; extern double mA_PER_pulse;