From 2880d130e88d3836c2461fb5d61472b50f860d20 Mon Sep 17 00:00:00 2001
From: philipp schoenberger <philipp.schoenberger@ndsatcom.com>
Date: Sat, 22 Aug 2015 14:14:33 +0200
Subject: [PATCH] fix acceleration bug for bang bang

---
 lwrserv/SvrData.cpp                  |  2 +-
 lwrserv/include/BangBangTrajectory.h | 11 ++++++-----
 lwrserv/include/Trajectroy.h         |  5 ++++-
 3 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp
index 5338703..b59d682 100644
--- a/lwrserv/SvrData.cpp
+++ b/lwrserv/SvrData.cpp
@@ -24,7 +24,7 @@ SvrData::SvrData(void)
     double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
     double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
 
-    robot.currentVelocityPercentage = 20;
+    robot.currentVelocityPercentage = 10;
     robot.currentAccelerationPercentage = 10;
 
     for (unsigned int i = 0; i < JOINT_NUMBER; ++i)
diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h
index b3fec41..68984d3 100644
--- a/lwrserv/include/BangBangTrajectory.h
+++ b/lwrserv/include/BangBangTrajectory.h
@@ -33,12 +33,14 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             // calculate number of movement steps
             // one joint has to reach maxvelocity the others are stepped down to 
             // calculate time if acceleration is enouth to reach max speed
+            // s = a * t^2 / 2
             Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
             //TODO check if mintime is neceesary
+            Vec<float,SIZE> time = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
+            std::cout << time << "time \n ";
             std::cout << minBangBangTime  << "minBangBangTime \n ";
-            
 
-            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
+            Vec<float,SIZE> minStepsPerJoint = time;//jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
             minStepsPerJoint = minStepsPerJoint.ceil();
             std::cout << minStepsPerJoint << "minStepsPerJoint \n";
 
@@ -54,15 +56,14 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             // percentage of max velocity 
             //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
             // s =  a* t^2 / 2 => a = s* 2 / t^2
-            float floaterror = 1.0f;
-            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f*floaterror;
+            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f;
             std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";
 
             float count = 0.0f;
             for( int i = 0 ; i < this->steps; ++i)
             {
                 // acceleration phase
-                if (i < this->steps /2 )
+                if (i <= this->steps /2 )
                 {
                     this->nodes[i].acceleration = currMaxAcceleration ;
                     count +=1.0f;
diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h
index b9f05e0..4ed74b1 100644
--- a/lwrserv/include/Trajectroy.h
+++ b/lwrserv/include/Trajectroy.h
@@ -5,6 +5,9 @@
 #include <iostream>
 #include <fstream>
 
+#include <iomanip>
+#include <cmath>
+#include <limits>
 template <unsigned SIZE> class Trajectory;
 
 // all types of trajectories
@@ -308,7 +311,7 @@ class Trajectory
                 {
                     if (currentJoint != 0)
                         file << ", ";
-                    file << nodes[currentStep].jointPos(currentJoint);
+                    file << std::fixed << std::setprecision(20) << nodes[currentStep].jointPos(currentJoint);
                 }
                 file << "\n";
             }