cleanup code

lwrserv/program.cpp

@@ -3,22 +3,13 @@
 #include "sgn.h"
 #include <error.h>
 #include <errno.h>
+#include <math.h>
+
+#include "SvrData.h"
 
 void printMat(Mat4f M)
 {
-#if 0
-    for (int i=0;i<M.rows;i++)
-    {
-        for (int j=0;j<M.cols;j++)
-        {
-            cout << M.at<float>(i,j) << " ";
-        }
-        cout << "\n\r";
-    }
-    cout << "\n\r\n\r";
-#else
     cout << M << endl;
-#endif 
 }
 
 float* abctomat(float a, float b, float c)
@@ -260,6 +251,9 @@ float* quattovec(float quat[4])
 
 void *threadFriDataExchange(void *arg)
 {
+    // unused 
+    (void) arg;
+
     //doRalf();
 
     friRemote friInst;
@@ -325,6 +319,9 @@ void *threadFriDataExchange(void *arg)
             double currentDist [LBR_MNJ] = {0};
             double currentDistLast [LBR_MNJ] = {0};
 
+            Vec<float,LBR_MNJ> maxVelocity = SvrData::getInstance()->getMaxVelocity();
+            Vec<float,LBR_MNJ> maxAccelaration = SvrData::getInstance()->getMaxAcceleration();
+
             // calculate delta positions of movement
             for ( int i= 0; i < LBR_MNJ; i++)
             {
@@ -485,6 +482,7 @@ end:
         //float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]};
         //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
 
+        // innerhlab 5ms
         friInst.doPositionControl(MSRMSRJNTPOS);
 
         //i = 0;