Diff: main.cpp

Revision:

1:c4fa1bfe7672

Parent:

0:f7e7848048d5

--- a/main.cpp	Mon Nov 28 22:18:47 2022 +0000
+++ b/main.cpp	Tue Nov 29 21:25:30 2022 +0000
@@ -162,7 +162,7 @@
         
         // If there are new inputs, this code will run
         if (server.getParams(input_params,NUM_INPUTS)) {
-            
+            jumped = false;
                         
             // Get inputs from MATLAB          
             start_period                = input_params[0];    // First buffer time, before trajectory
@@ -342,11 +342,12 @@
 //                current_des2 = (-K_yy*(th2 - th2_des) - D_yy*(dth2 - dth2_des))/k_t; 
                   
                 
-                if (t.read() >= tipre && t.read() <= tih) {
-                    
-                    current_des2 = -1*(-K_yy*(th2 - thpre) - D_yy*(dth2))/k_t; // negative sign so goes clockwise
-                    
-                } else if (t.read() >= tih  && jumped == false) { //&& th2 > th2_limu
+                //if (t.read() >= tipre && t.read() <= tih) {
+//                    
+//                    current_des2 = -1*(-K_yy*(th2 - thpre) - D_yy*(dth2))/k_t; // negative sign so goes clockwise
+//                    
+//                } 
+                if (t.read() >= tih  && jumped == false) { //&& th2 > th2_limu
                     current_des2 = -max_torque/k_t;
 //                    pc.printf("im here");
 //                    pc.printf("currentdes2: %f \n\r", current_des2);
@@ -360,9 +361,9 @@
                     current_des2 = 0.0;
                 }
 
-                if (t.read() >= tihend)  {
-                    current_des2 = 0.0;
-                }            
+                //if (t.read() >= tihend)  {
+//                    current_des2 = 0.0;
+//                }            
                            
                 // Cartesian impedance  
                 // Note: As with the joint space laws, be careful with signs!