use robot velo instead of max velo

lwrserv/SvrData.cpp

@@ -257,6 +257,8 @@ VecJoint SvrData::getRobotVelocity()
 
     pthread_mutex_lock(&dataLock);
     percent = robot.currentVelocityPercentage / 100.0f;
+    std::cout << "percent " << percent << " velo " << retval;
+    std::cout << robot.max.velocity << "max velo \n";
     retval = robot.max.velocity * percent;
     pthread_mutex_unlock(&dataLock);
     return retval;

lwrserv/SvrHandling.cpp

@@ -709,8 +709,8 @@ void moveHomRowWiseStatus(SocketObject& client, string& arg)
     // calculate trajectory
     VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
     float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
-    VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
-    VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();
+    VecJoint maxJointVelocity     =  SvrData::getInstance()->getRobotVelocity();
+    VecJoint maxJointAcceleration =  SvrData::getInstance()->getRobotAcceleration();
 
     class Trajectory<JOINT_NUMBER>* newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
 

lwrserv/include/LinearTrajectory.h

@@ -22,6 +22,7 @@ class LinearJointTrajectory : public Trajectory<SIZE>
             // calculate maximum velocity and acceleration
             Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
             Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
+            std::cout << "max : " << maxJointLocalVelocity << "\n";
 
             // calculate delta movement
             Vec<float,SIZE> jointMovement = jointEnd - jointStart;
@@ -31,7 +32,10 @@ class LinearJointTrajectory : public Trajectory<SIZE>
 
             // calculate number of movement steps
             Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
+            std::cout << "minsteps : " << minStepsPerJoint << "\n";
+            std::cout << "steps : " << minStepsPerJoint.max()  << "\n";
             this->steps = ceil(minStepsPerJoint.max());
+            std::cout << "steps : " << this->steps << "\n";
             if (this->steps == 0)
                 this->steps +=1;
 

lwrserv/include/vec.h

@@ -165,10 +165,10 @@ public:
 
     Vec<T, SIZE> operator * (T tScale) 
     {
-        T atBuffer[SIZE];
+        Vec<T, SIZE> vec;
         for (unsigned int i=0; i<SIZE; i++)
-            atBuffer[i] = m_atData[i]*tScale;
-        return Vec<T, SIZE> (atBuffer);
+            vec(i) = m_atData[i]*tScale;
+        return vec;
     }
 
     Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)

lwrserv/program.cpp

@@ -215,7 +215,6 @@ void *threadRobotMovement(void *arg)
             {
                 // mark that we reached the end of the trajectory
                 // stay on current position
-                cout << "end of trajectorys \n";
                 goto end;
             }
             else