first bangbang try

lwrserv/include/BangBangTrajectroy.h

@@ -8,10 +8,45 @@ class BangBangJointTrajectroy : public Trajectory<SIZE>
 {
     public:
         BangBangJointTrajectroy(
-                unsigned int steps_,
-                float totalTime_,
-                Vec<float,SIZE> jointMovement);
+                float sampleTimeMs,
+                Vec<float,SIZE> maxJointVelocity,
+                Vec<float,SIZE> maxJointAcceleration,
+                Vec<float,SIZE> jointStart,
+                Vec<float,SIZE> jointEnd
+                )
+        {
+            float MStoSec = 1000.0f;
+            // calculate maximum velocity and acceleration
+            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
+            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
+
+            // calculate delta movement
+            Vec<float,SIZE> jointMovement = jointEnd - jointStart;
+            Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
+
+            // calculate number of movement steps
+            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
+            this->steps = ceil(minStepsPerJoint.max());
+            if (this->steps == 0)
+                this->steps +=1;
 
+            std::cout << "steps : " << this->steps << minStepsPerJoint<< "\n";
+
+            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
+
+            Vec<float,SIZE> jointLast  = jointStart;
+            for( int i = 0 ; i < this->steps; ++i)
+            {
+                jointLast = jointLast + jointMovement.celldivide((float) this->steps );
+                this->nodes[i].jointPos = jointLast;
+                this->nodes[i].velocity = maxJointLocalVelocity; 
+                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
+            }
+            // last step myth be to wide so cut it to the wanted position
+            this->nodes[this->steps-1].jointPos = jointEnd;
+            this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth
+            this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f); 
+        }
         ~BangBangJointTrajectroy();
     private:
 

lwrserv/include/LinearTrajectory.h

@@ -23,7 +23,6 @@ class LinearJointTrajectory : public Trajectory<SIZE>
             Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
             Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
 
-            std::cout << "maxv : " << maxJointLocalVelocity << "\n";
             // calculate delta movement
             Vec<float,SIZE> jointMovement = jointEnd - jointStart;
             Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
@@ -36,8 +35,6 @@ class LinearJointTrajectory : public Trajectory<SIZE>
             if (this->steps == 0)
                 this->steps +=1;
 
-            std::cout << "steps : " << this->steps << minStepsPerJoint<< "\n";
-
             this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
             Vec<float,SIZE> jointLast  = jointStart;