Diff: main.cpp

Revision:

65:443a5f0e8ee1

Parent:

64:dc5b5c258579

Child:

66:d385a08132d7

--- a/main.cpp	Mon May 18 00:36:59 2020 +0000
+++ b/main.cpp	Tue May 19 01:36:41 2020 +0000
@@ -636,18 +636,67 @@
                         if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
                         else pos.ref = pos.ref - 12.0f;
 
+
+
+
+                        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_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);
 
-                        CONTROL_MODE = MODE_JOINT_CONTROL;
+                        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;
@@ -655,16 +704,19 @@
                         FINDHOME_POSITION = 0;
                         FINDHOME_POSITION_OLD = 0;
                         FINDHOME_VELOCITY = 0;
-                        cnt_findhome = 0;
-                        cnt_vel_findhome = 0;
-                        FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+                        
+                        if(cnt_findhome_wait > 10) {
+                            cnt_findhome = 0;
+                            cnt_vel_findhome = 0;
+                            FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+                        }
 
-
-                        cnt_findhome = 0;
                         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;