Sungwoo Kim
Original Version of STM Board
Diff: main.cpp
- Revision:
- 60:64181f1d3e60
- Parent:
- 59:f308b1656d9c
- Child:
- 61:bc8c8270f0ab
diff -r f308b1656d9c -r 64181f1d3e60 main.cpp
--- a/main.cpp Mon Mar 16 04:50:54 2020 +0000
+++ b/main.cpp Tue Mar 31 05:12:37 2020 +0000
@@ -835,7 +835,7 @@
VALVE_ID_timer = VALVE_ID_timer + 1;
if(VALVE_ID_timer < TMR_FREQ_5k*1) {
- Vout.ref = 3000.0f * sin(2*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
+ Vout.ref = 3000.0f * sin(2.0f*3.14f*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) {
@@ -908,26 +908,22 @@
} else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
valve_pos.ref = DDV_POS_AVG;
VALVE_POS_CONTROL(valve_pos.ref);
- if(CUR_VELOCITY >= 0)
- VEL_POINT = VEL_POINT + 1;
- else
- VEL_POINT = VEL_POINT - 1;
+
} 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)>100) {
+ if((FINAL_POS - START_POS)>200) {
DZ_case = 1;
- } else if((FINAL_POS - START_POS)<-100) {
+ } else if((FINAL_POS - START_POS)<-200) {
DZ_case = -1;
} 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;
DZ_DIRECTION = 1;
VALVE_DZ_timer = 0;
@@ -939,9 +935,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)) {
- //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;
+ 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)) {
@@ -952,10 +948,7 @@
valve_pos.ref = VALVE_MAX_POS;
}
VALVE_POS_CONTROL(valve_pos.ref);
- if(CUR_VELOCITY >= 0)
- VEL_POINT = VEL_POINT + 1;
- else
- VEL_POINT = VEL_POINT - 1;
+
} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Ref_Valve_Pos_Old = valve_pos.ref;
FINAL_POS = pos.sen;
@@ -968,8 +961,6 @@
DZ_DIRECTION = 1 * DZ_case;
}
- VEL_POINT = 0;
-
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
@@ -982,23 +973,20 @@
}
} 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;
- pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
- CONTROL_MODE = MODE_JOINT_CONTROL;
+ 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)) {
- valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
+ 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);
- if(CUR_VELOCITY >= 0)
- VEL_POINT = VEL_POINT + 1;
- 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)) {
@@ -1006,18 +994,15 @@
FINAL_POS = pos.sen;
if((FINAL_POS - START_POS)>100) {
- DZ_DIRECTION = -1;
+ DZ_DIRECTION = 1 * DZ_case;
} else if((FINAL_POS - START_POS)<-100) {
- DZ_DIRECTION = 1;
+ DZ_DIRECTION = -1 * DZ_case;
} else {
- DZ_DIRECTION = 1;
+ DZ_DIRECTION = -1 * DZ_case;
}
- VEL_POINT = 0;
-
-
VALVE_DZ_timer = 0;
- DZ_index= DZ_index *2;
+ 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));
@@ -1027,15 +1012,15 @@
ROM_RESET_DATA();
- CONTROL_MODE = MODE_NO_ACT;
+ 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)) {
- //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;
+ 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)) {
@@ -1046,10 +1031,7 @@
valve_pos.ref = VALVE_MAX_POS;
}
VALVE_POS_CONTROL(valve_pos.ref);
- if(CUR_VELOCITY >= 0)
- VEL_POINT = VEL_POINT + 1;
- else
- VEL_POINT = VEL_POINT - 1;
+
} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Ref_Valve_Pos_Old = valve_pos.ref;
FINAL_POS = pos.sen;
@@ -1061,8 +1043,6 @@
} else {
DZ_DIRECTION = 1;
}
-
- VEL_POINT = 0;
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
@@ -1075,9 +1055,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;
- pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
- CONTROL_MODE = MODE_JOINT_CONTROL;
+ 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)) {
@@ -1088,24 +1068,20 @@
valve_pos.ref = VALVE_MAX_POS - 1;
}
VALVE_POS_CONTROL(valve_pos.ref);
- if(CUR_VELOCITY >= 0)
- VEL_POINT = VEL_POINT + 1;
- 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)
- DZ_DIRECTION = -1;
- else
+
+ if((FINAL_POS - START_POS)>100) {
DZ_DIRECTION = 1;
- if(abs(FINAL_POS - START_POS) < 100)
+ } else if((FINAL_POS - START_POS)<-100) {
+ DZ_DIRECTION = -1;
+ } else {
DZ_DIRECTION = 1;
-
- VEL_POINT = 0;
-
+ }
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
@@ -1320,7 +1296,11 @@
torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
}
}
+
VALVE_POS_CONTROL(valve_pos.ref);
+
+ Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
+ V_out = (float) Vout.ref;
}
@@ -1458,7 +1438,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) (V_out));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen));
} 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));
}
@@ -1479,7 +1459,7 @@
} else {
t_value = V_out;
}
- CAN_TX_TORQUE((int16_t) (t_value)); //1300
+ CAN_TX_TORQUE((int16_t) (t_value), (int16_t) (V_out)); //1300
}