Sungwoo Kim
LIGHT2
Diff: main.cpp
- Revision:
- 251:91d3ec3a087e
- Parent:
- 250:f69cd0553be0
- Child:
- 252:a08f301231f1
diff -r f69cd0553be0 -r 91d3ec3a087e main.cpp
--- a/main.cpp Wed Jun 29 00:19:21 2022 +0000
+++ b/main.cpp Fri Jul 01 05:37:08 2022 +0000
@@ -356,24 +356,21 @@
}
}
Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+ int R_FET = 1.5f;
+ int R_Motor = 5.4f;
+ Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET;
}
// Valve feedforward for SW valve
void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS)
{
int i = 0;
- float valve_pos_ref_dz = 0.0f;
if(REF_VALVE_POS > VALVE_MAX_POS) {
REF_VALVE_POS = VALVE_MAX_POS;
} else if(REF_VALVE_POS < VALVE_MIN_POS) {
REF_VALVE_POS = VALVE_MIN_POS;
}
- if (REF_VALVE_POS >= 0) {
- valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER;
- } else {
- valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER;
- }
- valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen);
+ valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen);
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;
@@ -383,16 +380,19 @@
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(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) REF_VALVE_POS <= 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]);
+ VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - 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];
+ 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) REF_VALVE_POS - 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;
+ int R_FET = 1.5f;
+ int R_Motor = 5.4f;
+ Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET;
}
// PWM duty vs. voltage output of L6205 in STM board
@@ -544,19 +544,20 @@
for (int ij = 0; ij<150; ij++) {
ADC3->CR2 |= 0x40000000;
- LVDT_new = ((float)ADC3->DR);
+ LVDT_new = ((float)ADC3->DR - 2047.0f);
+ if(DIR_VALVE_ENC < 0) LVDT_new = 0.0f - LVDT_new;
LVDT_sum = LVDT_sum + LVDT_new;
}
LVDT_H = 0;
LVDT_L = 1;
- LVDT_new = LVDT_sum * 0.01f*2.0f;
+ LVDT_new = (LVDT_sum + 2047.0f * 150.0f) * 0.01f*2.0f;
float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
valve_pos.sen = LVDT_LPF;
- if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
+
CNT_TMR4++;
}
@@ -796,6 +797,9 @@
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]);
+ int R_FET = 1.5f;
+ int R_Motor = 5.4f;
+ Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET;
} else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
VALVE_POS_TMP = 0;
data_num = 0;
@@ -898,7 +902,8 @@
} 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_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+ valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 2048 / 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) {
@@ -920,7 +925,8 @@
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
+// if(DZ_index >= 128) {
+ if(DZ_index >= 4096) {
FIRST_DZ = valve_pos_raw.ref;
DZ_NUM = 2;
Ref_Valve_Pos_Old = FIRST_DZ;
@@ -934,7 +940,8 @@
} 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_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
+ valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 2048 / 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) {
@@ -958,7 +965,8 @@
VALVE_DZ_timer = 0;
DZ_index= DZ_index * 2;
- if(DZ_index >= 128) {
+// if(DZ_index >= 128) {
+ if(DZ_index >= 4096) {
SECOND_DZ = valve_pos_raw.ref;
VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
first_check = 0;
@@ -979,7 +987,8 @@
} 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_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
+// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
+ valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 2048 / 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) {
@@ -1000,7 +1009,8 @@
}
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
+// if(DZ_index >= 128) {
+ if(DZ_index >= 4096) {
FIRST_DZ = valve_pos_raw.ref;
DZ_NUM = 2;
Ref_Valve_Pos_Old = FIRST_DZ;
@@ -1014,7 +1024,8 @@
} 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_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
+// valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
+ valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 2048 / 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) {
@@ -1038,7 +1049,8 @@
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
- if(DZ_index >= 128) {
+// if(DZ_index >= 128) {
+ if(DZ_index >= 4096) {
SECOND_DZ = valve_pos_raw.ref;
VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
first_check = 0;
@@ -1285,12 +1297,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_DZ(valve_pos_pulse);
+ 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.ref);
V_out = Vout.ref;
}
break;
@@ -1387,6 +1399,7 @@
////////////////////////////////////////////////////////////////////
/////////////////// PWM Command ///////////////////////////////////
////////////////////////////////////////////////////////////////////
+
if(DIR_VALVE<0) {
V_out = -V_out;
}
@@ -1438,7 +1451,6 @@
valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
}
CAN_TX_PWM((int16_t)(valve_pos_can));
-// CAN_TX_PWM((int16_t)(valve_pos.sen));
}
}