Kim GiJeong
eeprom_test
Diff: main.cpp
- Revision:
- 66:d385a08132d7
- Parent:
- 65:443a5f0e8ee1
- Child:
- 67:cfc88fd0591e
--- a/main.cpp Tue May 19 01:36:41 2020 +0000
+++ b/main.cpp Thu May 21 02:08:49 2020 +0000
@@ -615,6 +615,7 @@
//REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
pos.ref = pos.sen;
vel.ref = 0.0f;
+ I_ERR_INT = 0.0f;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
int cnt_check_enc = (TMR_FREQ_5k/20);
@@ -636,30 +637,116 @@
if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
else pos.ref = pos.ref - 12.0f;
-
-
+ pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
double I_REF_POS = 0.0f;
- double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- double I_REF_VC = 0.0f; // I_REF for velocity compensation
-
double temp_vel_pos = 0.0f;
- double temp_vel_torq = 0.0f;
double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
+ temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s
} else if ((OPERATING_MODE && 0x01) == 1) {
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
+ temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s
}
if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
I_REF = I_REF_POS;
+ I_ERR_INT = 0.0f;
+
+
+ } else {
+ float VALVE_POS_RAW_FORCE_FB = 0.0f;
+ VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
+
+ if (VALVE_POS_RAW_FORCE_FB >= 0) {
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
+ } else {
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
+ }
+
+ VALVE_POS_CONTROL(valve_pos.ref);
+
+ V_out = (float) Vout.ref;
+
+ }
+
+
+
+// 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);
+
+ cnt_findhome_wait = 0;
+// CONTROL_MODE = MODE_JOINT_CONTROL;
+ alpha_trans = 0.0f;
+
+
+ } else {
+ ENC_SET(HOMEPOS_OFFSET);
+// ENC_SET_ZERO();
+ INIT_REF_POS = HOMEPOS_OFFSET;
+ REF_POSITION = 0;
+ REF_VELOCITY = 0;
+ FINDHOME_POSITION = 0;
+ FINDHOME_POSITION_OLD = 0;
+ FINDHOME_VELOCITY = 0;
+
+ cnt_findhome = 0;
+ cnt_vel_findhome = 0;
+ FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+
+ pos.ref = 0.0f;
+ vel.ref = 0.0f;
+ pos.ref_home_pos = 0.0f;
+ vel.ref_home_pos = 0.0f;
+
+ I_ERR_INT = 0.0f;
+
+// cnt_findhome_wait = 0;
+// FINDHOME_STAGE = FINDHOME_INIT;
+// CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+ }
+
+// } else if (FINDHOME_STAGE == FINDHOME_WAIT) {
+// cnt_findhome_wait = cnt_findhome_wait + 1;
+// if(cnt_findhome_wait > 3*TMR_FREQ_5k){
+// FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+// pos.sen = 0.0f;
+// vel.sen = 0.0f;
+// }
+
+ } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
+ int T_move = 2*TMR_FREQ_5k;
+ 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
+
+// CONTROL_MODE = MODE_JOINT_CONTROL;
+ pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
+
+ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
+
+ double I_REF_POS = 0.0f;
+
+ double temp_vel_pos = 0.0f;
+ double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
+
+ if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
+ temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s
+ } else if ((OPERATING_MODE && 0x01) == 1) {
+ temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s
+ }
+ if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
+ else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
+
+ I_REF = I_REF_POS;
+ I_ERR_INT = 0.0f;
@@ -678,108 +765,6 @@
V_out = (float) Vout.ref;
}
-
-
-
-// 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);
-
- cnt_findhome_wait = 0;
-// CONTROL_MODE = MODE_JOINT_CONTROL;
- alpha_trans = 0.0f;
-
-
- } else {
- ENC_SET(HOMEPOS_OFFSET);
-// for(int i_hp=0; i_hp<100; i_hp++){
-// ENC_SET(HOMEPOS_OFFSET);
-// }
-// ENC_SET_ZERO();
- INIT_REF_POS = HOMEPOS_OFFSET;
- REF_POSITION = 0;
- REF_VELOCITY = 0;
- FINDHOME_POSITION = 0;
- FINDHOME_POSITION_OLD = 0;
- FINDHOME_VELOCITY = 0;
-
- if(cnt_findhome_wait > 10) {
- cnt_findhome = 0;
- cnt_vel_findhome = 0;
- FINDHOME_STAGE = FINDHOME_ZEROPOSE;
- }
-
- pos.ref = 0.0f;
- vel.ref = 0.0f;
- pos.ref_home_pos = 0.0f;
- vel.ref_home_pos = 0.0f;
-
- cnt_findhome_wait = cnt_findhome_wait + 1;
- //FINDHOME_STAGE = FINDHOME_INIT;
- //CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
-
-
- }
- } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
- int T_move = 2*TMR_FREQ_5k;
- 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;
- //pos.ref = 0.0f;
- vel.ref = 0.0f;
-
- // input for position control
-
-// CONTROL_MODE = MODE_JOINT_CONTROL;
- alpha_trans = 0.0f;
-
- double torq_ref = 0.0f;
- pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
- pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-
- if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-
- double I_REF_POS = 0.0f;
- double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
- double I_REF_VC = 0.0f; // I_REF for velocity compensation
-
- double temp_vel_pos = 0.0f;
- double temp_vel_torq = 0.0f;
- double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-
- if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- } else if ((OPERATING_MODE && 0x01) == 1) {
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
- // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
- }
- if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
- else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
-
- I_REF = I_REF_POS;
-
-
-
- } else {
- float VALVE_POS_RAW_FORCE_FB = 0.0f;
- VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
-
- if (VALVE_POS_RAW_FORCE_FB >= 0) {
- valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
- } else {
- valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
- }
-
- VALVE_POS_CONTROL(valve_pos.ref);
-
- V_out = (float) Vout.ref;
-
- }
-
-
-
// pos.err = pos.ref - (float)pos.sen;
// float VALVE_POS_RAW_POS_FB = 0.0f;