Diff: main.cpp

Revision:

88:d6e591bece22

Parent:

87:471334725012

Child:

89:a7b45368ea0f

--- a/main.cpp	Thu Jul 16 03:37:10 2020 +0000
+++ b/main.cpp	Fri Jul 17 00:58:13 2020 +0000
@@ -374,12 +374,12 @@
         }
         
         else if(NN_Control_Flag == 1) {
-            float output1[32] = { 0.0f };
-            float output2[32] = { 0.0f };
-            float output3[32] = { 0.0f };
+            float output1[16] = { 0.0f };
+            float output2[16] = { 0.0f };
+            float output3[16] = { 0.0f };
             float output = 0.0f;
 
-            for (int index2 = 0; index2 < 32; index2++) {
+            for (int index2 = 0; index2 < 16; index2++) {
                 for (int index1 = 0; index1 < num_input; index1++) {
                     output1[index2] = output1[index2]
                                       + h1[index1][index2] * input_NN[index1];
@@ -390,8 +390,8 @@
                 }
             }
 
-            for (int index2 = 0; index2 < 32; index2++) {
-                for (int index1 = 0; index1 < 32; index1++) {
+            for (int index2 = 0; index2 < 16; index2++) {
+                for (int index1 = 0; index1 < 16; index1++) {
                     output2[index2] = output2[index2]
                                       + h2[index1][index2] * output1[index1];
                 }
@@ -401,8 +401,8 @@
                 }
             }
 
-            for (int index2 = 0; index2 < 32; index2++) {
-                for (int index1 = 0; index1 < 32; index1++) {
+            for (int index2 = 0; index2 < 16; index2++) {
+                for (int index1 = 0; index1 < 16; index1++) {
                     output3[index2] = output3[index2]
                                       + h3[index1][index2] * output2[index1];
                 }
@@ -413,7 +413,7 @@
             }
 
             for (int index2 = 0; index2 < 1; index2++) {
-                for (int index1 = 0; index1 < 32; index1++) {
+                for (int index1 = 0; index1 < 16; index1++) {
                     output = output + hout[index1] * output3[index1];
                 }
                 output = output + bout[index2];
@@ -427,28 +427,28 @@
                 valve_pos.ref = -output*0.0001f*((double)VALVE_MIN_POS - (double) VALVE_CENTER) + (double) VALVE_CENTER;
             }
 
-            // torque feedback
-            torq.err = f_past[0] - torq.sen; //[N]
-//            torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-            torq.err_sum += torq.err/(float) 1500.0f; //[N]
-    
-
-            valve_pos.ref = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f + DDV_JOINT_POS_FF(vel.sen) + valve_pos.ref * 0.01f;
-
-            if(I_GAIN_JOINT_TORQUE != 0) {
-                double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
-                if(valve_pos.ref>VALVE_MAX_POS) {
-                    double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
-                    valve_pos_rem = valve_pos_rem * Ka;
-                    valve_pos.ref = VALVE_MAX_POS;
-                    torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
-                } else if(valve_pos.ref < VALVE_MIN_POS) {
-                    double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
-                    valve_pos_rem = valve_pos_rem * Ka;
-                    valve_pos.ref = VALVE_MIN_POS;
-                    torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
-                }
-            }
+//            // torque feedback
+//            torq.err = f_past[0] - torq.sen; //[N]
+////            torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
+//            torq.err_sum += torq.err/(float) 1500.0f; //[N]
+//    
+//
+//            valve_pos.ref = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f + DDV_JOINT_POS_FF(vel.sen) + valve_pos.ref * 0.01f;
+//
+//            if(I_GAIN_JOINT_TORQUE != 0) {
+//                double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
+//                if(valve_pos.ref>VALVE_MAX_POS) {
+//                    double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
+//                    valve_pos_rem = valve_pos_rem * Ka;
+//                    valve_pos.ref = VALVE_MAX_POS;
+//                    torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
+//                } else if(valve_pos.ref < VALVE_MIN_POS) {
+//                    double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
+//                    valve_pos_rem = valve_pos_rem * Ka;
+//                    valve_pos.ref = VALVE_MIN_POS;
+//                    torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
+//                }
+//            }
             
             
             if(LED==1) {
@@ -1800,7 +1800,7 @@
 //                if(OPERATING_MODE==5) {
 //                    t_value = (double) value;
 //                } else if(CURRENT_CONTROL_MODE==1) {
-//                    t_value = cur.sen;
+//                    t_value = cur.sen * 1000.0f;
 //                } else {
 //                    t_value = V_out;
 //                }