Sungwoo Kim
2011
Diff: main.cpp
- Revision:
- 226:4e660aa77eb7
- Parent:
- 225:c2cfe349f5c1
- Child:
- 227:8ac5c6162bc1
--- 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);
}