1
0
mirror of git://projects.qi-hardware.com/cae-tools.git synced 2024-12-23 09:43:20 +02:00
cae-tools/gp2rml/gp2rml.c

239 lines
4.8 KiB
C

/*
* gp2rml.c - Convert from gnuplot to RML
*
* Written 2010-2012 by Werner Almesberger
* Copyright 2010-2012 Werner Almesberger
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*/
/*
* When setting the pen up and pen down positions (!PZ), pen up must be a
* zero or positive coordinate relative to Z0 and pen down must be negative
* or zero.
*
* Our pen up position is defined as the maximum pen down height plus some
* clearance. All Z coordinates are relative to Z0.
*
* All this means that we have to set Z0 (with !ZO, that's zet-oh) such that
* it is between the maximum pen down and pen up. Since pen up is also defined
* via the maximum pen down, we have to read all coordinates before we can
* calculate Z0 and then emit the correct positioning commands. Yuck.
*
* We choose Z0 = maximum pen down.
*/
/*
* @@@ speed selection anomaly:
*
* Seems that !ZZ movements of the MDX-15 only use !VZ and VS is completely
* ignored.
*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#define V_MAX 15.0
struct segment {
double x, y, z;
struct segment *next;
};
struct path {
struct segment *segments;
struct path *next;
};
static struct path *paths = NULL, *curr_path;
static struct segment **next_seg;
static double z_max = -1e6; /* -1 km :) */
#define units(mm) ((int) round((mm)*40.0))
#define alloc_type(t) \
({ t *alloc_tmp = malloc(sizeof(t)); \
if (!alloc_tmp) \
abort(); \
alloc_tmp; })
static void new_path(void)
{
struct path *new;
new = alloc_type(struct path);
if (paths)
curr_path->next = new;
else
paths = new;
curr_path = new;
new->segments = NULL;
new->next = NULL;
next_seg = &new->segments;
}
static void process_file(FILE *file)
{
int lineno = 0;
char buf[1024];
double x, y, z;
int n;
struct segment *seg;
new_path();
while (fgets(buf, sizeof(buf), file)) {
lineno++;
if (*buf == '!' || *buf == '#')
continue;
n = sscanf(buf, "%lf %lf %lf\n", &x, &y, &z);
switch (n) {
case -1:
if (curr_path->segments)
new_path();
continue;
case 2:
z = 0;
/* fall through */
case 3:
break;
default:
fprintf(stderr, "invalid data at line %d\n", lineno);
exit(1);
}
seg = alloc_type(struct segment);
seg->x = x;
seg->y = y;
seg->z = z;
seg->next = NULL;
*next_seg = seg;
next_seg = &seg->next;
if (z_max < z)
z_max = z;
}
}
static void output_paths(double z_clear, double xy_speed, double z_speed)
{
const struct path *path;
const struct segment *seg;
double x = 0, y = 0, z = 0;
double d, s = 0, t = 0;
double txy, tz;
printf("IN;!MC1;PA\n");
printf("!ZO%d;!PZ0,%d;PU\n", units(z_max), units(z_clear));
printf("VS%f;!VZ%f\n", xy_speed, z_speed);
for (path = paths; path; path = path->next) {
seg = path->segments;
if (!seg)
continue;
printf("!PZ%d,%d;PA%d,%d;PD\n",
units(seg->z-z_max), units(z_clear),
units(seg->x), units(seg->y));
d = hypot(x-seg->x, y-seg->y)+(z-z_max);
s += d;
t += d/V_MAX;
x = seg->x;
y = seg->y;
z = seg->z;
seg = seg->next;
while (seg) {
printf("!ZZ%d,%d,%d\n", units(seg->x), units(seg->y),
units(seg->z-z_max));
d = hypot(x-seg->x, y-seg->y);
txy = d/xy_speed;
tz = fabs(z-seg->z)/z_speed;
t += txy > tz ? txy : tz;
d = hypot(d, z-seg->z);
s += d;
x = seg->x;
y = seg->y;
z = seg->z;
seg = seg->next;
}
printf("PU\n");
d = z_max+z_clear-z;
s += d;
t += d/V_MAX;
z = z_max+z_clear;
}
printf("!MC0;!ZO0;!PZ0,0;PU0,0;IN\n");
d = -z+hypot(x, y);
s += d;
t += d/V_MAX;
fprintf(stderr, "Distance %.1f mm, time %.1f s\n", s, t);
}
static void usage(const char *name)
{
fprintf(stderr,
"usage: %s z_clear[mm] xy_speed z_speed [file]\n\n"
" z_clear clearance above the highest peak, in mm (must be > 0)\n"
" Unit (\"mm\") can optionally be specified\n"
" xy_speed cutting speed, in mm/s\n"
" z_speed vertical speed when lowering the pen, in mm/s\n"
, name);
exit(1);
}
int main(int argc, const char **argv)
{
FILE *file;
char *end;
int i;
double p[3];
switch (argc) {
case 4:
file = stdin;
break;
case 5:
file = fopen(argv[4], "r");
if (!file) {
perror(argv[4]);
return 1;
}
break;
default:
usage(*argv);
}
for (i = 0; i != 3; i++) {
p[i] = strtod(argv[i+1], &end);
/*
* Allow the clearance to have a unit, for consistency in
* mkmk-simple
*/
if (!i && *end && !strcmp(end, "mm"))
continue;
if (*end || p[i] <= 0)
usage(*argv);
}
process_file(file);
output_paths(p[0], p[1], p[2]);
if (ferror(stdout))
perror("stdout");
if (fclose(stdout) == EOF)
perror("stdout");
return 0;
}