Sungwoo Kim
LIGHT2
Diff: main.cpp
- Revision:
- 59:f308b1656d9c
- Parent:
- 58:2eade98630e2
- Child:
- 60:64181f1d3e60
--- a/main.cpp Mon Mar 09 12:43:04 2020 +0000
+++ b/main.cpp Mon Mar 16 04:50:54 2020 +0000
@@ -292,7 +292,7 @@
break;
}
}
- V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
+ Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
}
#define LT_MAX_IDX 57
@@ -489,10 +489,10 @@
CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
CUR_TORQUE_sum = 0;
- TORQUE_VREF += 0.0003 * (0.0f - CUR_TORQUE_mean);
+ TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean);
if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
- if (TORQUE_VREF < 0) TORQUE_VREF = 0;
+ if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
dac_1 = TORQUE_VREF / 3.3f;
@@ -594,8 +594,8 @@
cnt_findhome = 0;
cnt_vel_findhome = 0;
//REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
- pos.ref_home_pos = pos.sen;
- vel.ref_home_pos = 0.0f;
+ pos.ref = pos.sen;
+ vel.ref = 0.0f;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
int cnt_check_enc = (TMR_FREQ_5k/500);
@@ -614,20 +614,19 @@
if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
//REFERENCE_MODE = MODE_REF_NO_ACT;
- if (HOMEPOS_OFFSET > 0) pos.ref_home_pos = pos.ref_home_pos + 2.0f;
- else pos.ref_home_pos = pos.ref_home_pos - 2.0f;
- pos.err = pos.ref_home_pos - pos.sen;
- float VALVE_POS_RAW_POS_FB = 0.0f;
- VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
- VALVE_POS_CONTROL(valve_pos.ref);
+ if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 2.0f;
+ else pos.ref = pos.ref - 2.0f;
+
+// pos.err = pos.ref_home_pos - pos.sen;
+// float VALVE_POS_RAW_POS_FB = 0.0f;
+// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
+// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
+// VALVE_POS_CONTROL(valve_pos.ref);
+
+ CONTROL_MODE = MODE_JOINT_CONTROL;
+ alpha_trans = 0.0f;
+
- //float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
- //I_REF = 0.04f*wn_Pos*((float)joint_pos_err/ENC_PULSE_PER_POSITION);
- //// L velocity >> mA convert
- //if(I_REF>5.0f) I_REF = 5.0f;
- //if(I_REF<-5.0f) I_REF = -5.0f;
- //FLAG_CURRNET_CONTROL = true;
} else {
ENC_SET(HOMEPOS_OFFSET);
INIT_REF_POS = HOMEPOS_OFFSET;
@@ -642,15 +641,18 @@
}
} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
int T_move = 2*TMR_FREQ_5k;
- pos.ref_home_pos = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
- vel.ref_home_pos = 0.0f;
+ pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
+ vel.ref = 0.0f;
// input for position control
- pos.err = pos.ref_home_pos - (float)pos.sen;
- float VALVE_POS_RAW_POS_FB = 0.0f;
- VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
- VALVE_POS_CONTROL(valve_pos.ref);
+
+ CONTROL_MODE = MODE_JOINT_CONTROL;
+ alpha_trans = 0.0f;
+// pos.err = pos.ref - (float)pos.sen;
+// float VALVE_POS_RAW_POS_FB = 0.0f;
+// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
+// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
+// VALVE_POS_CONTROL(valve_pos.ref);
cnt_findhome++;
if (cnt_findhome >= T_move) {
@@ -829,12 +831,13 @@
// }
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) {
- V_out = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
+ Vout.ref = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
} else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
- V_out = 1000.0f*(ID_index_array[ID_index]);
+ 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;
@@ -842,6 +845,8 @@
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;
@@ -860,7 +865,7 @@
VALVE_POS_AVG_OLD = VALVE_MIN_POS;
}
}
- //ROM_RESET_DATA();
+ ROM_RESET_DATA();
ID_index = 0;
CONTROL_UTILITY_MODE = MODE_NO_ACT;
}
@@ -870,27 +875,27 @@
}
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)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
pos_plus_end = pos.sen;
} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
pos_minus_end = pos.sen;
} else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
} else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
data_num = data_num + 1;
VALVE_POS_TMP = VALVE_POS_TMP + value;
} else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
DDV_POS_AVG = VALVE_POS_TMP / data_num;
START_POS = pos.sen;
VALVE_POS_TMP = 0;
@@ -919,8 +924,8 @@
} else {
DZ_case = 0;
}
-
- CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
+
+ //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
VEL_POINT = 0;
first_check = 1;
@@ -934,7 +939,9 @@
} 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)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ //Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
} 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)) {
@@ -975,7 +982,9 @@
}
} 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)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
} 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)) {
@@ -991,6 +1000,8 @@
else
VEL_POINT = VEL_POINT - 1;
} 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;
@@ -1022,7 +1033,9 @@
}
} else if(DZ_case == 0 && DZ_NUM ==1) {
if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
} 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)) {
@@ -1062,7 +1075,9 @@
}
} else {
if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
} 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)) {
@@ -1078,6 +1093,8 @@
else
VEL_POINT = VEL_POINT - 1;
} 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(VEL_POINT >= 0)
@@ -1098,6 +1115,8 @@
first_check = 0;
VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
+
+ ROM_RESET_DATA();
CONTROL_UTILITY_MODE = MODE_NO_ACT;
DZ_index = 1;
@@ -1109,20 +1128,21 @@
}
case MODE_DDV_POS_VS_FLOWRATE: {
+ CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
VALVE_FR_timer = VALVE_FR_timer + 1;
if(first_check == 0) {
if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
//CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
pos_plus_end = pos.sen;
// CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
} else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
} else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
// CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
- V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+ Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
pos_minus_end = pos.sen;
first_check = 1;
VALVE_FR_timer = 0;
@@ -1133,7 +1153,9 @@
}
} else {
if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
- V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
+ pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
data_num = 0;
valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
@@ -1151,6 +1173,7 @@
} else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) {
FINAL_POS = pos.sen;
one_period_end = 1;
+ V_out = 0.0f;
}
if(one_period_end == 1) {
@@ -1161,7 +1184,6 @@
}
JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
- //ROM_RESET_DATA();
VALVE_FR_timer = 0;
one_period_end = 0;
ID_index= ID_index +1;
@@ -1171,7 +1193,7 @@
if(max_check == 1 && min_check == 1) {
VALVE_POS_NUM = ID_index;
- //ROM_RESET_DATA();
+ ROM_RESET_DATA();
ID_index = 0;
first_check = 0;
VALVE_FR_timer = 0;
@@ -1215,6 +1237,7 @@
V_out = valve_pos.ref;
} else { //SW Valve
VALVE_POS_CONTROL(valve_pos.ref);
+ V_out = Vout.ref;
}
break;
@@ -1435,7 +1458,7 @@
}
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (V_out));
} else if (SENSING_MODE == 1) {
CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}