Diff: main.cpp

Revision:

64:368bcd2a230a

Parent:

63:376a20d2fbfd

Child:

65:549e0426cd91

--- a/main.cpp	Thu May 07 03:52:28 2020 +0000
+++ b/main.cpp	Tue May 12 06:24:31 2020 +0000
@@ -654,7 +654,7 @@
                         FINDHOME_VELOCITY = 0;
                         cnt_findhome = 0;
                         cnt_vel_findhome = 0;
-//                        FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+                        FINDHOME_STAGE = FINDHOME_ZEROPOSE;
 
 
                         cnt_findhome = 0;
@@ -662,8 +662,8 @@
                         vel.ref = 0.0f;
                         pos.ref_home_pos = 0.0f;
                         vel.ref_home_pos = 0.0f;
-                        FINDHOME_STAGE = FINDHOME_INIT;
-                        CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+                        //FINDHOME_STAGE = FINDHOME_INIT;
+                        //CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
                         
                         
                     }
@@ -677,10 +677,6 @@
                 
 //                    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]
@@ -708,12 +704,11 @@
                         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) {
@@ -1560,18 +1555,18 @@
             if (flag_data_request[1] == HIGH) {
                 //valve position
                 double t_value = 0;
-//                if(valve_pos.ref>=(float) VALVE_CENTER) {
-//                    t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                if(valve_pos.ref>=(float) VALVE_CENTER) {
+                    t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                } else {
+                    t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                }
+//                if(OPERATING_MODE==5) {
+//                    t_value = (double) value;
+//                } else if(CURRENT_CONTROL_MODE==1) {
+//                    t_value = cur.sen;
 //                } else {
-//                    t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+//                    t_value = V_out;
 //                }
-                if(OPERATING_MODE==5) {
-                    t_value = (double) value;
-                } else if(CURRENT_CONTROL_MODE==1) {
-                    t_value = cur.sen;
-                } else {
-                    t_value = V_out;
-                }
                 CAN_TX_TORQUE((int16_t) (t_value)); //1300
                 //CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
                 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300