mirror of
git://projects.qi-hardware.com/cae-tools.git
synced 2024-12-23 09:55:32 +02:00
239 lines
3.7 KiB
C
239 lines
3.7 KiB
C
/*
|
|
* cngt.c - Tool change utility for MDX-15/20
|
|
*
|
|
* Written 2010-2011 by Werner Almesberger
|
|
* Copyright 2010-2011 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.
|
|
*/
|
|
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <ctype.h>
|
|
|
|
#include "serial.h"
|
|
#include "getkey.h"
|
|
|
|
|
|
#define MAX_POS 10
|
|
#define STEP_XY 5 /* xy movement step, 5 mm */
|
|
#define STEP_FINE_XY 0.5 /* xy movement fine step, 0.5 mm */
|
|
#define STEP_Z 1 /* z movement step, 1 mm */
|
|
#define STEP_FINE_Z 0.1 /* z movement step, 0.1 mm */
|
|
|
|
|
|
static double pos_x[MAX_POS];
|
|
static double pos_y[MAX_POS];
|
|
static double z0, height;
|
|
static double cx, cy, cz;
|
|
static int positioning = 0;
|
|
|
|
|
|
#define UNITS(mm) ((mm)*40.0)
|
|
|
|
|
|
static void move(void)
|
|
{
|
|
serial_printf("!PZ%.1f,0;PD%.1f,%.1f\n",
|
|
UNITS(cz), UNITS(cx), UNITS(cy));
|
|
}
|
|
|
|
|
|
static void up(void)
|
|
{
|
|
cz = z0+height;
|
|
move();
|
|
}
|
|
|
|
|
|
static void down(void)
|
|
{
|
|
cz = z0;
|
|
move();
|
|
}
|
|
|
|
|
|
static int do_key(char c)
|
|
{
|
|
double step_xy = STEP_FINE_XY;
|
|
double step_z = STEP_FINE_Z;
|
|
|
|
if (isupper(c)) {
|
|
step_xy = STEP_XY;
|
|
step_z = STEP_Z;
|
|
}
|
|
|
|
/* Anything but x/y positioning */
|
|
|
|
switch (c) {
|
|
case 'U':
|
|
case 'u':
|
|
if (positioning) {
|
|
cz += step_z;
|
|
move();
|
|
} else {
|
|
up();
|
|
}
|
|
return 0;
|
|
case 'D':
|
|
case 'd':
|
|
if (positioning) {
|
|
cz -= step_z;
|
|
move();
|
|
} else {
|
|
down();
|
|
}
|
|
return 0;
|
|
case 'q':
|
|
return 1;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
/* Only x/y positioning */
|
|
|
|
if (!positioning)
|
|
up();
|
|
|
|
switch (c) {
|
|
case 'H':
|
|
case 'h':
|
|
cx -= step_xy;
|
|
break;
|
|
case 'J':
|
|
case 'j':
|
|
cy -= step_xy;
|
|
break;
|
|
case 'K':
|
|
case 'k':
|
|
cy += step_xy;
|
|
break;
|
|
case 'L':
|
|
case 'l':
|
|
cx += step_xy;
|
|
break;
|
|
default:
|
|
if (positioning)
|
|
return 0;
|
|
if (c < '0' || c > '9')
|
|
return 0;
|
|
cx = pos_x[c-'0'];
|
|
cy = pos_y[c-'0'];
|
|
}
|
|
|
|
move();
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static void gp_minmax(const char *name,
|
|
double *xa, double *ya, double *xb, double *yb)
|
|
{
|
|
FILE *file;
|
|
char buf[1024];
|
|
double x, y, z;
|
|
int n;
|
|
int first = 1;
|
|
|
|
file = fopen(name, "r");
|
|
if (!file) {
|
|
perror(name);
|
|
exit(1);
|
|
}
|
|
while (fgets(buf, sizeof(buf), file)) {
|
|
if (*buf == '#')
|
|
continue;
|
|
n = sscanf(buf, "%lf %lf %lf\n", &x, &y, &z);
|
|
switch (n) {
|
|
case 3:
|
|
/* fall through */
|
|
case 2:
|
|
if (first || x < *xa)
|
|
*xa = x;
|
|
if (first || x > *xb)
|
|
*xb = x;
|
|
if (first || y < *ya)
|
|
*ya = y;
|
|
if (first || y > *yb)
|
|
*yb = y;
|
|
first = 0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
fclose(file);
|
|
}
|
|
|
|
|
|
static void usage(const char *name)
|
|
{
|
|
fprintf(stderr,
|
|
"usage: %s z0 height [file | x y ...]\n"
|
|
" %s z0\n"
|
|
, name, name);
|
|
exit(1);
|
|
}
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
double xa = 0, ya = 0, xb = 0, yb = 0;
|
|
int i;
|
|
char c;
|
|
|
|
if (argc < 2)
|
|
usage(*argv);
|
|
|
|
z0 = atof(argv[1]);
|
|
|
|
if (argc == 2) {
|
|
positioning = 1;
|
|
height = 0;
|
|
} else {
|
|
height = atof(argv[2]);
|
|
if (argc & 1) {
|
|
for (i = 3; i != argc; i += 2) {
|
|
pos_x[(i-3)/2] = atof(argv[i]);
|
|
pos_y[(i-3)/2] = atof(argv[i+1]);
|
|
}
|
|
} else {
|
|
if (argc != 4)
|
|
usage(*argv);
|
|
gp_minmax(argv[3], &xa, &ya, &xb, &yb);
|
|
pos_x[1] = pos_x[4] = pos_x[7] = xa;
|
|
pos_x[2] = pos_x[5] = pos_x[8] = (xa+xb)/2;
|
|
pos_x[3] = pos_x[6] = pos_x[9] = xb;
|
|
pos_y[1] = pos_y[2] = pos_y[3] = ya;
|
|
pos_y[4] = pos_y[5] = pos_y[6] = (ya+yb)/2;
|
|
pos_y[7] = pos_y[8] = pos_y[9] = yb;
|
|
}
|
|
}
|
|
|
|
serial_open("/dev/ttyUSB0");
|
|
serial_printf("\nIN;!MC0\n");
|
|
|
|
cx = pos_x[0];
|
|
cy = pos_y[0];
|
|
cz = z0+height;
|
|
move();
|
|
|
|
while ((c = getkey())) {
|
|
if (do_key(c))
|
|
break;
|
|
if (positioning)
|
|
printf("%f %f %f\r\n", cx, cy, cz);
|
|
}
|
|
|
|
if (!positioning)
|
|
up();
|
|
|
|
serial_close();
|
|
return 0;
|
|
}
|