TA = Turn Angle, specified at runtime for arcs
TS = Turn Speed, calculated for arcs
DS = Drive Speed, specified at runtime for lines, computed for arcs
DD = Drive Distance, specified at runtime for lines, computed for arcs
AT = Arc Time, specified at runtime for arcs, computed for lines
AR = Arc Radius, specified at runtime for arcs
Final X position, computed at end of every run
Final Y position, computed at end of every run
Distance from desired position, vector distance from desired goal to actual goal
Distance travelled, integrated drive speed over entire run
Time to execute, estimated time to complete run
DESIRED SOLUTION: Item: 0,STUA TA: 90.000 TS: 40.909 DS:0.357 DD: 0.785 AT: 2.200 AR: 0.500 Item: 1,STUA TA: -90.000 TS: -52.941 DS:0.924 DD: 1.571 AT: 1.700 AR: 1.000 Item: 2,drud TA: 0.000 TS: 0.000 DS:1.000 DD: 3.000 AT: 3.000 AR: 0.000 Item: 3,STUA TA: 82.000 TS: 13.898 DS:0.243 DD: 1.431 AT: 5.900 AR: 1.000 Item: 4,drud TA: 0.000 TS: 0.000 DS:0.600 DD: 0.500 AT: 0.833 AR: 0.000 Item: 5,STUA TA: 180.000 TS: 54.545 DS:1.047 DD: 3.456 AT: 3.300 AR: 1.100 Item: 6,drud TA: 0.000 TS: 0.000 DS:0.200 DD: 1.200 AT: 6.000 AR: 0.000 Item: 7,STUA TA:-270.000 TS: -38.028 DS:0.597 DD: 4.241 AT: 7.100 AR: 0.900 Item: 8,drud TA: 0.000 TS: 0.000 DS:0.400 DD: 1.200 AT: 3.000 AR: 0.000 Final X position: 3.637 feet Final Y position: 2.823 feet Distance travelled: 17.38 feet Time to execute: 33.03 seconds NON-OPTIMIZED SOLUTION: Item: 0,STUA TA: 90.000 TS: 40.900 DS:0.300 DD: 0.660 AT: 2.200 AR: 0.500 Item: 1,STUA TA: -90.000 TS: -53.000 DS:0.900 DD: 1.528 AT: 1.698 AR: 1.000 Item: 2,drud TA: 0.000 TS: 0.000 DS:1.000 DD: 3.000 AT: 3.000 AR: 0.000 Item: 3,STUA TA: 82.000 TS: 13.800 DS:0.200 DD: 1.188 AT: 5.942 AR: 1.000 Item: 4,drud TA: 0.000 TS: 0.000 DS:0.600 DD: 0.500 AT: 0.833 AR: 0.000 Item: 5,STUA TA: 180.000 TS: 54.500 DS:1.000 DD: 3.303 AT: 3.303 AR: 1.100 Item: 6,drud TA: 0.000 TS: 0.000 DS:0.200 DD: 1.200 AT: 6.000 AR: 0.000 Item: 7,STUA TA:-270.000 TS: -38.100 DS:0.500 DD: 3.543 AT: 7.087 AR: 0.900 Item: 8,drud TA: 0.000 TS: 0.000 DS:0.400 DD: 1.200 AT: 3.000 AR: 0.000 Final X position: 3.585 feet Final Y position: 2.389 feet Distance from desired position: 0.438 feet Distance travelled: 16.12 feet Time to execute: 33.06 seconds OPTIMIZED SOLUTION: Item: 0,STUAE TA: 30.100 TS: 34.300 DS:0.300 DD: 0.263 AT: 0.876 AR: 0.500 Item: 1,STUAE TA: 30.000 TS: 45.800 DS:0.400 DD: 0.262 AT: 0.654 AR: 0.500 Item: 2,STUAC TA: 30.100 TS: 34.300 DS:0.300 DD: 0.263 AT: 0.876 AR: 0.500 Item: 3,STUAE TA: -29.900 TS: -51.600 DS:0.900 DD: 0.522 AT: 0.580 AR: 1.000 Item: 4,STUAE TA: -30.000 TS: -57.300 DS:1.000 DD: 0.524 AT: 0.524 AR: 1.000 Item: 5,STUAC TA: -29.900 TS: -51.600 DS:0.900 DD: 0.522 AT: 0.580 AR: 1.000 Item: 6,drud TA: 0.000 TS: 0.000 DS:1.000 DD: 3.000 AT: 3.000 AR: 0.000 Item: 7,STUAE TA: 27.400 TS: 11.400 DS:0.200 DD: 0.478 AT: 2.391 AR: 1.000 Item: 8,STUAE TA: 27.300 TS: 17.100 DS:0.300 DD: 0.476 AT: 1.588 AR: 1.000 Item: 9,STUAC TA: 27.400 TS: 11.400 DS:0.200 DD: 0.478 AT: 2.391 AR: 1.000 Item:10,drud TA: 0.000 TS: 0.000 DS:0.600 DD: 0.500 AT: 0.833 AR: 0.000 Item:11,STUAE TA: 60.100 TS: 52.000 DS:1.000 DD: 1.154 AT: 1.154 AR: 1.100 Item:12,STUAE TA: 60.000 TS: 57.200 DS:1.100 DD: 1.152 AT: 1.047 AR: 1.100 Item:13,STUAC TA: 60.100 TS: 52.000 DS:1.000 DD: 1.154 AT: 1.154 AR: 1.100 Item:14,drud TA: 0.000 TS: 0.000 DS:0.200 DD: 1.200 AT: 6.000 AR: 0.000 Item:15,STUAE TA: -89.900 TS: -31.900 DS:0.500 DD: 1.412 AT: 2.824 AR: 0.900 Item:16,STUAE TA: -90.000 TS: -38.200 DS:0.600 DD: 1.414 AT: 2.356 AR: 0.900 Item:17,STUAC TA: -89.900 TS: -31.900 DS:0.500 DD: 1.412 AT: 2.824 AR: 0.900 Item:18,drud TA: 0.000 TS: 0.000 DS:0.400 DD: 1.200 AT: 3.000 AR: 0.000 Final X position: 3.632 feet Final Y position: 2.841 feet Distance from desired position: 0.018 feet Distance travelled: 17.38 feet Time to execute: 34.65 seconds
/*****************************************************************************
*
* Robot Trajectory Simulator
* By Roman Mach
*
* 2/27/92
*
* Written using Turbo C++, v1.0
*
*****************************************************************************/
#include < stdio.h>
#include < string.h>
#include < graphics.h>
#include < stdlib.h>
#include < conio.h>
#include < math.h>
#include "shade.h" /* use for generating bitmapped output of display */
void dotrajectory(double, double, double, int);
void drawbackground(void);
void writefinal(void);
void initcolors(void);
void opengraph(void);
void calculate(void);
void makearray(void);
void getoptimum(void);
#define rad (3.14159265 / 180)
/* available command types */
#define STUA "STUA"
#define STUAC "STUAC"
#define STUAE "STUAE"
#define drud "drud"
#define MAINWIDTH 4 /* number of supplied parameters */
#define TYPES 3 /* number of different tests to perform */
/*****************************************************************************
1. Enter desired time to complete arc, ignored for lines, in first column
2. Enter desired arc radius for arcs or drive dist for lines in second column
3. Enter turn angle for arcs, ignored for lines, in third column
4. Enter zero if arc or drive speed if line in fourth column
Example:
double driver[][MAINWIDTH] =
{
3, 0.5, 90.0, 0.0,
0, 1.0, 0.0, 0.5,
};
This will move the robot (viewed from top) in an arc clockwise,
and then in a straight line.
NOTE 1: angles for robot are revesed from standard cartesian space so reverse
when entering into the array.
NOTE 2: it is possible to specify negative drive speeds.
NOTE 3: LLAMA's STUA functions expect positive and negative turn speeds and
only positive turn angles, which is not the case here.
NOTE 4: LLAMA's STUA functions accept negative drive velocities. These
algorithms do not use negative drive speeds in the conversion process.
arc time, arc radius, turn angle, drive speed(0==arc,other=line)
seconds, feet, degrees, feet per second
(drive dist)
*****************************************************************************/
double driver[][MAINWIDTH] =
{
2.2, 0.5, 90.0, 0.0,
1.7, 1.0, -90.0, 0.0,
0.0, 3.0, 0.0, 1.0,
5.9, 1.0, 82.0, 0.0,
0.0, 0.5, 0.0, 0.6,
3.3, 1.1, 180.0, 0.0,
0.0, 1.2, 0.0, 0.2,
7.1, 0.9, -270.0, 0.0,
0.0, 1.2, 0.0, 0.4
};
FILE *output;
/* pointers to matrix containing various parameters for each test */
/* these pointers need to be initiallized by doing a "malloc" */
double **arctime;
double **arcradius;
double **turnangle;
double **turnspeed;
double **drivespeed;
double **drivedist;
char ***desc;
/* this contains the number of "until" commands needed to execute a test */
int sizevect[TYPES];
/* various motion simulator variables */
double ap, bp; /* "previous position" variables for x, y */
double ds, tm; /* distance moved, and time elapsed */
double x0,a0,b0; /* instantaneous drive speed, x and y positions */
double edx, edy, err; /* error difference variables for positional tests */
double slice = 0.001; /* simulator resolution, decrease to improve accuracy */
double dt; /* delta time variable */
double scale = 500; /* used to scale image size on screen */
int xscr, yscr; /* used to draw images on screen */
double xlsc, ylsc; /* used as temp variables for screen */
int gmc, gac; /* maximum and average color variables for screen */
int xmax, ymax; /* maximum screen x and y positions */
int ux, uy; /* temp variables */
void
main(void)
{
int i, j;
double drv, ome;
opengraph();
if((output = fopen("simout1.txt","w"))==NULL) /* write results to file */
{
printf("error opening file\n");
exit(0);
}
makearray(); /* malloc space for arrays */
initcolors(); /* get screen details */
ux = xlsc = (int) ((float) xmax / -32.0 ); /* place origin on screen */
uy = ylsc = (int) ((float) ymax * 1.05 );
dt = slice / 2;
drawbackground(); /* draw grid marks */
calculate(); /* calculate all variables */
for(j=0;j< TYPES;j++) /* perform tests */
{
if(!j)
{
setcolor(gac);
setlinestyle(0,0,3);
}
else
{
setcolor(gmc);
setlinestyle(0,0,1);
}
x0 = a0 = b0 = ds = tm = ap = bp = 0; /* reset simulator for each test */
xlsc = ux; /* reset "line-to" function */
ylsc = uy;
for (i = 0; i< sizevect[j]; i++) /* do once for each command */
{
fprintf(output,"Item:%2d,%s\tTA:%8.3f TS:%8.3f DS:%5.3f DD:%6.3f AT:%7.3f AR:%6.3f\n",
i, desc[j][i],
turnangle[j][i], turnspeed[j][i],
drivespeed[j][i], drivedist[j][i],
arctime[j][i], arcradius[j][i]
);
/* go do running simulation of motion */
dotrajectory(arctime[j][i], drivespeed[j][i], turnspeed[j][i], i);
}
if(!j) /* check for delta position output */
{
edx = a0;
edy = b0;
err = 0;
}
else
{
err = hypot(edx-a0, edy-b0);
}
fprintf(output,"\n");
fprintf(output,"Final X position:\t%7.3f feet\n",a0*2);
fprintf(output,"Final Y position:\t%7.3f feet\n",b0*2);
if(j!=0)
fprintf(output,"Distance from desired position:\t%7.3f feet\n",2*err);
fprintf(output,"Distance travelled:\t%8.2f feet\n",ds*2);
fprintf(output,"Time to execute:\t%8.2f seconds\n\n\n",tm);
}
/* saveimg("x1.bmp",gmc,INVIMG);*/ /* used to create bitmapped file of screen */
close(output);
getch();
closegraph();
}
/***********************************************
*
* Calculate unknown parameters
*
***********************************************/
void
calculate()
{
int i,j;
/* first calculate full precision STUA and drud commands */
for(i=0;i< sizevect[0];i++)
{
arctime[0][i] = driver[i][0]; /* obtain values from matrix */
arcradius[0][i] = driver[i][1];
turnangle[0][i] = driver[i][2];
/* if STUA do this */
if(!driver[i][3])
{
drivespeed[0][i] = rad*arcradius[0][i]*fabs(turnangle[0][i])/arctime[0][i];
turnspeed[0][i] = turnangle[0][i] / arctime[0][i];
drivedist[0][i] = fabs(drivespeed[0][i] * arctime[0][i]);
strcpy(desc[0][i],STUA);
}
/* otherwise do this */
else
{
drivespeed[0][i] = driver[i][3];
turnspeed[0][i] = 0;
drivedist[0][i] = fabs(driver[i][1]);
arctime[0][i] = fabs(drivedist[0][i] / drivespeed[0][i]);
arcradius[0][i] = 0;
strcpy(desc[0][i],drud);
}
}
/* next calculate simple STUA reduced precision commands */
/* use "floor" function to put appropriate values into tenths-of */
for(i=0;i< sizevect[1];i++)
{
arcradius[1][i] = arcradius[0][i];
turnangle[1][i] = floor(driver[i][2] * 10) / 10;
drivespeed[1][i] = floor(drivespeed[0][i] * 10) / 10;
turnspeed[1][i] = floor(turnspeed[0][i] * 10) / 10;
if(!driver[i][3])
{
arctime[1][i] = fabs(turnangle[1][i] / turnspeed[1][i]);
drivedist[1][i] = fabs(drivespeed[1][i] * arctime[1][i]);
strcpy(desc[1][i],STUA);
}
else
{
drivedist[1][i] = floor(drivedist[0][i] * 10) / 10;
arctime[1][i] = fabs(drivedist[1][i] / drivespeed[1][i]);
strcpy(desc[1][i],drud);
}
}
/* finally, get optimize STUAE and STUAC functions */
getoptimum();
}
/***********************************************
*
* Convert STUA's to STUAE's and STUAC's
*
***********************************************/
void
getoptimum()
{
int i,j;
double getang, getspd;
double maxspeed, minspeed;
double minang, maxang;
j=0; /* set up variable to build up new elements */
for(i=0;i< sizevect[0];i++) /* go through accurate list */
{
if(!driver[i][3]) /* do this if STUA-type */
{
getang = turnangle[0][i] / 3; /* divide desired angle into 3 parts */
minang = floor(getang * 10) / 10; /* get a min and max version */
maxang = floor(getang * 10 + 1) /10;
/* get drive speed, then get integer version and convert to min and max values */
getspd = rad * arcradius[0][i] * fabs(turnangle[0][i]) / arctime[0][i];
minspeed = floor(getspd * 10) / 10; /* get a min and max version */
maxspeed = floor(getspd * 10 + 1) / 10;
/* set up first STUAE */
/* note that minspeed is used with maxangle to help minimize errors */
arctime[2][j] = rad * arcradius[0][i] * fabs(maxang) / minspeed;
turnspeed[2][j] = floor((maxang / arctime[2][j]) * 10) / 10;
drivedist[2][j] = minspeed * arctime[2][j];
arcradius[2][j] = arcradius[0][i];
drivespeed[2][j] = minspeed;
turnangle[2][j] = maxang;
strcpy(desc[2][j],STUAE);
j++;
/* set up second STUAE */
/* note that min's and max's have been reversed from previous */
arctime[2][j] = rad * arcradius[0][i] * fabs(minang) / maxspeed;
turnspeed[2][j] = floor((minang / arctime[2][j]) * 10) / 10;
drivedist[2][j] = maxspeed * arctime[2][j];
arcradius[2][j] = arcradius[0][i];
drivespeed[2][j] = maxspeed;
turnangle[2][j] = minang;
strcpy(desc[2][j],STUAE);
j++;
/* finally, do a STUAC, similar to the first STUAE */
arctime[2][j] = rad * arcradius[0][i] * fabs(maxang) / minspeed;
turnspeed[2][j] = floor((maxang / arctime[2][j]) * 10) / 10;
drivedist[2][j] = minspeed * arctime[2][j];
arcradius[2][j] = arcradius[0][i];
drivespeed[2][j] = minspeed;
turnangle[2][j] = maxang;
strcpy(desc[2][j],STUAC);
j++;
}
else /* otherwise do a drud */
{
arcradius[2][j] = 0;
drivespeed[2][j] = floor(driver[i][3] * 10) / 10;
drivedist[2][j] = floor(fabs(driver[i][1]) * 10) / 10;
turnspeed[2][j] = 0;
turnangle[2][j] = 0;
arctime[2][j] = fabs(drivedist[2][j] / drivespeed[2][j]);
strcpy(desc[2][j],drud);
j++;
}
}
}
/***********************************************
*
* Initialize all array/matrix pointers
*
***********************************************/
void
makearray()
{
int size, i, j;
size = sizeof (driver) / (sizeof (double) * MAINWIDTH);
arctime = (double **) malloc(sizeof (double *) * TYPES);
arcradius = (double **) malloc(sizeof (double *) * TYPES);
turnspeed = (double **) malloc(sizeof (double *) * TYPES);
drivespeed = (double **) malloc(sizeof (double *) * TYPES);
drivedist = (double **) malloc(sizeof (double *) * TYPES);
desc = (char ***) malloc(sizeof (char **) * TYPES);
sizevect[0] = sizevect[1] = sizevect[2] = size;
for(i=0;i< size;i++)
if(!driver[i][3])
sizevect[2] += 2;/* need to generate new elements, get more space */
for(i=0;i< TYPES;i++)
{
arctime[i] = (double *) malloc(sizeof (double) * sizevect[i]);
arcradius[i] = (double *) malloc(sizeof (double) * sizevect[i]);
turnspeed[i] = (double *) malloc(sizeof (double) * sizevect[i]);
drivespeed[i] = (double *) malloc(sizeof (double) * sizevect[i]);
drivedist[i] = (double *) malloc(sizeof (double) * sizevect[i]);
desc[i] = (char **) malloc(sizeof (char *) * sizevect[i]);
for(j=0;j< sizevect[i];j++)
desc[i][j] = (char *) malloc(sizeof (char) * 10);
}
}
/***********************************************
*
* Draw reference grids on screen
*
***********************************************/
void
drawbackground()
{
int i, j, tx, ty;
line(ux-1000,uy,ux+1000,uy);
line(ux,uy-1000,ux,uy+1000);
for(i=-1;i< 12;i++)
for(j=-12;j< 1;j++)
{
tx = i * scale / 2 + ux;
ty = j * scale / 2 + uy;
line(tx-1,ty,tx+1,ty);
line(tx,ty+1,tx,ty-1);
rectangle(tx-2,ty-2,tx+2,ty+2);
}
outtextxy(ux+3,3,"y");
outtextxy(xmax-20,uy+3,"x");
}
/***********************************************
*
* Calculate running position estimate
*
***********************************************/
void
dotrajectory(double time, double drv, double ome, int sequence)
{
double sumtot = 0;
double pie, fill;
char strn[30];
int i;
tm += time; /* calculate accumulated time */
pie = dt * ome * 2; /* this represents an instantaneous velocity vector */
for(i=0; (sumtot+=slice) < time; i++)
{
x0 += pie; /* this represents an integrated velocity vector */
fill = x0 * rad; /* convert to appriopriate representaion */
a0 += dt * drv * cos(fill); /* calculate instantaneous x,y change of positions */
b0 += dt * drv * sin(fill); /* and integrate to get current*/
ds += hypot(a0-ap, b0-bp); /* calculate change of position, distance moved */
ap = a0; /* save for later */
bp = b0;
if(!(i%10)) /* draw a line on the screen every 10 calculations */
{
xscr = ux + a0 * scale;
yscr = uy - b0 * scale;
if(!i) /* draw a marker for every command */
{
rectangle(xscr-1,yscr-1,xscr+1,yscr+1);
/* outtextxy(xscr+4,yscr+6,itoa(sequence,strn,10));*/
}
line (xscr, yscr, xlsc, ylsc);
xlsc = xscr;
ylsc = yscr;
}
}
}
/***********************************************
*
* Get screen data
*
***********************************************/
void
initcolors()
{
gmc = getmaxcolor();
if (gmc != 1)
gac = gmc / 2;
else
gac = 0;
setcolor(gac);
xmax = getmaxx();
ymax = getmaxy();
}
/***********************************************
*
* Initialize Graphics
*
***********************************************/
void
opengraph()
{
int gdriver = DETECT, gmode, errorcode;
initgraph(& gdriver, & gmode, "d:\\tc\\bgi");
errorcode = graphresult();
if (errorcode != grOk)
{
printf("Graphics error: %s\n",grapherrormsg(errorcode));
printf("press any key to halt:");
getch();
exit(1);
}
}