distribution-201229

Dependencies:   mbed FastPWM

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;
             }