Sungwoo Kim
LIGHT2
Diff: main.cpp
- Revision:
- 48:889798ff9329
- Parent:
- 47:fdcb8bd86fd6
- Child:
- 49:e7bcfc244d40
--- a/main.cpp Tue Jan 21 13:39:27 2020 +0000
+++ b/main.cpp Wed Jan 29 13:50:24 2020 +0000
@@ -73,7 +73,6 @@
int timer_while = 0;
int while_index = 0;
-
extern int CID_RX_CMD;
extern int CID_RX_REF_POSITION;
extern int CID_RX_REF_PWM;
@@ -240,7 +239,7 @@
{
int i = 0;
- float Ref_Valve_Pos_FF = DDV_CENTER;
+ float Ref_Valve_Pos_FF = 0.0f;
for(i=0; i<VALVE_POS_NUM; i++) {
if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) {
if(i==0) {
@@ -266,8 +265,9 @@
}
Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
-
+// Ref_Valve_Pos_FF = 0.0;
return Ref_Valve_Pos_FF;
+
}
@@ -323,9 +323,9 @@
for(i=0; i<24; i++) {
if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
if(i==0) {
- VALVE_PWM_RAW_FF = (float) 1000.0 / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - 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.0* (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.0* (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;
}
@@ -442,26 +442,28 @@
//unsigned long CNT_TMR3 = 0;
//float FREQ_TMR3 = (float)FREQ_5k;
float FREQ_TMR3 = (float)FREQ_1k;
-//float DT_TMR3 = (float)DT_5k;
-float DT_TMR3 = (float)DT_1k;
+float DT_TMR3 = (float)DT_5k;
+//float DT_TMR3 = (float)DT_1k;
int cnt_trans = 0;
+double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
+
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
if(MODE_POS_FT_TRANS == 1){
- alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+ alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
- if(cnt_trans>3*TMR_FREQ_5k)
+ if((float)cnt_trans * DT_TMR3 > 3.0f)
MODE_POS_FT_TRANS = 2;
}
- if(MODE_POS_FT_TRANS == 3){
- alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
+ else if(MODE_POS_FT_TRANS == 3){
+ alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
cnt_trans++;
torq.err_sum = 0;
- if(cnt_trans>3*TMR_FREQ_5k)
+ if((float) cnt_trans * DT_TMR3 > 3.0f )
MODE_POS_FT_TRANS = 0;
}
else if(MODE_POS_FT_TRANS == 2){
@@ -494,18 +496,18 @@
pos.err = pos.ref - pos.sen; //[pulse]
vel.err = vel.ref - vel.sen; //[pulse/s]
- //double K_spring = 1.0f; //[N/mm]
- //double D_damper = 0.0001f;
- torq.ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
+ double torq_ref = 0.0f;
+ torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
// torque feedback
- torq.err = torq.ref - torq.sen; //[pulse]
+ torq.err = torq_ref - torq.sen; //[pulse]
torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
if (torq.err_sum > 1000) torq.err_sum = 1000;
if (torq.err_sum<-1000) torq.err_sum = -1000;
- VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) / (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION * 0.01f + DDV_JOINT_POS_FF(vel.ref));
+ VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f
+ + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref));
if (VALVE_POS_RAW_FORCE_FB >= 0) {
valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
@@ -556,7 +558,8 @@
VALVE_POS_RAW_FORCE_FB = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
VALVE_POS_RAW_FORCE_FB = VALVE_POS_RAW_FORCE_FB * 0.01f;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+// valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref);
if (valve_pos.ref >= 0) {
valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS;
@@ -1090,7 +1093,7 @@
pos.ref_home_pos = 0.0f;
vel.ref_home_pos = 0.0f;
FINDHOME_STAGE = FINDHOME_INIT;
- CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
+ CONTROL_MODE = MODE_JOINT_CONTROL;
}
}
@@ -1323,14 +1326,12 @@
V_out = (float) P_GAIN_JOINT_POSITION * 0.01f * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
DDV_POS_AVG = VALVE_POS_TMP / data_num;
START_POS = pos.sen;
- //CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
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);
- //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
} else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
valve_pos.ref = DDV_POS_AVG;
@@ -1343,18 +1344,6 @@
valve_pos.ref = DDV_POS_AVG;
VALVE_POS_CONTROL(valve_pos.ref);
FINAL_POS = pos.sen;
- // if(VEL_POINT >= 0)
- // {
- // DZ_case = 1;
- // }
- // else
- // {
- // DZ_case = -1;
- // }
- // if(abs(FINAL_POS - START_POS) < 100)
- // {
- // DZ_case = 0;
- // }
if((FINAL_POS - START_POS)>100) {
DZ_case = 1;
@@ -1371,7 +1360,6 @@
Ref_Valve_Pos_Old = DDV_POS_AVG;
DZ_NUM = 1;
DZ_index = 1;
- //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
}
} else {
@@ -1395,12 +1383,6 @@
} 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 * DZ_case;
- // else
- // DZ_DIRECTION = -1 * DZ_case;
- // if(abs(FINAL_POS - START_POS) < 100)
- // DZ_DIRECTION = 1 * DZ_case;
if((FINAL_POS - START_POS)>100) {
DZ_DIRECTION = 1 * DZ_case;
@@ -1421,8 +1403,6 @@
DZ_index = 1;
DZ_DIRECTION = 1;
}
-
- //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index));
}
} 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)) {
@@ -1444,12 +1424,6 @@
} 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 * DZ_case;
- // else
- // DZ_DIRECTION = -1 * DZ_case;
- // if(abs(FINAL_POS - START_POS) < 100)
- // DZ_DIRECTION = -1 * DZ_case;
if((FINAL_POS - START_POS)>100) {
DZ_DIRECTION = -1;
@@ -1476,8 +1450,6 @@
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
-
- //CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
}
} else if(DZ_case == 0 && DZ_NUM ==1) {
if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
@@ -1499,12 +1471,6 @@
} 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
- // DZ_DIRECTION = -1;
- // if(abs(FINAL_POS - START_POS) < 100)
- // DZ_DIRECTION = 1;
if((FINAL_POS - START_POS)>100) {
DZ_DIRECTION = 1;
@@ -1515,7 +1481,6 @@
}
VEL_POINT = 0;
- //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION));
VALVE_DZ_timer = 0;
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
@@ -1557,7 +1522,6 @@
VALVE_DZ_timer = 0;
- //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index));
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
SECOND_DZ = valve_pos.ref;
@@ -1569,8 +1533,6 @@
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
-
- //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index));
}
}
}
@@ -1631,25 +1593,19 @@
JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
//ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
- //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
VALVE_FR_timer = 0;
one_period_end = 0;
ID_index= ID_index +1;
- // CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index));
V_out = 0.0f;
}
if(max_check == 1 && min_check == 1) {
- //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
VALVE_POS_NUM = ID_index;
//ROM_RESET_DATA();
ID_index = 0;
first_check = 0;
VALVE_FR_timer = 0;
- // CAN_TX_PRES((int16_t) (123), (int16_t) (123));
CONTROL_MODE = MODE_NO_ACT;
}
}
@@ -1741,8 +1697,13 @@
//torque
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
- CAN_TX_TORQUE((int16_t) (cur.sen));
- // CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
+ double t_value = 0;
+ if(value>=DDV_CENTER){
+ t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+ }else{
+ t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+ }
+ CAN_TX_TORQUE((int16_t) (t_value));
}
if (flag_data_request[2] == HIGH) {
@@ -1757,7 +1718,7 @@
if (flag_data_request[3] == HIGH) {
//PWM
- CAN_TX_PWM((int16_t) CUR_PWM);
+ CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
// CAN_TX_PWM((int16_t) cnt_vel_findhome);
// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
@@ -1766,7 +1727,7 @@
if (flag_data_request[4] == HIGH) {
//valve position
- CAN_TX_VALVE_POSITION((int16_t) (value), (int16_t) valve_pos.ref, (int16_t) V_out);
+ CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
//CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
//CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
@@ -1775,6 +1736,7 @@
// CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
// CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
}
+
}
TIM2->SR = 0x0; // reset the status register