Yeseong Jeong
/
HydraulicControlBoard_Start
20210203
Diff: main.cpp
- Revision:
- 88:d6e591bece22
- Parent:
- 87:471334725012
- Child:
- 89:a7b45368ea0f
--- a/main.cpp Thu Jul 16 03:37:10 2020 +0000 +++ b/main.cpp Fri Jul 17 00:58:13 2020 +0000 @@ -374,12 +374,12 @@ } else if(NN_Control_Flag == 1) { - float output1[32] = { 0.0f }; - float output2[32] = { 0.0f }; - float output3[32] = { 0.0f }; + float output1[16] = { 0.0f }; + float output2[16] = { 0.0f }; + float output3[16] = { 0.0f }; float output = 0.0f; - for (int index2 = 0; index2 < 32; index2++) { + for (int index2 = 0; index2 < 16; index2++) { for (int index1 = 0; index1 < num_input; index1++) { output1[index2] = output1[index2] + h1[index1][index2] * input_NN[index1]; @@ -390,8 +390,8 @@ } } - for (int index2 = 0; index2 < 32; index2++) { - for (int index1 = 0; index1 < 32; index1++) { + for (int index2 = 0; index2 < 16; index2++) { + for (int index1 = 0; index1 < 16; index1++) { output2[index2] = output2[index2] + h2[index1][index2] * output1[index1]; } @@ -401,8 +401,8 @@ } } - for (int index2 = 0; index2 < 32; index2++) { - for (int index1 = 0; index1 < 32; index1++) { + for (int index2 = 0; index2 < 16; index2++) { + for (int index1 = 0; index1 < 16; index1++) { output3[index2] = output3[index2] + h3[index1][index2] * output2[index1]; } @@ -413,7 +413,7 @@ } for (int index2 = 0; index2 < 1; index2++) { - for (int index1 = 0; index1 < 32; index1++) { + for (int index1 = 0; index1 < 16; index1++) { output = output + hout[index1] * output3[index1]; } output = output + bout[index2]; @@ -427,28 +427,28 @@ valve_pos.ref = -output*0.0001f*((double)VALVE_MIN_POS - (double) VALVE_CENTER) + (double) VALVE_CENTER; } - // torque feedback - torq.err = f_past[0] - torq.sen; //[N] -// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] - torq.err_sum += torq.err/(float) 1500.0f; //[N] - - - valve_pos.ref = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f + DDV_JOINT_POS_FF(vel.sen) + valve_pos.ref * 0.01f; - - if(I_GAIN_JOINT_TORQUE != 0) { - double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; - if(valve_pos.ref>VALVE_MAX_POS) { - double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; - valve_pos_rem = valve_pos_rem * Ka; - valve_pos.ref = VALVE_MAX_POS; - torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f; - } else if(valve_pos.ref < VALVE_MIN_POS) { - double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS; - valve_pos_rem = valve_pos_rem * Ka; - valve_pos.ref = VALVE_MIN_POS; - torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f; - } - } +// // torque feedback +// torq.err = f_past[0] - torq.sen; //[N] +//// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] +// torq.err_sum += torq.err/(float) 1500.0f; //[N] +// +// +// valve_pos.ref = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f + DDV_JOINT_POS_FF(vel.sen) + valve_pos.ref * 0.01f; +// +// if(I_GAIN_JOINT_TORQUE != 0) { +// double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; +// if(valve_pos.ref>VALVE_MAX_POS) { +// double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; +// valve_pos_rem = valve_pos_rem * Ka; +// valve_pos.ref = VALVE_MAX_POS; +// torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f; +// } else if(valve_pos.ref < VALVE_MIN_POS) { +// double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS; +// valve_pos_rem = valve_pos_rem * Ka; +// valve_pos.ref = VALVE_MIN_POS; +// torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f; +// } +// } if(LED==1) { @@ -1800,7 +1800,7 @@ // if(OPERATING_MODE==5) { // t_value = (double) value; // } else if(CURRENT_CONTROL_MODE==1) { -// t_value = cur.sen; +// t_value = cur.sen * 1000.0f; // } else { // t_value = V_out; // }