![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
rainbow
Diff: main.cpp
- Revision:
- 238:4e660aa77eb7
- Parent:
- 237:c2cfe349f5c1
- Child:
- 239:8ac5c6162bc1
diff -r c2cfe349f5c1 -r 4e660aa77eb7 main.cpp --- a/main.cpp Wed Jul 07 10:09:21 2021 +0000 +++ b/main.cpp Tue Jul 13 01:40:56 2021 +0000 @@ -73,6 +73,7 @@ State pres_B; State cur; // unit : mA State valve_pos; +State valve_pos_raw; State INIT_Vout; State INIT_Valve_Pos; @@ -240,8 +241,8 @@ make_delay(); ////// bno rom -// spi_eeprom_write(RID_BNO, (int16_t) 1); -// make_delay(); + spi_eeprom_write(RID_BNO, (int16_t) 1); + make_delay(); //////// // rom @@ -280,13 +281,13 @@ dac_1 = FORCE_VREF / 3.3f; dac_2 = 0.0f; } else if (SENSING_MODE == 1) { - if (DIR_VALVE_ENC > 0) { +// 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 { +// dac_1 = PRES_B_VREF / 3.3f; +// dac_2 = PRES_A_VREF / 3.3f; +// } } make_delay(); @@ -383,6 +384,43 @@ Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; } +// Valve feedforward for SW valve +void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS_10000) +{ + int i = 0; + float valve_pos_ref_dz = 0.0f; + if(REF_VALVE_POS_10000 > 10000.0f) { + REF_VALVE_POS_10000 = 10000.0f; + } else if(REF_VALVE_POS_10000 < -10000.0f) { + REF_VALVE_POS_10000 = -10000.0f; + } + if (REF_VALVE_POS_10000 >= 0) { + valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; + } else { + valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; + } + valve_pos_err = (float) (valve_pos_ref_dz - value); + valve_pos_err_diff = valve_pos_err - valve_pos_err_old; + valve_pos_err_old = valve_pos_err; + valve_pos_err_sum += valve_pos_err; + if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f; + if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f; + + VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff; + + for(i=0; i<24; i++) { + if(valve_pos_ref_dz >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) valve_pos_ref_dz <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { + if(i==0) { + VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i]); + } else { + VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1]; + } + break; + } + } + Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; +} + // PWM duty vs. voltage output of L6205 in STM board #define LT_MAX_IDX 57 float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f, @@ -457,20 +495,20 @@ 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) { +// if (DIR_VALVE_ENC > 0) { 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_B_PULSE_PER_BAR; - } else { - 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_B_PULSE_PER_BAR; - } +// } else { +// 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_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 + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator 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.0f); // unit : Nm - } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator 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.0f); // unit : N } @@ -786,21 +824,21 @@ 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); + valve_pos_raw.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos_raw.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); + valve_pos_raw.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos_raw.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); + valve_pos_raw.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos_raw.ref); FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>10) { + if((FINAL_POS - START_POS)>1) { DZ_case = 1; - } else if((FINAL_POS - START_POS)<-10) { + } else if((FINAL_POS - START_POS)<-1) { DZ_case = -1; } else { DZ_case = 0; @@ -821,21 +859,21 @@ } 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_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + if(valve_pos_raw.ref <= VALVE_MIN_POS) { + valve_pos_raw.ref = VALVE_MIN_POS; + } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { + valve_pos_raw.ref = VALVE_MAX_POS; } - VALVE_POS_CONTROL(valve_pos.ref); + VALVE_POS_CONTROL(valve_pos_raw.ref); } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { - Ref_Valve_Pos_Old = valve_pos.ref; + Ref_Valve_Pos_Old = valve_pos_raw.ref; FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>2) { + if((FINAL_POS - START_POS)>1) { DZ_DIRECTION = 1 * DZ_case; - } else if((FINAL_POS - START_POS)<-2) { + } else if((FINAL_POS - START_POS)<-1) { DZ_DIRECTION = -1 * DZ_case; } else { DZ_DIRECTION = 1 * DZ_case; @@ -844,7 +882,7 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { - FIRST_DZ = valve_pos.ref; + FIRST_DZ = valve_pos_raw.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; DZ_index = 1; @@ -857,23 +895,23 @@ } 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_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + if(valve_pos_raw.ref <= VALVE_MIN_POS) { + valve_pos_raw.ref = VALVE_MIN_POS; + } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { + valve_pos_raw.ref = VALVE_MAX_POS; } - VALVE_POS_CONTROL(valve_pos.ref); + VALVE_POS_CONTROL(valve_pos_raw.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; + Ref_Valve_Pos_Old = valve_pos_raw.ref; FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>2) { + if((FINAL_POS - START_POS)>1) { DZ_DIRECTION = 1 * DZ_case; - } else if((FINAL_POS - START_POS)<-2) { + } else if((FINAL_POS - START_POS)<-1) { DZ_DIRECTION = -1 * DZ_case; } else { DZ_DIRECTION = -1 * DZ_case; @@ -882,7 +920,7 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index * 2; if(DZ_index >= 128) { - SECOND_DZ = valve_pos.ref; + SECOND_DZ = valve_pos_raw.ref; VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; @@ -902,21 +940,21 @@ } 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_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; + if(valve_pos_raw.ref <= VALVE_MIN_POS) { + valve_pos_raw.ref = VALVE_MIN_POS; + } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { + valve_pos_raw.ref = VALVE_MAX_POS; } - VALVE_POS_CONTROL(valve_pos.ref); + VALVE_POS_CONTROL(valve_pos_raw.ref); } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { - Ref_Valve_Pos_Old = valve_pos.ref; + Ref_Valve_Pos_Old = valve_pos_raw.ref; FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>2) { + if((FINAL_POS - START_POS)>1) { DZ_DIRECTION = 1; - } else if((FINAL_POS - START_POS)<-2) { + } else if((FINAL_POS - START_POS)<-1) { DZ_DIRECTION = -1; } else { DZ_DIRECTION = 1; @@ -924,7 +962,7 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { - FIRST_DZ = valve_pos.ref; + FIRST_DZ = valve_pos_raw.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; DZ_index = 1; @@ -937,23 +975,23 @@ } 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_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; + if(valve_pos_raw.ref <= VALVE_MIN_POS) { + valve_pos_raw.ref = VALVE_MIN_POS; + } else if(valve_pos_raw.ref > VALVE_MAX_POS) { + valve_pos_raw.ref = VALVE_MAX_POS; } - VALVE_POS_CONTROL(valve_pos.ref); + VALVE_POS_CONTROL(valve_pos_raw.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; + Ref_Valve_Pos_Old = valve_pos_raw.ref; FINAL_POS = pos.sen; - if((FINAL_POS - START_POS)>2) { + if((FINAL_POS - START_POS)>1) { DZ_DIRECTION = -1; - } else if((FINAL_POS - START_POS)<-2) { + } else if((FINAL_POS - START_POS)<-1) { DZ_DIRECTION = 1; } else { DZ_DIRECTION = 1; @@ -962,7 +1000,7 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index *2; if(DZ_index >= 128) { - SECOND_DZ = valve_pos.ref; + SECOND_DZ = valve_pos_raw.ref; VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; @@ -980,7 +1018,7 @@ } break; } - + case MODE_STEP_TEST: { float valve_pos_ref = 0.0f; if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) { @@ -989,10 +1027,13 @@ 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; + valve_pos_raw.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; + valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; } + + VALVE_POS_CONTROL(valve_pos_raw.ref); + 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); @@ -1000,7 +1041,7 @@ 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; + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; cnt_step_test++; if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) { buffer_data_size = cnt_step_test; @@ -1024,14 +1065,17 @@ 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; + valve_pos_raw.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; + valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; } + + VALVE_POS_CONTROL(valve_pos_raw.ref); + 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); @@ -1039,7 +1083,7 @@ 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; + CONTROL_MODE = MODE_VALVE_OPEN_LOOP; cnt_freq_test++; if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) { buffer_data_size = cnt_freq_test; @@ -1071,7 +1115,7 @@ case MODE_VALVE_POSITION_CONTROL: { if (OPERATING_MODE == 5) { //SW Valve - VALVE_POS_CONTROL(valve_pos.ref); + VALVE_POS_CONTROL_DZ(valve_pos.ref); V_out = Vout.ref; } else if (CURRENT_CONTROL_MODE == 0) { //PWM V_out = valve_pos.ref; @@ -1100,7 +1144,7 @@ vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s // position control command =============================================================================================================================================== - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s // L when P-gain = 100, f_cut = 10Hz } else { @@ -1110,24 +1154,25 @@ // 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; + K_LPF = (1.0f-alpha_SpringDamper) * K_LPF + alpha_SpringDamper * K_SPRING; + D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + 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 + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode + float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm torq.err = torq_ref_act - torq.sen; torq.err_int += torq.err/((float)TMR_FREQ_5k); temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s } else { - float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N + float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N force.err = force_ref_act - force.sen; +// force.err = 0.0f - force.sen; force.err_int += force.err/((float)TMR_FREQ_5k); temp_vel_FT = 0.001f * (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 + if ((OPERATING_MODE & 0b01) == 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 @@ -1142,6 +1187,7 @@ Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM 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; +// I_REF = Qact*2.0f; } else { // SW valve valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))); } @@ -1149,6 +1195,7 @@ Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM 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; +// I_REF = Qact*2.0f; } else { // SW valve valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))); } @@ -1206,12 +1253,12 @@ 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); +// 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_DZ(valve_pos_pulse); V_out = Vout.ref; } break; @@ -1340,7 +1387,7 @@ I_REF_fil_old = I_REF_fil_DZ; // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV - double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV + double V_MAX = VALVE_VOLTAGE_LIMIT*1000.0f; // Maximum Voltage : 12V = 12000mV double Ka = 3.0f / KP_I; if (VALVE_PWM_RAW > V_MAX) { @@ -1431,7 +1478,8 @@ // Valve Position (ID:1300) if (flag_data_request[1] == HIGH) { - CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); +// CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); + CAN_TX_PWM((int16_t)(alpha3)); } // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400) @@ -1448,6 +1496,9 @@ } else { valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); } + + valve_pos_ref_can = (float)valve_pos.ref; + CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can); }