diff --git a/paramfile/Makefile b/paramfile/Makefile
new file mode 100644
index 0000000..6ee721c
--- /dev/null
+++ b/paramfile/Makefile
@@ -0,0 +1,12 @@
+CC=g++
+CFLAGS=-I. -std=c++11 -g
+FILES = Test.cpp
+OUT_EXE = Test
+ 
+build: $(FILES)
+	$(CC) -o $(OUT_EXE) $(FILES) $(CFLAGS)
+ 
+clean:
+	rm -f *.o core
+ 
+rebuild: clean build
diff --git a/paramfile/README b/paramfile/README
new file mode 100644
index 0000000..790bc56
--- /dev/null
+++ b/paramfile/README
@@ -0,0 +1,37 @@
+24.5.2019 Wayne Glettig
+---
+
+When working on NUC, install DDD. DDD is great to debug simple c files.
+
+
+To do:
+
+Implement C++ Class to use for motion control for SmarGon.
+
+What is missing:
+
+COMPONENTS:
+
+NewGon STATE CLASS, like an Object (as in Struct with methods) containing:
+    * Configuration PARAMETERS
+    * Current Operational STATUS
+of NewGon.
+
+
+NewGon TRAJECTORY Models: implement Trapezoidal & S-curve motion, or following mode. 
+That can be applied either to a physical axis or to a virtual axis.
+
+
+
+
+QUESTIONS:
+  * How to implement Geometrical Models, IK etc.?
+  * 
+  
+
+
+
+
+
+
+
diff --git a/paramfile/Test.cpp b/paramfile/Test.cpp
new file mode 100644
index 0000000..abea4fc
--- /dev/null
+++ b/paramfile/Test.cpp
@@ -0,0 +1,82 @@
+#include <iostream>
+#include "NewGon.h"
+
+int parse(std::string str, NewGon myGon) {
+    if (str[0]=='Q' || str[0]=='q') // Q: End program
+       return 0;
+
+    if (str[0] == 'A' || str[0]== 'a') 
+       if (str[1] == '1')
+          std::cout << "axis[1].name: " << myGon.axis[1].name << "\n"; 
+       if (str[1] == '2')
+          std::cout << "axis[2].name: " << myGon.axis[2].name << "\n";
+       if (str[1] == '3')
+          std::cout << "axis[3].name: " << myGon.axis[3].name << "\n";
+       if (str[1] == '4')
+          std::cout << "axis[4].name: " << myGon.axis[4].name << "\n";
+       if (str[1] == '5')
+          std::cout << "axis[5].name: " << myGon.axis[5].name << "\n";
+       if (str[1] == '6')
+          std::cout << "axis[6].name: " << myGon.axis[6].name << "\n";
+    if (str[0]== 'S'|| str[0]=='s')
+    {
+        myGon.runstep();
+        std::cout << "axis[1].x: " << myGon.axis[1].pos << "\n"; 
+    }
+
+    return 1;
+}
+
+int main ()
+{
+    int ret;
+    //Define Goniometer
+    NewGon myGon; //create Goniometer object
+
+    //Configure Goniometer
+    myGon.setName("Wayne's NewGon");
+    myGon.axis[1].setName("s1");
+    myGon.axis[2].setName("s2");
+    myGon.axis[3].setName("s3");
+    myGon.axis[4].setName("s4");
+    myGon.axis[5].setName("s5");
+    myGon.axis[6].setName("s6");
+    myGon.axis[1].setUnitUsr("mm");
+    myGon.axis[1].setUnitCtr("nm");
+   
+    
+    //User Feedback
+    std::cout << "=============================" << "\n";
+    std::cout << "PROGRAM TO DEBUG NewGon class" << "\n";
+    std::cout << "-----------------------------" << "\n";
+    std::cout << "myGon.name : '" << myGon.name << "'\n";
+    std::cout << "myGon.MODE : " << myGon.MODE  << "\n";
+    std::cout << "myGon.naxes: " << myGon.naxes << "\n";
+/*    for (int i=1; i<=myGon.naxes; i++) {
+       std::cout << "---------------------------------" << "\n";
+        std::cout << "myGon.axis["<<i<<"].name    : '"  << myGon.axis[i].name << "'\n";
+        std::cout << "myGon.axis["<<i<<"].unit_usr: '"  << myGon.axis[i].unit_usr << "'\n";
+        std::cout << "myGon.axis["<<i<<"].unit_ctr: '"  << myGon.axis[i].unit_ctr << "'\n";
+        std::cout << "myGon.axis["<<i<<"].scale   : "   << myGon.axis[i].scale << "\n";
+        std::cout << "myGon.axis["<<i<<"].offset  : "   << myGon.axis[i].offset << "\n";
+        std::cout << "myGon.axis["<<i<<"].LimNeg  : "   << myGon.axis[i].LimNeg << "\n";
+        std::cout << "myGon.axis["<<i<<"].LimPos  : "   << myGon.axis[i].LimPos << "\n";
+        std::cout << "myGon.axis["<<i<<"].maxSpeed: "   << myGon.axis[i].maxSpeed << "\n";
+        std::cout << "myGon.axis["<<i<<"].minSpeed: "   << myGon.axis[i].minSpeed << "\n";
+        std::cout << "myGon.axis["<<i<<"].maxJogSpeed: "<< myGon.axis[i].maxJogSpeed << "\n";
+        std::cout << "myGon.axis["<<i<<"].minJogSpeed: "<< myGon.axis[i].minJogSpeed << "\n";
+        std::cout << "myGon.axis["<<i<<"].moveSpeed: "  << myGon.axis[i].moveSpeed << "\n";
+        std::cout << "myGon.axis["<<i<<"].jogSpeed: "   << myGon.axis[i].jogSpeed << "\n";  
+    }
+*/
+    std::string strCommand;
+    int loop = 1;
+    while (loop) {
+        std::cout << "> ";
+        std::cin >> strCommand;
+        loop = parse(strCommand, myGon);
+    }
+    return 0;
+}
+
+
diff --git a/paramfile/readparamfile.h b/paramfile/readparamfile.h
new file mode 100644
index 0000000..38446ac
--- /dev/null
+++ b/paramfile/readparamfile.h
@@ -0,0 +1,207 @@
+#include <math.h>
+
+class Trajectory
+{
+    public:
+    double t = 0;  //current time
+    double x = 0;  //current position
+    double v = 0;  //current velocity
+    double a = 0;  //current acceleration
+
+    double xT = 0; //target position
+    double xdb=0.0001; //position loop deadband.
+ 
+    //parameters
+    double vmax = 0;  //maximum velocity
+    double amax = 0;  //maximum acceleration
+  
+    double dt = 0.001; //time step
+
+    int set_target(double target);
+    int runstep ();
+    private:
+
+};
+
+
+int Trajectory::set_target(double target)
+{
+    xT = target;
+    return 0;
+}
+
+int Trajectory::runstep()
+{
+    t += dt; //increment by time step
+    double xrem = xT-x; //remaining path
+    if (fabs(xrem) < xdb) return 1; //if in deadband, don't do anythhing
+
+    double xsgn = xrem<0?-1:1; //sgn(xrem)
+    double bremsweg = v*v/2/amax; //bremsweg (based on current speed)
+    double asgn = fabs(xrem)<fabs(bremsweg)?-1:1; //if bremsweg too short deccelerate
+    a = xsgn*asgn*amax;  //calculate new a
+    v = v + a*dt;        //calculate new v
+    if (v> vmax) {v =  vmax; a=0;}  //bound v
+    if (v<-vmax) {v = -vmax; a=0;}
+    x = x + v*dt + 0.5*a*dt*dt; //calculate new x
+    return 1;
+}  
+
+
+///////////////////////////////////////////////////////////////////////////////
+class AxisDef
+{
+    public:
+    static const int NAMELENGTH = 8;
+    //Axis Parameters
+    char name[NAMELENGTH+1];       //axis name
+    char unit_usr[NAMELENGTH+1];   //user unit name (string)
+    char unit_ctr[NAMELENGTH+1];   //control unit name (string)
+    double scale = 1.;    //
+    double offset = 0.;   //UserUnits = (ControlUnits * scale) + offset
+    //All limit in User Units:
+    double LimNeg = 0.;
+    double LimPos = 0.;
+    double maxSpeed = 0.;
+    double minSpeed = 0.;
+    double maxJogSpeed = 0.;
+    double minJogSpeed = 0.;
+    //Nominal speeds:
+    double moveSpeed = 0.;
+    double jogSpeed = 0.;
+
+
+    Trajectory traj;
+    //AXIS STATUS:
+    double timestamp = 0.;
+    int state_init = 0;
+    int state_motion = 0;
+    double pos = 0.;
+    double v = 0.;
+    double a = 0.;
+ 
+    double targetpos = 0.;
+
+    //Default Constructor:
+    AxisDef();
+
+    //Methods:
+    int setName(const char * newname);
+    int setUnitUsr(const char * newname);
+    int setUnitCtr(const char * newname);
+    int setStr(const char * setstring, char * targetstring);
+};
+
+AxisDef::AxisDef() 
+{
+    //initialize strings to "" (to avoid crap upon creation)
+    setName("");
+    setUnitUsr("");
+    setUnitCtr(""); 
+}
+
+int AxisDef::setName(const char * newname)
+{
+    return setStr(newname, name);
+}
+
+int AxisDef::setUnitUsr(const char * newname)
+{
+    return setStr(newname, unit_usr);
+}
+
+int AxisDef::setUnitCtr(const char * newname)
+{
+    return setStr(newname, unit_ctr);
+}
+
+int AxisDef::setStr(const char * setstring, char * targetstring)
+// use: newgon.setName("Name")
+// only the first NAMELENTH charaters will be used.
+// if the given string is longer than NAMELENGTH, return 1
+// if all goes well return 0
+{
+    for (int i=0; i<NAMELENGTH+1; i++) 
+    {
+        if (setstring[i] == 0) //if end of string of newname
+        {
+            targetstring[i]=setstring[i];
+            break;
+        }
+        else { //normal characters in newname
+            targetstring[i]=setstring[i];
+        }
+	if (i==NAMELENGTH) //if the end of name is reached, terminate with 0
+        {
+            targetstring[i]=0;
+            return 1; // return 1 means: string too long.
+        }          
+    }
+    return 0;
+}
+
+
+
+
+
+////////////////////////////////////////////////////////////////
+class NewGon
+{
+    public:
+    static const int naxes = 6;       //define here the number of axes of NewGon
+    static const int namelength = 20;  //define here the max string length of names
+    
+    //Create all Axis objects
+    AxisDef axis[naxes+1];
+
+    //General Parameters
+    char name[namelength+1];
+
+    //GENERAL STATUS
+    int MODE = 0.;
+    double timestamp = 0.;
+
+    //Constructors:
+    NewGon() { }
+
+    //Methods:
+    int setName(const char * newname);
+    int runstep();
+};
+
+//NewGon::NewGon()
+//{
+
+//}
+
+int NewGon::setName(const char * newname)
+// use: newgon.setName("Name")
+// only the first NAMELENTH charaters will be used.
+// if the given string is longer than NAMELENGTH, return 1
+// if all goes well return 0
+{
+    for (int i=0; i<namelength+1; i++) 
+    {
+        if (newname[i] == 0) //if end of string of newname
+        {
+            name[i]=newname[i];
+            break;
+        }
+        else { //normal characters in newname
+            name[i]=newname[i];
+        }
+	if (i==namelength) //if the end of name is reached, terminate with 0
+        {
+            name[i]=0;
+            return 1; // return 1 means: string too long.
+        }          
+    }
+    return 0;
+}
+
+int NewGon::runstep()
+{
+    return 0;
+}
+
+
diff --git a/traj/Motion Calculation.pdf b/traj/Motion Calculation.pdf
deleted file mode 100644
index 2590e8d..0000000
Binary files a/traj/Motion Calculation.pdf and /dev/null differ
diff --git a/traj/Test.m b/traj/Test.m
deleted file mode 100644
index ac213d1..0000000
--- a/traj/Test.m
+++ /dev/null
@@ -1,16 +0,0 @@
-% [ T X V A ] = fastest_track_function(x0,v0,xT,vT,vmax,amax,dmax,t0,dt)
-
-
-% List of Points at time (one point = one line in matrix)
-% Each Line: t, x, y, z
-P = [0, 0,0,0;...
-     1, 1,1,1;...
-     2, 0,1,1;...
-     3, 2,3,1;...
-     4, 4,4,3];
- 
-% For each dimension interpolate the fastest track function
-X = pvt_varVmax(P(:,1), P(:,2));
-plot (X,Y,Z);
-
- 
\ No newline at end of file
diff --git a/traj/fastest_track_algorithm.m b/traj/fastest_track_algorithm.m
deleted file mode 100644
index bfc508e..0000000
--- a/traj/fastest_track_algorithm.m
+++ /dev/null
@@ -1,124 +0,0 @@
-% Implementation of a fastest track motion algorithm.
-% 23.8.2018, Wayne Glettig
-% Fastest track from x0 to xT, with the following given constraints:
-%   vmax: maximum speed of travel 
-%   amax: maximum acceleration
-%   dmax: maximum deceleration
-% The algorithm also takes in account start and end velocities v0 & vT
-% And plots the result in (x,t), (v,t) and (a,t) plots.
-
-
-%% INPUT PARAMETERS:
-x0=0;  %Start position
-v0=0;  %Start velocity
-
-xT=1e-6; %Target position
-vT=0;  %Target velocity
-
-% Limits: Maximum allowed velocity and acceleration
-vmax=0.5;       %Maxium allowed velocity
-amax=16.21;     %Maximum acceleration
-dmax=-amax;   %Maximum deceleration
-
-t0=0;     %Start time 
-dt=0.0000001;  %time step
-
-%% CALCULATE PROFILE:
-% First, we set the motion direction, depending on start and end positions. 
-% Here we also make the algorithm immune to vmax, amax & dmax signs.
-dir=sign(xT-x0);
-amax = dir*abs(amax);
-dmax = -dir*abs(dmax);
-vmax = dir*abs(vmax);
-
-% The motion profile is comprised of three phases:
-% 1) Acceleration phase with full amax acceleration
-% 2) Constant velocity phase with vmax
-% 3) Deceleration phase with full dmax deceleration
-
-% Now if the distance is too short to be able to accelerate up to vmax, 
-% there is no constant velocity phase (phase 2).
-% Let's calculate the displacement required to accelerate (v0->vmax) and 
-% deccelerate (vmax->vT):
-s_acc  = v0*(vmax-v0)/amax + (vmax-v0)^2/2/amax;
-s_decc = vmax*(vT-vmax)/dmax + (vT-vmax)^2/2/dmax;
-
-% Let's see if s_acc and s_decc fit within the required displacement xT-x0.
-if (abs(xT-x0)<abs(s_acc+s_decc))
-    %So, if it doesnt fit, an intermediate vlim is targeted, which is a 
-    %local vmax to accelerate to, until it's time to decelerate.
-    vlim = dir*sqrt((v0^2/2/amax - vT^2/2/dmax + xT - x0)/(1/2/amax - 1/2/dmax));
-    %Also, the constant velocity phase is over a distance of zero.
-    s_vconst = 0;
-    % Recalculate the effective displacements using vlim instead of vmax
-    s_acc  = v0*(vlim-v0)/amax + (vlim-v0)^2/2/amax
-    s_decc = vlim*(vT-vlim)/dmax + (vT-vlim)^2/2/dmax;
-else
-    vlim =vmax;
-    %The constant velocity distance is the remaining distance.
-    s_vconst = (xT-x0)-(s_acc+s_decc);
-end
-
-
-% So now the distances have been established, let's look at the times.
-% t0 --acceleration--> t1 --vconst--> t2 --deceleration--> t3
-%         t_acc            t_vconst           t_decc
-%
-t_acc = (vlim-v0)/amax;
-t_decc = (vT-vlim)/dmax;
-t_vconst = s_vconst/vlim;
-
-t1 = t0 + t_acc;
-t2 = t1 + t_vconst;
-t3 = t2 + t_decc;
-
-
-
-%% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-%Vectors for results
-T = t0:dt:t3;
-X = zeros(length(T),1);
-V = zeros(length(T),1);
-A = zeros(length(T),1);
-
-for (i = 1:length(T))
-   t = T(i);
-   if (t < t1) %acceleration phase
-       a= amax;
-       v= v0 + a*(t-t0);
-       x= x0 + v0*(t-t0) + 1/2*a*(t-t0)^2;
-   elseif (t < t2) %constant v phase
-       a= 0;
-       v= vlim;
-       x= x0 + s_acc + v*(t-t1);
-   elseif (t < t3) %deceleration phase
-       a=dmax;
-       v= vlim + a*(t-t2);
-       x= x0 + s_acc + s_vconst + vlim*(t-t2) +1/2*a *(t-t2)^2;
-   end
-   %Write to vectors
-   X(i)=x;
-   V(i)=v;
-   A(i)=a;
-end
-
-
-%% PLOT CURVES:    
-subplot (3,1,1);
-plot(T, X,'r+-');
-hold on;
-grid on;
-ylabel('x(t)[m]');
-
-subplot (3,1,2);
-plot(T, V,'g+-');
-hold on;
-grid on;
-ylabel('v(t)[m/s]');
-
-subplot (3,1,3);
-plot(T, A,'b+-');
-hold on;
-grid on;
-ylabel('a(t)[m/s^2]');
-xlabel('t[s]');
diff --git a/traj/fastest_track_function.m b/traj/fastest_track_function.m
deleted file mode 100644
index c1a88d0..0000000
--- a/traj/fastest_track_function.m
+++ /dev/null
@@ -1,122 +0,0 @@
-function [ T X V A ] = fastest_track_function(x0,v0,xT,vT,vmax,amax,dmax,t0,dt)
-
-
-%UNTITLED Summary of this function goes here
-%   Detailed explanation goes here
-% Implementation of a fastest track motion algorithm.
-% 23.8.2018, Wayne Glettig
-% Fastest track from x0 to xT, with the following given constraints:
-%   vmax: maximum speed of travel 
-%   amax: maximum acceleration
-%   dmax: maximum deceleration
-% The algorithm also takes in account start and end velocities v0 & vT
-% And plots the result in (x,t), (v,t) and (a,t) plots.
-
-
-%% INPUT PARAMETERS:
-% x0=0;  %Start position
-% v0=-1;  %Start velocity
-% 
-% xT=-10; %Target position
-% vT=-1;  %Target velocity
-% 
-% vmax=2; %Maxium allowed velocity
-% amax=1;     %Maximum acceleration
-% dmax=-amax; %Maximum deceleration
-% 
-% t0=0;     %Start time 
-% dt=0.01;  %time step
-
-%% CALCULATE PROFILE:
-% First, we set the motion direction, depending on start and end positions. 
-% Here we also make the algorithm immune to vmax, amax & dmax signs.
-dir=sign(xT-x0);
-amax = dir*abs(amax);
-dmax = -dir*abs(dmax);
-vmax = dir*abs(vmax);
-
-% The motion profile is comprised of three phases:
-% 1) Acceleration phase with full amax acceleration
-% 2) Constant velocity phase with vmax
-% 3) Deceleration phase with full dmax deceleration
-
-% Now if the distance is too short to be able to accelerate up to vmax, 
-% there is no constant velocity phase (phase 2).
-% Let's calculate the displacement required to accelerate (v0->vmax) and 
-% deccelerate (vmax->vT):
-s_acc  = v0*(vmax-v0)/amax + (vmax-v0)^2/2/amax;
-s_decc = vmax*(vT-vmax)/dmax + (vT-vmax)^2/2/dmax;
-
-% Let's see if s_acc and s_decc fit within the required displacement xT-x0.
-if (abs(xT-x0)<abs(s_acc+s_decc))
-    %So, if it doesnt fit, an intermediate vlim is targeted, which is a 
-    %local vmax to accelerate to, until it's time to decelerate.
-    vlim = dir*sqrt((v0^2/2/amax - vT^2/2/dmax + xT - x0)/(1/2/amax - 1/2/dmax));
-    %Also, the constant velocity phase is over a distance of zero.
-    s_vconst = 0;
-    % Recalculate the effective displacements using vlim instead of vmax
-    s_acc  = v0*(vlim-v0)/amax + (vlim-v0)^2/2/amax
-    s_decc = vlim*(vT-vlim)/dmax + (vT-vlim)^2/2/dmax;
-else
-    vlim =vmax;
-    %The constant velocity distance is the remaining distance.
-    s_vconst = (xT-x0)-(s_acc+s_decc);
-end
-
-
-% So now the distances have been established, let's look at the times.
-% t0 --acceleration--> t1 --vconst--> t2 --deceleration--> t3
-%         t_acc            t_vconst           t_decc
-%
-t_acc = (vlim-v0)/amax;
-t_decc = (vT-vlim)/dmax;
-t_vconst = s_vconst/vlim;
-
-t1 = t0 + t_acc;
-t2 = t1 + t_vconst;
-t3 = t2 + t_decc;
-
-
-
-%% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-%Vectors for results
-T = t0:dt:t3;
-X = zeros(length(T),1);
-V = zeros(length(T),1);
-A = zeros(length(T),1);
-
-for (i = 1:length(T))
-   t = T(i);
-   if (t < t1) %acceleration phase
-       a= amax;
-       v= v0 + a*(t-t0);
-       x= x0 + v0*(t-t0) + 1/2*a*(t-t0)^2;
-   elseif (t < t2) %constant v phase
-       a= 0;
-       v= vlim;
-       x= x0 + s_acc + v*(t-t1);
-   elseif (t < t3) %deceleration phase
-       a=dmax;
-       v= vlim + a*(t-t2);
-       x= x0 + s_acc + s_vconst + vlim*(t-t2) +1/2*a *(t-t2)^2;
-   end
-   %Write to vectors
-   X(i)=x;
-   V(i)=v;
-   A(i)=a;
-end
-
-end
-
-% %% PLOT CURVES:    
-% subplot (3,1,1);
-% plot(T, X,'+-');
-% hold on;
-% 
-% subplot (3,1,2);
-% plot(T, V,'+-');
-% hold on;
-% 
-% subplot (3,1,3);
-% plot(T, A,'+-');
-% hold on;
\ No newline at end of file
diff --git a/traj/min_jerk_xy_algorithm.m b/traj/min_jerk_xy_algorithm.m
deleted file mode 100644
index 5b1ef40..0000000
--- a/traj/min_jerk_xy_algorithm.m
+++ /dev/null
@@ -1,55 +0,0 @@
-% Implementation of a minimum jerk motion algorithm, based on Flash & 
-% Hogan (1985)
-% 23.8.2018, Wayne Glettig
-% Path from (x0, y0) to (xT, yT) with the minimum jerk:
-%    (x0, y0) : Start position in x-y plane
-%    (xT, yT) : End position in x-y plane
-% The algorithm also takes in account start and end velocities v0 & vT
-% And plots the result in (x,t), (v,t) and (a,t) plots.
-
-
-%% INPUT PARAMETERS:
-x0=0;  %Start position in x
-y0=0;  %Start position in y
-
-xT=-10; %Target position in x
-yT=-10; %Target position in y
-
-t0=0;     %Start time 
-dt=0.01;  %time step
-tT=1;     %End time
-%% CALCULATE PROFILE: STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-%Vectors for results
-T = t0:dt:tT;
-X = zeros(length(T),1);
-Y = zeros(length(T),1);
-
-for (i = 1:length(T))
-   t = T(i);
-   
-   x = x0 + (x0-xT)*(15*t^4-6*t^5-10*t^3);
-   y = y0 + (y0-yT)*(15*t^4-6*t^5-10*t^3);
-   
-   %Write to vectors
-   X(i)=x;
-   Y(i)=y;
-end
-
-
-%% PLOT CURVES:    
-plot (X,Y,'r+');
-% subplot (3,1,1);
-% plot(T, X,'r+-');
-% hold on;
-% ylabel('x(t)[m]');
-% 
-% subplot (3,1,2);
-% plot(T, V,'g+-');
-% hold on;
-% ylabel('v(t)[m/s]');
-% 
-% subplot (3,1,3);
-% plot(T, A,'b+-');
-% hold on;
-% ylabel('a(t)[m/s^2]');
-% xlabel('t[s]');
diff --git a/traj/pathmotion_inc.m b/traj/pathmotion_inc.m
deleted file mode 100644
index 0ec1597..0000000
--- a/traj/pathmotion_inc.m
+++ /dev/null
@@ -1,115 +0,0 @@
-
-
-
-
-% List of Points at time (one point = one line in matrix)
-% Each Line: t, x, y, z
-P = [0, 0,0,0;...
-     1, 1,1,1;...
-     2, 0,1,1;...
-     3, 2,3,1];
-
-% For each dimension:
-     
-% subplot(1,2,1);
-% plot(P(:,2), P(:,3));
-% xlabel('x');
-% ylabel('y');
-% subplot(1,2,2);
-% plot(P(:,1), P(:,2));
-% xlabel('t');
-% ylabel('x');
-
-
-
-%Points(t, x,y,z) = pt(P)
-%Look at each dimension separately
-%Boundary conditions:
-  % maxaccel = 0.1
-
-% From the PMAC Documentation for PVT Motion profiles:
-% From the specified parameters for the move piece, and the beginning 
-% position and velocity (from the end of the previous piece), PMAC 
-% computes the only third-order position trajectory path to meet the 
-% constraints. This results in linearly changing acceleration, a parabolic 
-% velocity profile, and a cubic position profile for the piece.
-
-% Command:
-% PVT300
-% X5:50
-% Axis X, t=300, p=5, v=50
-clf
-%Vectors of results
-T=[];
-X=[];
-V=[];
-A=[];
-%Input Parameters
-x0=0;
-v0=0;
-a0=0;
-
-xT=7;
-
-
-vmax=1;
-amax=1;
-dt=0.001;
-errorband=0.0001;
-
-%Start values
-x=x0;
-v=v0;
-a=a0;
-
-
-t=0;
-loop =1;
-asign = 1;
-while (loop)
-    t=t+dt; 
-    %bremsweg s = v0^2/2/a
-    %if (xT-x < v^2/2/amax) %check bremsweg. if smaller, decel full.
-    xrem = xT-x;  %remaining x  
-    %if (abs(xrem) < abs(v*dt - 0.5*amax*dt^2))%check bremsweg. if smaller, decel full.
-    if (abs(xT-x) < abs(v^2/2/amax))%check bremsweg. if smaller, decel full.
-        %a=-sign(xT-x)*amax;
-        asign =-1;
-        %disp(['deccelerating t=', num2str(t),' a=',num2str(a)])
-    else 
-        asign = 1;
-    end
-    
-    a=sign(xT-x)*asign*amax;  %calculate new a
-    v=v+a*dt;                 %calculate new v
-    if abs(v)>vmax %limit velocity
-        v=sign(v)*vmax;
-        a=0;
-    end
-    x=x + v*dt + 0.5*a*dt^2;  %calculate new x
-    if (abs(xT-x)<errorband || t > 10) %if reached target or loopcounter = 1000
-        loop=0;
-    end
-%    if (abs(xT-x) > abs(xT-x0))
-%        disp('target couldnt be reached');
-%        loop =0;
-%    end
-    T=[T,t];
-    X=[X,x];
-    V=[V,v];
-    A=[A,a];
-end
-subplot (3,1,1);
-plot(T, X,'r+');
-ylabel('x(t)[m]');
-
-subplot (3,1,2);
-plot(T, V,'g+');
-ylabel('v(t)[m/s]');
-
-subplot (3,1,3);
-plot(T, A,'b+');
-ylabel('a(t)[m/s^2]');
-xlabel('t[s]');
-
-hold on;
\ No newline at end of file
diff --git a/traj/pt.m b/traj/pt.m
deleted file mode 100644
index 26baf43..0000000
--- a/traj/pt.m
+++ /dev/null
@@ -1,25 +0,0 @@
-function [c1 c2 c3 c4 v1 a1] = pt(x0,x1,t0,t1, v0,a0)
-%PVT Summary of this function goes here
-%   Detailed explanation goes here
-
-A= [t0^3, t0^2, t0,  1,0,0; ...
-    t1^3, t1^2, t1,  1,0,0; ...
-    3*t0^2, 2*t0, 1, 0,0,0; ...
-    3*t1^2, 2*t1, 1, 0,-1,0; ...
-    6*t0, 2,  0,   0,0,0; ...
-    6*t1  , 2,  0,   0,0,-1];
-    
-b = [x0;x1;v0;0;a0;0] ;
-
-x= A\b;
-
-c1=x(1);
-c2=x(2);
-c3=x(3);
-c4=x(4);
-v1=x(5);
-a1=x(6);
-
-
-end
-
diff --git a/traj/pt4.m b/traj/pt4.m
deleted file mode 100644
index 18283fb..0000000
--- a/traj/pt4.m
+++ /dev/null
@@ -1,52 +0,0 @@
-function [c1 c2 c3 c4 c5 v1 a1] = pt(x0,x1,t0,t1, v0,a0)
-
-%PVT Summary of this function goes here
-%   Detailed explanation goes here
-
-A= [   t0^4,   t0^3,   t0^2, t0, 1,0,0; ...
-       t1^4,   t1^3,   t1^2, t1, 1,0,0; ...
-     4*t0^3, 3*t0^2, 2*t0  ,  1, 0,0,0; ...
-     4*t1^3, 3*t1^2, 2*t1  ,  1, 0,-1,0; ...
-    12*t0^2, 6*t0  ,   2   ,  0, 0,0,0 ; ...
-    12*t1^2, 6*t1  ,   2   ,  0, 0,0,-1];
-    
-b = [x0;x1;v0;0;a0;0] ;
-
-x= A\b;
-
-c1=x(1);
-c2=x(2);
-c3=x(3);
-c4=x(4);
-c5=x(5);
-v1=x(6);
-a1=x(7);
-
-
-end
-
-% 
-% %PVT Summary of this function goes here
-% %   Detailed explanation goes here
-% 
-% A= [t0^3, t0^2, t0,  1,0,0; ...
-%     t1^3, t1^2, t1,  1,0,0; ...
-%     3*t0^2, 2*t0, 1, 0,0,0; ...
-%     3*t1^2, 2*t1, 1, 0,-1,0; ...
-%     6*t0, 2,  0,   0,0,0; ...
-%     6*t1  , 2,  0,   0,0,-1];
-%     
-% b = [x0;x1;v0;0;a0;0] ;
-% 
-% x= A\b;
-% 
-% c1=x(1);
-% c2=x(2);
-% c3=x(3);
-% c4=x(4);
-% v1=x(5);
-% a1=x(6);
-% 
-% 
-% end
-% 
diff --git a/traj/pt4Test.m b/traj/pt4Test.m
deleted file mode 100644
index 04bf0a7..0000000
--- a/traj/pt4Test.m
+++ /dev/null
@@ -1,68 +0,0 @@
-clear all;
-%% INPUT PARAMETERS:
- xT=[0, 1, -2, 4, 5, -1];     %positions
- tT=[0, 1,  2, 3,  4, 5];     %times
-%    v=[0, -1, 3/2, -1, -1, 0];  %velocities
-vcurr=0;
-acurr=0;
-
-for i = 1:(numel(tT)-1)
-    
-    dt=0.01;
-
-    [c1, c2, c3, c4, c5, v1, a1] = pt4(xT(i),xT(i+1),tT(i),tT(i+1), vcurr, acurr);
-    vcurr=v1;
-    acurr=a1;
-    %% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-    %Vectors for results
-    T = tT(i):dt:tT(i+1);
-    X = zeros(length(T),1);
-    V = zeros(length(T),1);
-    A = zeros(length(T),1);
-    J = zeros(length(T),1);
-
-    for (ii = 1:length(T))
-       t = T(ii);
-       j= 24*c1*t   + 6*c2;
-       a= 12*c1*t^2 + 6*c2*t   + 2*c3;
-       v=  4*c1*t^3 + 3*c2*t^2 + 2*c3*t   + c4;
-       x=    c1*t^4 +   c2*t^3 +   c3*t^2 + c4*t + c5;
-%        j= 6*c1;
-%        a= 6*c1*t + 2*c2;
-%        v= 3*c1*t^2 + 2*c2*t + c3;
-%        x= c1*t^3 + c2*t^2 + c3*t + c4;
-       %Write to vectors
-       X(ii)=x;
-       V(ii)=v;
-       A(ii)=a;
-       J(ii)=j;
-    end
-
-
-    %% PLOT CURVES:    
-    subplot (4,1,1);
-    plot(T, X,'r-');
-    hold on;
-    ylabel('x(t)[m]');
-
-    subplot (4,1,2);
-    plot(T, V,'g-');
-    hold on;
-    ylabel('v(t)[m/s]');
-
-    subplot (4,1,3);
-    plot(T, A,'b-');
-    hold on;
-    ylabel('a(t)[m/s^2]');
-    
-    subplot (4,1,4);
-    plot(T, J,'k-');
-    hold on;
-    ylabel('j(t)[m/s^3]');
-    xlabel('t[s]');
-end
-
-    subplot (4,1,1);
-    stem(tT,xT);
-    hold on;
-   
\ No newline at end of file
diff --git a/traj/ptTest.m b/traj/ptTest.m
deleted file mode 100644
index ca639c5..0000000
--- a/traj/ptTest.m
+++ /dev/null
@@ -1,68 +0,0 @@
-clear all;
-%% INPUT PARAMETERS:
-% xT=[0, 1, -2, 4, 5, -1];     %positions
-% tT=[0, 1,  2, 3,  4, 5];     %times
-n =5
-tT=linspace(0,10,n);
-xT=rand(n,1);
-
-%    v=[0, -1, 3/2, -1, -1, 0];  %velocities
-vcurr=0;
-acurr=0;
-
-for i = 1:(numel(tT)-1)
-    
-    dt=0.01;
-
-    [c1, c2, c3, c4,v1, a1] = pt(xT(i),xT(i+1),tT(i),tT(i+1), vcurr, acurr);
-    vcurr=v1;
-    acurr=a1;
-    %% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-    %Vectors for results
-    T = tT(i):dt:tT(i+1);
-    X = zeros(length(T),1);
-    V = zeros(length(T),1);
-    A = zeros(length(T),1);
-    J = zeros(length(T),1);
-
-    for (ii = 1:length(T))
-       t = T(ii);
-       j= 6*c1;
-       a= 6*c1*t + 2*c2;
-       v= 3*c1*t^2 + 2*c2*t + c3;
-       x= c1*t^3 + c2*t^2 + c3*t + c4;
-       %Write to vectors
-       X(ii)=x;
-       V(ii)=v;
-       A(ii)=a;
-       J(ii)=j;
-    end
-
-
-    %% PLOT CURVES:    
-    subplot (4,1,1);
-    plot(T, X,'r-');
-    hold on;
-    ylabel('x(t)[m]');
-
-    subplot (4,1,2);
-    plot(T, V,'g-');
-    hold on;
-    ylabel('v(t)[m/s]');
-
-    subplot (4,1,3);
-    plot(T, A,'b-');
-    hold on;
-    ylabel('a(t)[m/s^2]');
-    
-    subplot (4,1,4);
-    plot(T, J,'k-');
-    hold on;
-    ylabel('j(t)[m/s^3]');
-    xlabel('t[s]');
-end
-
-    subplot (4,1,1);
-    stem(tT,xT);
-    hold on;
-   
\ No newline at end of file
diff --git a/traj/pvt.m b/traj/pvt.m
deleted file mode 100644
index a5c423d..0000000
--- a/traj/pvt.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function [c1 c2 c3 c4 ] = pvt(x0,x1,v0,v1,t0,t1)
-%PVT Summary of this function goes here
-%   Detailed explanation goes here
-
-A= [t0^3, t0^2, t0, 1; ...
-    t1^3, t1^2, t1, 1; ...
-    3*t0^2, 2*t0, 1, 0; ...
-    3*t1^2, 2*t1, 1, 0];
-    
-b = [x0;x1;v0;v1] ;
-
-x= A\b;
-
-c1=x(1);
-c2=x(2);
-c3=x(3);
-c4=x(4);
-
-
-end
-
diff --git a/traj/pvtTest.m b/traj/pvtTest.m
deleted file mode 100644
index 5a8ebca..0000000
--- a/traj/pvtTest.m
+++ /dev/null
@@ -1,70 +0,0 @@
-clear all;
-%% INPUT PARAMETERS:
-% tT=[0, 1,  2, 3,  4, 5];     %times
-% xT=[0, 1, -2, 4, 5, -1];     %positions
-% vT=[0, 0,  0, 0, 0, 0 ];     %velocities
-n=10
-tT=linspace(0,10,n);
-xT=rand(n,1);
-vT=zeros(n,1);
-for i = 2:(numel(tT)-1)
-   vT(i)=(xT(i+1)-xT(i-1))/tT(2)
-
-end
-
-
-dt=0.01;
-
-for i = 1:(numel(tT)-1)
-    [c1, c2, c3, c4 ] = pvt(xT(i),xT(i+1), vT(i),vT(i+1), tT(i),tT(i+1));
-
-    %% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-    %Vectors for results
-    T = tT(i):dt:tT(i+1);
-    X = zeros(length(T),1);
-    V = zeros(length(T),1);
-    A = zeros(length(T),1);
-    J = zeros(length(T),1);
-
-    for (ii = 1:length(T))
-       t = T(ii);
-       j= 6*c1;
-       a= 6*c1*t + 2*c2;
-       v= 3*c1*t^2 + 2*c2*t + c3;
-       x= c1*t^3 + c2*t^2 + c3*t + c4;
-       %Write to vectors
-       X(ii)=x;
-       V(ii)=v;
-       A(ii)=a;
-       J(ii)=j;
-    end
-
-
-    %% PLOT CURVES:    
-    subplot (4,1,1);
-    plot(T, X,'r-');
-    hold on;
-    ylabel('x(t)[m]');
-
-    subplot (4,1,2);
-    plot(T, V,'g-');
-    hold on;
-    ylabel('v(t)[m/s]');
-
-    subplot (4,1,3);
-    plot(T, A,'b-');
-    hold on;
-    ylabel('a(t)[m/s^2]');
-    
-    subplot (4,1,4);
-    plot(T, J,'k-');
-    hold on;
-    ylabel('j(t)[m/s^3]');
-    xlabel('t[s]');
-end
-
-
-    subplot (4,1,1);
-    stem(tT,xT);
-    hold on;
-   
\ No newline at end of file
diff --git a/traj/pvt_polynomial_algorithm.m b/traj/pvt_polynomial_algorithm.m
deleted file mode 100644
index e90bab9..0000000
--- a/traj/pvt_polynomial_algorithm.m
+++ /dev/null
@@ -1,80 +0,0 @@
-% Implementation of a PVT motion algorithm.
-% 23.8.2018, Wayne Glettig
-% Fastest track from x0 to xT, with the following given constraints:
-%   vmax: maximum speed of travel 
-%   amax: maximum acceleration
-%   dmax: maximum deceleration
-% The algorithm also takes in account start and end velocities v0 & vT
-% And plots the result in (x,t), (v,t) and (a,t) plots.
-
-
-%% INPUT PARAMETERS:
-x0=0;  %Start position
-v0=1;  %Start velocity
-
-xT=-5; %Target position
-vT=0;  %Target velocity
-
-vmax=0.5; %Maxium allowed velocity
-amax=0.4;     %Maximum acceleration
-dmax=-amax; %Maximum deceleration
-
-t0=0;     %Start time 
-tT=20;     %End Time
-dt=0.01;  %time step
-
-%% CALCULATE PROFILE:
-
-
-A= [t0^3, t0^2, t0, 1; ...
-    tT^3, tT^2, tT, 1; ...
-    3*t0^2, 2*t0, 1, 0; ...
-    3*tT^2, 2*tT, 1, 0];
-b= [x0;xT;v0;vT] ;
-
-x= A\b;
-
-c1=x(1);
-c2=x(2);
-c3=x(3);
-c4=x(4);
-
-
-
-
-%% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-%Vectors for results
-T = t0:dt:tT;
-X = zeros(length(T),1);
-V = zeros(length(T),1);
-A = zeros(length(T),1);
-J = zeros(length(T),1);
-
-for (i = 1:length(T))
-   t = T(i);
-   a= 6*c1*t + 2*c2;
-   v= 3*c1*t^2 + 2*c2*t + c3;
-   x= c1*t^3 + c2*t^2 + c3*t + c4;
-   %Write to vectors
-   X(i)=x;
-   V(i)=v;
-   A(i)=a;
-end
-
-
-%% PLOT CURVES:    
-subplot (3,1,1);
-plot(T, X,'r+-');
-hold on;
-ylabel('x(t)[m]');
-
-subplot (3,1,2);
-plot(T, V,'g+-');
-hold on;
-ylabel('v(t)[m/s]');
-
-subplot (3,1,3);
-plot(T, A,'b+-');
-hold on;
-ylabel('a(t)[m/s^2]');
-xlabel('t[s]');
diff --git a/traj/pvt_polynomial_algorithm_stretch.m b/traj/pvt_polynomial_algorithm_stretch.m
deleted file mode 100644
index 4dc440e..0000000
--- a/traj/pvt_polynomial_algorithm_stretch.m
+++ /dev/null
@@ -1,103 +0,0 @@
-% Implementation of a PVT motion algorithm.
-% Using variable amax to stretch out time.
-% Regognizes if motion can't be completed in requested time.
-% (checks if (tT-t0) < t_fastest) and sets time to fastest possible.
-% 23.8.2018, Wayne Glettig
-% Fastest track from x0 to xT, with the following given constraints:
-%   vmax: maximum speed of travel 
-%   amax: maximum acceleration
-%   dmax: maximum deceleration
-% The algorithm also takes in account start and end velocities v0 & vT
-% And plots the result in (x,t), (v,t) and (a,t) plots.
-
-
-%% INPUT PARAMETERS:
-x0=0;  %Start position
-v0=-1;  %Start velocity
-
-xT=-10; %Target position
-vT=0;  %Target velocity
-
-t0=0;     %Start time 
-tT=20;     %End Time
-dt=0.01;  %time step
-
-vmax=0.5; %Maxium allowed velocity
-if (v0 > vmax)
-    v0 = vmax;
-end
-if (v0 < -vmax)
-    v0 = -vmax;
-end
-%amax=0.4;     %Maximum acceleration
-%dmax=-amax; %Maximum deceleration
-
-%% CALCULATE PROFILE:
-
-
-A= [t0^3, t0^2, t0, 1; ...
-    tT^3, tT^2, tT, 1; ...
-    3*t0^2, 2*t0, 1, 0; ...
-    3*tT^2, 2*tT, 1, 0];
-b= [x0;xT;v0;vT] ;
-
-x= A\b;
-
-c1=x(1);
-c2=x(2);
-c3=x(3);
-c4=x(4);
-
-t_vmax = -2*c2/6/c1;
-if (t_vmax < t0)      %minima/maxima before t0
-    vmax= 3*c1*t0^2 + 2*c2*t0 + c3;
-elseif (t_vmax > tT)  %minima/maxima after tT
-    vmax= 3*c1*tT^2 + 2*c2*tT + c3;
-else
-    vmax= 3*c1*t_vmax^2 + 2*c2*t_vmax + c3;
-end
-
-if (abs(v0) > abs(vmax))
-    vmax = v0;
-end
-
-vmax
-    
-
-
-%% STEP THROUGH PROFILE WITH GIVEN SAMPLE RATE:
-%Vectors for results
-T = t0:dt:tT;
-X = zeros(length(T),1);
-V = zeros(length(T),1);
-A = zeros(length(T),1);
-J = zeros(length(T),1);
-
-for (i = 1:length(T))
-   t = T(i);
-   a= 6*c1*t + 2*c2;
-   v= 3*c1*t^2 + 2*c2*t + c3;
-   x= c1*t^3 + c2*t^2 + c3*t + c4;
-   %Write to vectors
-   X(i)=x;
-   V(i)=v;
-   A(i)=a;
-end
-
-
-%% PLOT CURVES:    
-subplot (3,1,1);
-plot(T, X,'r+-');
-hold on;
-ylabel('x(t)[m]');
-
-subplot (3,1,2);
-plot(T, V,'g+-');
-hold on;
-ylabel('v(t)[m/s]');
-
-subplot (3,1,3);
-plot(T, A,'b+-');
-hold on;
-ylabel('a(t)[m/s^2]');
-xlabel('t[s]');
diff --git a/traj/tmp.m b/traj/tmp.m
deleted file mode 100644
index b0aa9ab..0000000
--- a/traj/tmp.m
+++ /dev/null
@@ -1,15 +0,0 @@
-clear all;
-syms xT x0 v0 vT t0 t1 t2 t3 tT vmax amax dmax attn s_acc s_decc;
-
-% area under the v-t curve must be xT-x0
-% solve( xT-x0==attn*amax*(t1-t0)/2 + attn*dmax*(t3-t1)/2, attn)
-
-%s_acc  == v0*(vmax-v0)/attn*amax + (vmax-v0)^2/2/attn*amax;
-%s_decc == vmax*(vT-vmax)/attn*dmax + (vT-vmax)^2/2/attn*dmax;
-%xT-x0 == s_acc + s_decc;
-%solve(xT-x0 == v0*(vmax-v0)/attn*amax + (vmax-v0)^2/2/attn*amax + vmax*(vT-vmax)/attn*dmax + (vT-vmax)^2/2/attn*dmax, attn)
-solve(xT-x0 == v0*((tT-t0)-(vT-vmax)/(attn*dmax)) + (vmax-v0)^2/2/(attn*amax) + vmax*(vT-vmax)/(attn*dmax) + (vT-vmax)^2/2/(attn*dmax),attn)
-
-
-%xT-x0 == v0*((tT-t0)-(vT-vmax)/(attn*dmax)) + (vmax-v0)^2/2/(attn*amax) + vmax*(vT-vmax)/(attn*dmax) + (vT-vmax)^2/2/(attn*dmax);
-%(tT-t0)-(vT-vmax)/(attn*dmax) = (vmax-v0)/(attn*amax);
\ No newline at end of file
diff --git a/traj/trajectory.py b/traj/trajectory.py
deleted file mode 100644
index 761bfb8..0000000
--- a/traj/trajectory.py
+++ /dev/null
@@ -1,76 +0,0 @@
-# -*- coding: utf-8 -*-
-"""
-Spyder Editor
-
-This is a temporary script file.
-"""
-#import numpy as np
-import matplotlib.pyplot as plt
-
-
-class traj:
-    t = 0.
-    x = 0.
-    v = 0.
-    a = 0.
-    t_step =0.01
-    x_target = 10.
-    # parameters    
-    v_max = 0.
-    a_max = 1.
-    
-    def set (self, t, x_current, v_current):
-        return
-    def set_step (self, time):
-        self.t_step = time
-        return
-    def run (self, step_t):
-        #based on current position wrt target position
-        #check if with current position and velocity, we 
-        offdist = self.x_target-self.x
-        brakedist = -self.v*self.v/-(2*self.a_max)
-        
-        if (offdist > brakedist):
-            a_sign = 1
-        else:
-            a_sign = -1
-                 
-        #reset motion equations
-        self.a = a_sign*self.a_max
-        self.v = self.v + self.a*step_t
-        self.x = self.x + self.v*step_t
-        return
-    def runstep (self):
-        self.t = self.t+self.t_step
-        self.run(self.t)
-        return
-    
-
-#Create Object
-tr = traj();
-
-#initialize List
-T = [tr.t]
-X = [tr.x]
-V = [tr.v]
-A = [tr.a]
-#initialize trajectory
-tr.set(0, 0, 0)
-
-#step though trajectory
-for i in range (0,50):
-    tr.runstep()
-    T.append(tr.t)
-    X.append(tr.x)
-    V.append(tr.v)
-    A.append(tr.a)
-   
-#plot curves
-plt.plot(T,X,'-+')
-plt.plot(T,V)
-plt.plot(T,A)
-plt.title("Title")
-plt.xlabel('t[s]')
-plt.ylabel('x[m]')
-
-