Diff: main.cpp

Revision:

197:50e95ba0ade2

Parent:

196:56a7e26a4a8a

Child:

198:bc4dfecb8e32

--- a/main.cpp	Sat Dec 19 05:14:54 2020 +0000
+++ b/main.cpp	Sat Dec 19 08:57:06 2020 +0000
@@ -1,4 +1,4 @@
-//201219_1
+//201219_with_delay
 #include "mbed.h"
 #include "FastPWM.h"
 #include "INIT_HW.h"
@@ -265,6 +265,9 @@
 
 const float bout[1] = { -0.10351773351430893f };
 
+
+float valve_ref_pos_buffer[10] = {0.0f};
+
 /////////////////////////////////////////////////////////////////////////////////////////////RL
 float input_RL[num_input_RL] = { 0.0f };
 
@@ -2465,8 +2468,18 @@
                             torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
                         }
                     }
-
-                    VALVE_POS_CONTROL(valve_pos.ref);
+                    
+                    ///////////For Test LIMC///////////////////////////////////////////////////////////////////////////////////////////////////
+                    for(int i=0; i<9; i++) {
+                        valve_ref_pos_buffer[i] = valve_ref_pos_buffer[i+1];
+                    }
+                    valve_ref_pos_buffer[9] = valve_pos.ref;
+                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+                    
+                    ///////////For Test LIMC///////////////////////////////////////////////////////////////////////////////////////////////////
+                    //VALVE_POS_CONTROL(valve_pos.ref);
+                    VALVE_POS_CONTROL(valve_ref_pos_buffer[0]);
+                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 //                    Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
                     V_out = (float) Vout.ref;