Diff: main.cpp

Revision:

32:4b8c0fedaf2c

Parent:

31:66738bfecec5

Child:

33:91b17819ec30

--- a/main.cpp	Tue Oct 08 07:15:05 2019 +0000
+++ b/main.cpp	Mon Nov 11 09:48:00 2019 +0000
@@ -512,7 +512,7 @@
                 pos.err_sum += pos.err;
                 if (pos.err_sum > 1000) pos.err_sum = 1000;
                 if (pos.err_sum<-1000) pos.err_sum = -1000;
-                VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
+                VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
                 VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
 
 
@@ -535,8 +535,17 @@
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
                 VALVE_POS_RAW_FORCE_FB = 0.0f;
 
-                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB  + VALVE_CENTER_OFFSET; // valve center offset = - 2.0f;
                 //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+                
+                if (valve_pos.ref > DDV_CENTER)
+                {
+                    valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
+                }
+                else if(valve_pos.ref < DDV_CENTER)
+                {
+                    valve_pos.ref = valve_pos.ref - DDV_CENTER + VALVE_DEADZONE_MINUS;
+                }
                 VALVE_POS_CONTROL(valve_pos.ref);
                 
                 
@@ -1760,3 +1769,4 @@
 
 
 
+