From 872a7f07b3c03221a5654b2e7b66a1adc37f3f54 Mon Sep 17 00:00:00 2001
From: philipp schoenberger <philipp.schoenberger@ndsatcom.com>
Date: Tue, 30 Jun 2015 15:48:26 +0200
Subject: [PATCH] cleanup code

---
 lwrserv/program.cpp | 24 +++++++++++-------------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp
index f5d128a..0387e0c 100644
--- a/lwrserv/program.cpp
+++ b/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;
@@ -269,7 +263,7 @@ void *threadFriDataExchange(void *arg)
         //#######################################
         // Communication loop
 
-        // get current joint positions 
+        // get current joint positions
         for ( int i= 0; i < LBR_MNJ; i++)
         {
             MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i];
@@ -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;