mx/smargopolo
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added paramfile

Browse Source
This commit is contained in:
Wayne Glettig
2019-07-25 09:28:32 +02:00
parent c41c514668
commit 29deb702cc
20 changed files with 338 additions and 1010 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
paramfile/Makefile Normal file
View File

@@ -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

37
paramfile/README Normal file
View File

@@ -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.?
*

82
paramfile/Test.cpp Normal file
View File

@@ -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;
}

207
paramfile/readparamfile.h Normal file
View File

@@ -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;
}

BIN
traj/Motion Calculation.pdf
View File

Binary file not shown.

16
traj/Test.m
View File

@@ -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);

124
traj/fastest_track_algorithm.m
View File

@@ -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]');

122
traj/fastest_track_function.m
View File

@@ -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;

55
traj/min_jerk_xy_algorithm.m
View File

@@ -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]');

115
traj/pathmotion_inc.m
View File

@@ -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;

25
traj/pt.m
View File

@@ -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

52
traj/pt4.m
View File

@@ -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
%

68
traj/pt4Test.m
View File

@@ -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;

68
traj/ptTest.m
View File

@@ -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;

21
traj/pvt.m
View File

@@ -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

70
traj/pvtTest.m
View File

@@ -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;

80
traj/pvt_polynomial_algorithm.m
View File

@@ -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]');

103
traj/pvt_polynomial_algorithm_stretch.m
View File

@@ -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]');

15
traj/tmp.m
View File

@@ -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);

76
traj/trajectory.py
View File

@@ -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]')