Sungwoo Kim
rainbow
Diff: main.cpp
- Revision:
- 237:c2cfe349f5c1
- Parent:
- 236:fd7961dfa78a
- Child:
- 238:4e660aa77eb7
diff -r fd7961dfa78a -r c2cfe349f5c1 main.cpp
--- 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;