![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For LIGHT
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; }