![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 237:c2cfe349f5c1
- Parent:
- 236:fd7961dfa78a
- Child:
- 238:4e660aa77eb7
--- a/main.cpp Fri Jun 25 12:21:04 2021 +0000 +++ b/main.cpp Wed Jul 07 10:09:21 2021 +0000 @@ -240,7 +240,7 @@ make_delay(); ////// bno rom -// spi_eeprom_write(RID_BNO, (int16_t) 2); +// spi_eeprom_write(RID_BNO, (int16_t) 1); // make_delay(); //////// @@ -541,17 +541,19 @@ break; } - if (((OPERATING_MODE&0b010)>>1) == 0) { + if (((OPERATING_MODE&0b110)>>1) == 0) { K_v = 1.03f; // Q = K_v*sqrt(deltaP)*tanh(C_d*Xv); C_d = 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) { + } 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 + } else if (((OPERATING_MODE&0b110)>>1) == 2) { + C_d = 0.0000845f; // Q = C_d * Valve_pos * sqrt(deltaP*alpha/(1+alpha)) : Valve_pos = 10000, deltaP = 70, alpha = 1 -> Q = 5 } // ===================================================================== @@ -706,6 +708,356 @@ } break; } + + case MODE_DDV_POS_VS_PWM_ID: { + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; + VALVE_ID_timer = VALVE_ID_timer + 1; + + if(VALVE_ID_timer < TMR_FREQ_5k*1) { + Vout.ref = 3000.0f * sin(2.0f*3.14159f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); + } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { + Vout.ref = 1000.0f*(ID_index_array[ID_index]); + } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { + VALVE_POS_TMP = 0; + data_num = 0; + } else if(VALVE_ID_timer < TMR_FREQ_5k*3) { + data_num = data_num + 1; + VALVE_POS_TMP = VALVE_POS_TMP + value; + } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { + Vout.ref = 0.0f; + } else { + VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num; + VALVE_ID_timer = 0; + ID_index= ID_index +1; + } + + if(ID_index>=25) { + int i; + VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; + for(i=0; i<25; i++) { + VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); + if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) { + VALVE_MAX_POS = VALVE_POS_AVG[i]; + VALVE_POS_AVG_OLD = VALVE_MAX_POS; + } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) { + VALVE_MIN_POS = VALVE_POS_AVG[i]; + VALVE_POS_AVG_OLD = VALVE_MIN_POS; + } + } + VALVE_ELECTRIC_CENTER = VALVE_POS_VS_PWM[0]; + spi_eeprom_write(RID_VALVE_ELECTRIC_CENTER, (int16_t) VALVE_ELECTRIC_CENTER); + spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS); + spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS); + for(int i=0; i<25; i++) { + spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) VALVE_POS_VS_PWM[i]); + } + ID_index = 0; + CONTROL_UTILITY_MODE = MODE_NO_ACT; + } + + + break; + } + case MODE_DDV_DEADZONE_AND_CENTER: { + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; + VALVE_DZ_timer = VALVE_DZ_timer + 1; + if(first_check == 0) { + if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; + } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { + Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f; + pos_plus_end = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { + Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f; + pos_minus_end = pos.sen; + } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + data_num = data_num + 1; + VALVE_POS_TMP = VALVE_POS_TMP + value; + } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + DDV_POS_AVG = VALVE_POS_TMP / data_num; + START_POS = pos.sen; + VALVE_POS_TMP = 0; + data_num = 0; + + } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); + FINAL_POS = pos.sen; + + if((FINAL_POS - START_POS)>10) { + DZ_case = 1; + } else if((FINAL_POS - START_POS)<-10) { + DZ_case = -1; + } else { + DZ_case = 0; + } + + first_check = 1; + DZ_DIRECTION = 1; + VALVE_DZ_timer = 0; + Ref_Valve_Pos_Old = DDV_POS_AVG; + DZ_NUM = 1; + DZ_index = 1; + + } + } else { + if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) { + if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; + } + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + + if((FINAL_POS - START_POS)>2) { + DZ_DIRECTION = 1 * DZ_case; + } else if((FINAL_POS - START_POS)<-2) { + DZ_DIRECTION = -1 * DZ_case; + } else { + DZ_DIRECTION = 1 * DZ_case; + } + + VALVE_DZ_timer = 0; + DZ_index= DZ_index *2; + if(DZ_index >= 128) { + FIRST_DZ = valve_pos.ref; + DZ_NUM = 2; + Ref_Valve_Pos_Old = FIRST_DZ; + DZ_index = 1; + DZ_DIRECTION = 1; + } + } + } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) { + if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; + } + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { + Vout.ref = 0.0f; + } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + + if((FINAL_POS - START_POS)>2) { + DZ_DIRECTION = 1 * DZ_case; + } else if((FINAL_POS - START_POS)<-2) { + DZ_DIRECTION = -1 * DZ_case; + } else { + DZ_DIRECTION = -1 * DZ_case; + } + + VALVE_DZ_timer = 0; + DZ_index= DZ_index * 2; + if(DZ_index >= 128) { + SECOND_DZ = valve_pos.ref; + VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); + first_check = 0; + VALVE_DEADZONE_MINUS = (float) FIRST_DZ; + VALVE_DEADZONE_PLUS = (float) SECOND_DZ; + + spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); + spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS); + spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS); + + CONTROL_UTILITY_MODE = MODE_NO_ACT; + DZ_index = 1; + } + } + } else if(DZ_case == 0 && DZ_NUM ==1) { + if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; + } + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + + if((FINAL_POS - START_POS)>2) { + DZ_DIRECTION = 1; + } else if((FINAL_POS - START_POS)<-2) { + DZ_DIRECTION = -1; + } else { + DZ_DIRECTION = 1; + } + VALVE_DZ_timer = 0; + DZ_index= DZ_index *2; + if(DZ_index >= 128) { + FIRST_DZ = valve_pos.ref; + DZ_NUM = 2; + Ref_Valve_Pos_Old = FIRST_DZ; + DZ_index = 1; + DZ_DIRECTION = 1; + } + } + } else { + if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) { + Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f; + } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref > VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; + } + VALVE_POS_CONTROL(valve_pos.ref); + + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { + Vout.ref = 0.0f; + } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + + if((FINAL_POS - START_POS)>2) { + DZ_DIRECTION = -1; + } else if((FINAL_POS - START_POS)<-2) { + DZ_DIRECTION = 1; + } else { + DZ_DIRECTION = 1; + } + + VALVE_DZ_timer = 0; + DZ_index= DZ_index *2; + if(DZ_index >= 128) { + SECOND_DZ = valve_pos.ref; + VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); + first_check = 0; + VALVE_DEADZONE_MINUS = (float) FIRST_DZ; + VALVE_DEADZONE_PLUS = (float) SECOND_DZ; + + spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); + spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS); + spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS); + + CONTROL_UTILITY_MODE = MODE_NO_ACT; + DZ_index = 1; + } + } + } + } + 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_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; + } else { + valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; + } + ref_array[cnt_step_test] = valve_pos_ref; + if(value>=(float) VALVE_ELECTRIC_CENTER) { + pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER); + } else { + pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_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; + } +// if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) +// { +// CONTROL_UTILITY_MODE = MODE_NO_ACT; +// CONTROL_MODE = MODE_NO_ACT; +// CAN_TX_PWM((int16_t) (1)); //1300 +// } + + break; + } + case MODE_SEND_OVER: { + CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300 + CONTROL_UTILITY_MODE = MODE_NO_ACT; + CONTROL_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_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; + } else { + valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; + } + ref_array[cnt_freq_test] = valve_pos_ref; + if(value>=(float) VALVE_ELECTRIC_CENTER) { + pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER); + } else { + pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_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_VOUT((int16_t) (1)); //1300 + } + CONTROL_MODE = MODE_NO_ACT; + CONTROL_UTILITY_MODE = MODE_SEND_OVER; + + } + break; + } + default: break; } @@ -783,39 +1135,84 @@ // 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 + float valve_pos_pulse = 0.0f; 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; + if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve + I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d; + } else { // SW valve + valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))); + } } 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; + if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve + I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d; + } else { // SW valve + valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))); + } } - - float I_MAX = 10.0f; // Maximum Current : 10mA - - // Anti-windup for FT -// if (I_GAIN_JOINT_TORQUE != 0.0f) { - if (I_GAIN_JOINT_TORQUE > 0.001f) { - 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); - } 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); + + + + + if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve + + float I_MAX = 10.0f; // Maximum Current : 10mA + // Anti-windup for FT + // if (I_GAIN_JOINT_TORQUE != 0.0f) { + if (I_GAIN_JOINT_TORQUE > 0.001f) { + 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); + } 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); + } + } else { + if(I_REF > I_MAX) { + I_REF = I_MAX; + } else if (I_REF < -I_MAX) { + I_REF = -I_MAX; + } } - } else { - if(I_REF > I_MAX) { - I_REF = I_MAX; - } else if (I_REF < -I_MAX) { - I_REF = -I_MAX; + } else { //SW valve + float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000 + // Anti-windup for FT + // if (I_GAIN_JOINT_TORQUE != 0.0f) { + if (I_GAIN_JOINT_TORQUE > 0.001f) { + float Ka = 2.0f; + if (valve_pos_pulse > Valve_pos_MAX) { + float valve_pos_rem = valve_pos_pulse - Valve_pos_MAX; + valve_pos_pulse = Valve_pos_MAX; + float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) / ((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); + } else if (valve_pos_pulse < -Valve_pos_MAX) { + double valve_pos_rem = valve_pos_pulse - (-Valve_pos_MAX); + valve_pos_pulse = -Valve_pos_MAX; + float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) / ((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); + } + } else { + if(valve_pos_pulse > 10000.0f) { + valve_pos_pulse = 10000.0f; + } else if(valve_pos_pulse < -10000.0f) { + valve_pos_pulse = -10000.0f; + } } + if (valve_pos_pulse >= 0) { + valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; + } else { + valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; + } + VALVE_POS_CONTROL(valve_pos.ref); + V_out = Vout.ref; } break; } @@ -1021,14 +1418,14 @@ // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); // CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); -// CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); -// CAN_TX_POSITION_FT((int16_t) (logging*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); +// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f)); } } @@ -1039,7 +1436,19 @@ // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) if (flag_data_request[2] == HIGH) { - 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)); + float valve_pos_can = 0.0f; + if(value >= VALVE_ELECTRIC_CENTER) { + valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); + } else { + valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); + } + float valve_pos_ref_can = 0.0f; + if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) { + valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); + } else { + valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); + } + CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can); } TMR2_COUNT_CAN_TX = 0;