mirror of
git://projects.qi-hardware.com/cae-tools.git
synced 2024-12-23 03:22:25 +02:00
d64a34045f
Sounds weird, but the mill seems to be off by some 10%.
256 lines
5.1 KiB
C
256 lines
5.1 KiB
C
/*
|
|
* gp2rml.c - Convert from gnuplot to RML
|
|
*
|
|
* Written 2010-2013 by Werner Almesberger
|
|
* Copyright 2010-2013 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 :) */
|
|
static double z_scale = 1; /* < 1: compress; > 1: stretch */
|
|
|
|
|
|
#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);
|
|
s += d;
|
|
s += z-z_max;
|
|
t += d/V_MAX;
|
|
t += (z-z_max)/z_speed;
|
|
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)*z_scale));
|
|
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 [-s z_scale] z_clear[mm] xy_speed z_speed [file]\n\n"
|
|
" -s z_scale scale Z axis by z_scale (default: 1.0)\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)
|
|
{
|
|
const char *name;
|
|
FILE *file;
|
|
char *end;
|
|
int i;
|
|
double p[3];
|
|
|
|
name = *argv;
|
|
|
|
if (argc > 2 && !strcmp(argv[1], "-s")) {
|
|
if (argc < 3)
|
|
usage(name);
|
|
z_scale = strtod(argv[2], &end);
|
|
if (*end)
|
|
usage(name);
|
|
argc -= 2;
|
|
argv += 2;
|
|
}
|
|
|
|
switch (argc) {
|
|
case 4:
|
|
file = stdin;
|
|
break;
|
|
case 5:
|
|
file = fopen(argv[4], "r");
|
|
if (!file) {
|
|
perror(argv[4]);
|
|
return 1;
|
|
}
|
|
break;
|
|
default:
|
|
usage(name);
|
|
}
|
|
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(name);
|
|
}
|
|
|
|
process_file(file);
|
|
output_paths(p[0], p[1], p[2]);
|
|
|
|
if (ferror(stdout))
|
|
perror("stdout");
|
|
if (fclose(stdout) == EOF)
|
|
perror("stdout");
|
|
|
|
return 0;
|
|
}
|