Sungwoo Kim
distribution-201229
Diff: main.cpp
- Revision:
- 29:69f3f5445d6d
- Parent:
- 28:2a62d73e3dd0
- Child:
- 30:8d561f16383b
--- a/main.cpp Tue Oct 01 11:52:10 2019 +0000
+++ b/main.cpp Wed Oct 02 04:41:59 2019 +0000
@@ -922,108 +922,102 @@
}
case MODE_FIND_HOME: {
-
-
- ////////////////////////////////////////////FIND_HOME_PWM/////////////////////////////////////////////////////
- if (FLAG_FIND_HOME == true) {
+ if (FINDHOME_STAGE == FINDHOME_INIT) {
cnt_findhome = 0;
cnt_vel_findhome = 0;
//REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
- pos.ref = pos.sen;
- FLAG_FIND_HOME = false;
- }
-
- int check_enc = (TMR_FREQ_5k/10);
- if(cnt_findhome%check_enc == 0) {
- FINDHOME_POSITION = pos.sen;
- FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
- FINDHOME_POSITION_OLD = FINDHOME_POSITION;
- }
- cnt_findhome++;
- if(cnt_findhome == 10000) cnt_findhome = 0;
-
- if (abs(FINDHOME_VELOCITY) <= 1) {
- cnt_vel_findhome = cnt_vel_findhome + 1;
- } else {
- cnt_vel_findhome = 0;
- }
+ pos.ref_home_pos = pos.sen;
+ vel.ref_home_pos = 0.0;
+ FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
+ } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
+ int cnt_check_enc = (TMR_FREQ_5k/500);
+ if(cnt_findhome%cnt_check_enc == 0) {
+ FINDHOME_POSITION = pos.sen;
+ FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
+ FINDHOME_POSITION_OLD = FINDHOME_POSITION;
+ }
+ cnt_findhome++;
+ if(cnt_findhome == TMR_FREQ_5k) cnt_findhome = 0;
- if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
- //REFERENCE_MODE = MODE_REF_NO_ACT;
- if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 1.0;
- else pos.ref = pos.ref - 1.0;
- pos.err = pos.ref - (double) pos.sen;
- V_out = ((double) P_GAIN_JOINT_POSITION * pos.err) * 0.01;
- // if (HOMEPOS_OFFSET > 0) V_out = 100;
- // else V_out = -100;
- } else {
- ENC_SET(HOMEPOS_OFFSET);
- FLAG_REFERENCE_JOINT_POSITION = 1;
- INIT_REF_POS = HOMEPOS_OFFSET;
- REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
- TMR3_COUNT_REFERENCE = 0;
- vel.ref = 0.0;
- //REFERENCE_MODE = MODE_REF_COS_INC;
- CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
-
- FINDHOME_POSITION = 0;
- FINDHOME_POSITION_OLD = 0;
- FINDHOME_VELOCITY = 0;
- cnt_findhome = 0;
- cnt_vel_findhome = 0;
- }
+ if (abs(FINDHOME_VELOCITY) <= 1) {
+ cnt_vel_findhome = cnt_vel_findhome + 1;
+ } else {
+ cnt_vel_findhome = 0;
+ }
- /*
- ////////////////////////////////////////////FIND_HOME_CURRENT/////////////////////////////////////////////////////
- if (FLAG_FIND_HOME == true) {
- cnt_findhome = 0;
- cnt_vel_findhome = 0;
- REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
- Ref_Joint_Pos = pos.sen;
- FLAG_FIND_HOME = false;
- }
-
- int cnt_check_enc = (TMR_FREQ_5k/500);
- if(cnt_findhome%cnt_check_enc == 0){
- FINDHOME_POSITION = pos.sen;
- FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
- FINDHOME_POSITION_OLD = FINDHOME_POSITION;
- } cnt_findhome++;
- if(cnt_findhome == 10000) cnt_findhome = 0;
-
- if (abs(FINDHOME_VELOCITY) <= 1) {
- cnt_vel_findhome = cnt_vel_findhome + 1;
- } else {
- cnt_vel_findhome = 0;
- }
+ if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec
+ //REFERENCE_MODE = MODE_REF_NO_ACT;
+ if (HOMEPOS_OFFSET > 0) pos.ref_home_pos = pos.ref_home_pos + 1.0;
+ else pos.ref_home_pos = pos.ref_home_pos - 1.0;
+ pos.err = pos.ref_home_pos - pos.sen;
+ double VALVE_POS_RAW_POS_FB = 0.0;
+ VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * pos.err;
+ VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+ VALVE_POS_CONTROL(valve_pos.ref);
+
+ //double wn_Pos = 2.0*PI*5.0; // f_cut : 10Hz Position Control
+ //I_REF = 0.04*wn_Pos*((double)joint_pos_err/ENC_PULSE_PER_POSITION);
+ //// L velocity >> mA convert
+ //if(I_REF>5.0) I_REF = 5.0;
+ //if(I_REF<-5.0) I_REF = -5.0;
+ //FLAG_CURRNET_CONTROL = true;
+ } else {
+ ENC_SET(HOMEPOS_OFFSET);
+ 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;
+ }
+ } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
+ int T_move = 2*TMR_FREQ_5k;
+ pos.ref_home_pos = (0.0 - (double)INIT_REF_POS)*0.5*(1.0 - cos(3.14159 * (double)cnt_findhome / (double)T_move)) + (double)INIT_REF_POS;
+ vel.ref_home_pos = 0.0;
- if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec
- REFERENCE_MODE = MODE_REF_NO_ACT;
- if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
- else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
- pos.err = Ref_Joint_Pos - pos.sen;
- I_REF = 0.001*((double)pos.err);
- if(I_REF>5.0) I_REF = 5.0;
- if(I_REF<-5.0) I_REF = -5.0;
- cur.ref = I_REF;
- CurrentControl();
- } else {
- ENC_SET(HOMEPOS_OFFSET);
- FLAG_REFERENCE_JOINT_POSITION = 1;
- INIT_REF_POS = HOMEPOS_OFFSET;
- REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
- TMR3_COUNT_REFERENCE = 0;
- Ref_Joint_Vel = 0.0;
- REFERENCE_MODE = MODE_REF_COS_INC;
- CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
+ // input for position control
+ pos.err = (pos.ref_home_pos - (double)pos.sen) / ENC_PULSE_PER_POSITION;
+ double VALVE_POS_RAW_POS_FB = 0.0;
+ VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * pos.err;
+ VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+ VALVE_POS_CONTROL(valve_pos.ref);
+
- FINDHOME_POSITION = 0;
- FINDHOME_POSITION_OLD = 0;
- FINDHOME_VELOCITY = 0;
- cnt_findhome = 0;
- cnt_vel_findhome = 0;
+// if((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
+// double wn_Pos = 2.0*PI*5.0; // f_cut : 10Hz Position Control
+// double temp_vel = ( 0.01 * (double)P_GAIN_JOINT_POSITION * wn_Pos * pos.err)*PI/180.0; // rad/s
+// // L when P-gain = 100, f_cut = 10Hz
+// if (temp_vel > 0.0 ) I_REF = temp_vel*((double)PISTON_AREA_A*0.00006/(K_v*sqrt(2.0*alpha3/(alpha3+1.0))));
+// else I_REF = temp_vel*((double)PISTON_AREA_B*0.00006/(K_v*sqrt(2.0/(alpha3+1.0))));
+// // ------------------------------------------------------------------------
+// // L thetadot(rad/s) >> I_ref(mA)
+// } else if ((OPERATING_MODE && 0x01) == 1) { // Linear Mode
+// double wn_Pos = 2.0*PI*5.0; // f_cut : 10Hz Position Control
+// double temp_vel = ( 0.01 * (double)P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
+// // L when P-gain = 100, f_cut = 10Hz
+// if (temp_vel > 0.0 ) I_REF = temp_vel*((double)PISTON_AREA_A*0.00006/(K_v*sqrt(2.0*alpha3/(alpha3+1.0))));
+// else I_REF = temp_vel*((double)PISTON_AREA_B*0.00006/(K_v*sqrt(2.0/(alpha3+1.0))));
+// // ------------------------------------------------------------------------
+// // L xdot(mm/s) >> I_ref(mA)
+// }
+
+ cnt_findhome++;
+ if (cnt_findhome >= T_move) {
+ //REFERENCE_MODE = MODE_REF_DIRECT;
+ cnt_findhome = 0;
+ pos.ref = 0.0;
+ vel.ref = 0.0;
+ pos.ref_home_pos = 0.0;
+ vel.ref_home_pos = 0.0;
+ FINDHOME_STAGE = FINDHOME_INIT;
+ CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
+ }
}
- */
break;
}