/*
* Copyright (c) 2007 Hypertriton, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
* USE OF THIS SOFTWARE EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* This program generates velocity profiles using the squared sine algorithm.
* It has been used to simulate aspects of the trajectory planner implemented
* in machctl (http://machctl.hypertriton.com/).
*
* It requires the FreeSG math library. If the SG library is also available,
* trajectories can be plotted as well.
*/
#include
#include
#include
#ifdef HAVE_FREESG
#include
#include "voxelpath.h"
#endif
#include
#include
#include
M_Real L = 100.0;
M_Real F = 2000.0/1e3;
M_Real Amax = 8000.0/1e6;
M_Real Jmax = 100.0/1e6;
M_Real uTs = 0.0;
M_Real uTa = 0.0;
M_Real uTo = 0.0;
M_Plot *plVel, *plAcc, *plJerk;
M_PlotLabel *plblCase;
/* Generated constants for squared sine */
M_Real Aref;
M_Real v, k;
M_Real Ts, Ta, To;
M_Real t1, t2, t3, t4, t5, t6, t7;
M_Real v1, v2, v3;
/*
* Compute the constants used in the squared sine algorithm, given the
* parameters L (displacement), F, Amax and Jmax.
*/
static void
ComputeSquaredSineConstants(void)
{
const char *which;
/*
* Compute the shortest amount of time needed for the squared
* sine velocity profile for the given feedrate, under constraints
* of maximum acceleration and maximum jerk.
*/
if (F >= M_PI_2*((Amax*Amax)/Jmax) ) {
/*
* Feedrate F is achievable by using maximum acceleration
* and maximum jerk.
*/
if (L >= ( (F*F)/Amax + M_PI_2*((Amax*F)/Jmax) )){
/* Feedrate F is achievable at Amax acceleration. */
which = "1.1";
Ts = M_PI_2*(Amax/Jmax);
Ta = (F/Amax) + Ts;
To = L/F;
} else if (L < ( (F*F)/Amax + M_PI_2*((Amax*F)/Jmax) ) &&
L >= ( (M_PI*M_PI)/2.0) * (Amax*Amax*Amax) /
(Jmax*Jmax) ) {
/* Feedrate F is unachievable, Amax is achievable. */
which = "1.2";
Ts = M_PI_2*(Amax/Jmax);
Ta = (( -M_PI*(Amax*Amax) +
sqrt((M_PI*M_PI)*(Amax*Amax*Amax*Amax) +
16.0*Amax*(Jmax*Jmax)*L)
) /
( 4.0*Jmax*Amax )) + Ts;
To = Ta;
} else if (L <= ( ((M_PI*M_PI)/2.0)*(Amax*Amax*Amax) /
(Jmax*Jmax) )) {
/* Neither F nor Amax are achievable. */
which = "1.3";
Ts = cbrt( (M_PI*L)/(4.0*Jmax) );
Ta = 2.0*Ts;
To = Ta;
} else {
printf("CNC_MOVE: Bad case!\n");
return;
}
} else if (F < M_PI_2*((Amax*Amax)/Jmax) ) {
/*
* Feedrate F is achievable without using maximum
* acceleration.
*/
if (L >= sqrt( (2.0*M_PI*(F*F*F))/Jmax ) ) {
/* F is achievable, Amax is unachievable. */
which = "2.1";
Ts = sqrt( (M_PI*F)/(2.0*Jmax) );
Ta = 2.0*Ts;
To = L/F;
} else if (L < sqrt( (2.0*M_PI*(F*F*F))/Jmax )) {
/* Neither F nor Amax are achievable. */
which = "2.2";
Ts = cbrt( (M_PI*L)/(4.0*Jmax) );
Ta = 2.0*Ts;
To = Ta;
} else {
printf("CNC_MOVE: Bad case!\n");
return;
}
}
if (uTs) { Ts = uTs; }
if (uTa) { Ta = uTa; }
if (uTo) { To = uTo; }
k = M_PI/(2.0*Ts);
Aref = L/(Ta-Ts)/To;
v1 = (Aref/2.0) * (Ts - sin(2.0*k*Ts)/(2.0*k) );
v2 = Aref*(Ta - 2.0*Ts) + v1;
v3 = v1+v2;
t1 = Ts;
t2 = Ta-Ts;
t3 = Ta;
t4 = To;
t5 = To+Ts;
t6 = To+Ta-Ts;
t7 = To+Ta;
M_PlotLabelSetText(plVel, plblCase, "case %s", which);
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)(Ts*L/t7), 0, "Ts");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)(Ta*L/t7), 0, "Ta");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)(To*L/t7), 0, "To");
#if 0
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t1, 0, "t1");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t2, 0, "t2");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t3, 0, "t3");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t4, 0, "t4");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t5, 0, "t5");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t6, 0, "t6");
M_PlotLabelReplace(plVel, M_LABEL_X, (unsigned)t7, 0, "t7");
#endif
}
M_Real
SquaredSineStep(M_Real t)
{
if (t <= t1) {
v = (Aref/2.0)*( t - sin(2.0*k*t)/(2.0*k) );
} else if (t <= t2) {
v = Aref*(t-t1) + v1;
} else if (t <= t3) {
v = (Aref/2.0)*( (t-t2) + sin(2.0*k*(t-t2))/(2.0*k) ) + v2;
} else if (t <= t4) {
v = v3;
} else if (t <= t5) {
v = -(Aref/2.0)*( (t-t4) - sin(2.0*k*(t-t4))/(2.0*k) ) + v3;
} else if (t <= t6) {
v = -Aref*(t-t5) + v2;
} else if (t <= t7) {
v = -(Aref/2.0)*( (t-t6) + sin(2.0*k*(t-t6))/(2.0*k) ) + v1;
} else {
v = 0.0;
}
return (v);
}
static void
GeneratePlot(AG_Event *event)
{
M_Plotter *plt = AG_PTR(1);
M_Real t;
M_PlotClear(plVel);
M_PlotClear(plAcc);
M_PlotClear(plJerk);
ComputeSquaredSineConstants();
for (t = 0.0; t < L; t += 1.0) {
M_Real v;
v = SquaredSineStep(t*t7/L);
M_PlotReal(plVel, v);
M_PlotterUpdate(plt);
}
}
static void
CreatePlotView(void)
{
AG_Window *win;
M_Plotter *plt;
AG_Pane *pane;
AG_Button *btn;
AG_Box *box;
int i;
win = AG_WindowNew(0);
AG_WindowSetCaptionS(win, "Motion Profile");
pane = AG_PaneNew(win, AG_PANE_HORIZ, AG_PANE_EXPAND);
{
plt = M_PlotterNew(pane->div[1], M_PLOTTER_EXPAND);
plVel = M_PlotNew(plt, M_PLOT_LINEAR);
plblCase = M_PlotLabelNew(plVel, M_LABEL_OVERLAY, 0, 16,
"-");
M_PlotSetLabel(plVel, "m/s");
plAcc = M_PlotFromDerivative(plt, M_PLOT_LINEAR, plVel);
plAcc->yOffs = 42;
M_PlotSetLabel(plAcc, "m/s^2");
M_PlotSetScale(plAcc, 0.0, 1.0);
plJerk = M_PlotFromDerivative(plt, M_PLOT_LINEAR, plAcc);
plJerk->yOffs = 86;
M_PlotSetLabel(plJerk, "m/s^3");
M_PlotSetScale(plJerk, 0.0, 100.0);
M_PlotSetScale(plAcc, 0.0, 1.0);
}
box = AG_BoxNew(pane->div[0], AG_BOX_VERT, AG_BOX_EXPAND);
{
struct {
const char *name;
void *p;
double incr;
int prec;
} params[] = {
{ "L: ", &L, 10.0, 4 },
{ "F: ", &F, 0.01, 4 },
{ "Amax: ", &Amax, 0.01, 4 },
{ "Jmax: ", &Jmax, 0.0001, 8 },
{ "uTs: ", &uTs, 1.0, 2 },
{ "uTa: ", &uTa, 1.0, 2 },
{ "uTo: ", &uTo, 1.0, 2 },
};
int i;
for (i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
AG_Numerical *num;
num = M_NumericalNewReal(box, AG_NUMERICAL_HFILL, NULL,
params[i].name, params[i].p);
AG_NumericalSetIncrement(num, params[i].incr);
AG_NumericalSetPrecision(num, "f", params[i].prec);
AG_SetEvent(num, "numerical-changed",
GeneratePlot, "%p", plt);
}
#if 0
AG_LabelNewPolled(box, 0, "Aref: %lf", &Aref);
AG_LabelNewPolled(box, 0, "v1: %lf", &v1);
AG_LabelNewPolled(box, 0, "v2: %lf", &v2);
AG_LabelNewPolled(box, 0, "v3: %lf", &v3);
AG_LabelNewPolled(box, 0, "Ts: %lf", &Ts);
AG_LabelNewPolled(box, 0, "Ta: %lf", &Ta);
AG_LabelNewPolled(box, 0, "To: %lf", &To);
#endif
AG_ButtonNewFn(box, 0, "Generate", GeneratePlot, "%p", plt);
}
AG_WindowSetGeometryAlignedPct(win, AG_WINDOW_MC, 60, 40);
AG_WindowShow(win);
}
int
main(int argc, char *argv[])
{
AG_Window *win;
#ifdef HAVE_FREESG
SG *sg;
SG_Voxel *vol;
#endif
if (AG_InitCore("plot2-demo", 0) == -1) {
fprintf(stderr, "%s\n", AG_GetError());
return (1);
}
if (AG_InitGraphics("") == -1) {
fprintf(stderr, "%s\n", AG_GetError());
return (-1);
}
M_InitSubsystem();
AG_BindGlobalKey(AG_KEY_ESCAPE, AG_KEYMOD_ANY, AG_Quit);
AG_BindGlobalKey(AG_KEY_F8, AG_KEYMOD_ANY, AG_ViewCapture);
#ifdef HAVE_FREESG
SG_InitSubsystem();
sg = SG_New(NULL, "scene", 0);
SG_Translate(sg->def.cam, 60.0, 55.0, 170.0);
vol = SG_VoxelNew(sg->root, "Trajectory");
SG_VoxelAlloc3(vol, 128, 128, 128);
VoxelLine(vol);
VoxelPathDialog(vol);
VoxelPathView(sg, vol);
#endif
CreatePlotView();
AG_EventLoop();
AG_Destroy();
return (0);
}