Diff: main.cpp

Revision:

44:fe7d5cfd2eea

Parent:

43:b084e5f5d0d5

Child:

45:35fa6884d0c6

--- a/main.cpp	Tue Jan 07 08:39:22 2020 +0000
+++ b/main.cpp	Tue Jan 14 09:11:20 2020 +0000
@@ -627,6 +627,7 @@
             }
 
             case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
+                
                 pos.err = pos.ref - (float) pos.sen;
                 pos.err_diff = pos.err - pos.err_old;
                 pos.err_old = pos.err;
@@ -650,6 +651,30 @@
             }
 
             case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
+                
+                float VALVE_POS_RAW_FORCE_FB = 0.0f;
+                
+                pos.err = pos.ref - pos.sen; //[pulse]
+                vel.err = vel.ref - vel.sen; //[pulse/s]
+                double K_spring = 1.0f;
+                double D_damper = 0.0001f;
+                torq.ref = (K_spring * pos.err + D_damper * vel.err) * ENC_PULSE_PER_POSITION; //[Nm]
+                
+                // torque feedback
+                torq.err = torq.ref - torq.sen; //[pulse]
+                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
+                if (torq.err_sum > 100) torq.err_sum = 100;
+                if (torq.err_sum<-100) torq.err_sum = -100;
+                VALVE_POS_RAW_FORCE_FB = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * (float) TORQUE_SENSOR_PULSE_PER_TORQUE;
+
+                valve_pos.ref = DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+
+                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);
 
                 break;
             }
@@ -1810,3 +1835,4 @@
 
 
 
+