Root/cngt/cngt.c

1/*
2 * cngt.c - Tool change utility for MDX-15/20
3 *
4 * Written 2010-2011 by Werner Almesberger
5 * Copyright 2010-2011 Werner Almesberger
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 */
12
13
14#include <stdlib.h>
15#include <stdio.h>
16#include <ctype.h>
17
18#include "serial.h"
19#include "getkey.h"
20
21
22#define MAX_POS 10
23#define STEP_XY 5 /* xy movement step, 5 mm */
24#define STEP_FINE_XY 0.5 /* xy movement fine step, 0.5 mm */
25#define STEP_Z 1 /* z movement step, 1 mm */
26#define STEP_FINE_Z 0.1 /* z movement step, 0.1 mm */
27
28
29static double pos_x[MAX_POS];
30static double pos_y[MAX_POS];
31static double z0, height;
32static double cx, cy, cz;
33static int positioning = 0;
34
35
36#define UNITS(mm) ((mm)*40.0)
37
38
39static void move(void)
40{
41    serial_printf("!PZ%.1f,0;PD%.1f,%.1f\n",
42        UNITS(cz), UNITS(cx), UNITS(cy));
43}
44
45
46static void up(void)
47{
48    cz = z0+height;
49    move();
50}
51
52
53static void down(void)
54{
55    cz = z0;
56    move();
57}
58
59
60static int do_key(char c)
61{
62    double step_xy = STEP_FINE_XY;
63    double step_z = STEP_FINE_Z;
64
65    if (isupper(c)) {
66        step_xy = STEP_XY;
67        step_z = STEP_Z;
68    }
69
70    /* Anything but x/y positioning */
71
72    switch (c) {
73    case 'U':
74    case 'u':
75        if (positioning) {
76            cz += step_z;
77            move();
78        } else {
79            up();
80        }
81        return 0;
82    case 'D':
83    case 'd':
84        if (positioning) {
85            cz -= step_z;
86            move();
87        } else {
88            down();
89        }
90        return 0;
91    case 'q':
92        return 1;
93    default:
94        break;
95    }
96
97    /* Only x/y positioning */
98
99    if (!positioning)
100        up();
101
102    switch (c) {
103    case 'H':
104    case 'h':
105        cx -= step_xy;
106        break;
107    case 'J':
108    case 'j':
109        cy -= step_xy;
110        break;
111    case 'K':
112    case 'k':
113        cy += step_xy;
114        break;
115    case 'L':
116    case 'l':
117        cx += step_xy;
118        break;
119    default:
120        if (positioning)
121            return 0;
122        if (c < '0' || c > '9')
123            return 0;
124        cx = pos_x[c-'0'];
125        cy = pos_y[c-'0'];
126    }
127
128    move();
129
130    return 0;
131}
132
133
134static void gp_minmax(const char *name,
135    double *xa, double *ya, double *xb, double *yb)
136{
137    FILE *file;
138    char buf[1024];
139    double x, y, z;
140    int n;
141    int first = 1;
142
143    file = fopen(name, "r");
144    if (!file) {
145        perror(name);
146        exit(1);
147    }
148    while (fgets(buf, sizeof(buf), file)) {
149        if (*buf == '#')
150            continue;
151        n = sscanf(buf, "%lf %lf %lf\n", &x, &y, &z);
152        switch (n) {
153        case 3:
154            /* fall through */
155        case 2:
156            if (first || x < *xa)
157                *xa = x;
158            if (first || x > *xb)
159                *xb = x;
160            if (first || y < *ya)
161                *ya = y;
162            if (first || y > *yb)
163                *yb = y;
164            first = 0;
165            break;
166        default:
167            break;
168        }
169    }
170    fclose(file);
171}
172
173
174static void usage(const char *name)
175{
176    fprintf(stderr,
177"usage: %s z0 height [file | x y ...]\n"
178" %s z0\n"
179    , name, name);
180    exit(1);
181}
182
183
184int main(int argc, char **argv)
185{
186    double xa = 0, ya = 0, xb = 0, yb = 0;
187    int i;
188    char c;
189
190    if (argc < 2)
191        usage(*argv);
192
193    z0 = atof(argv[1]);
194
195    if (argc == 2) {
196        positioning = 1;
197        height = 0;
198    } else {
199        height = atof(argv[2]);
200        if (argc & 1) {
201            for (i = 3; i != argc; i += 2) {
202                pos_x[(i-3)/2] = atof(argv[i]);
203                pos_y[(i-3)/2] = atof(argv[i+1]);
204            }
205        } else {
206            if (argc != 4)
207                usage(*argv);
208            gp_minmax(argv[3], &xa, &ya, &xb, &yb);
209            pos_x[1] = pos_x[4] = pos_x[7] = xa;
210            pos_x[2] = pos_x[5] = pos_x[8] = (xa+xb)/2;
211            pos_x[3] = pos_x[6] = pos_x[9] = xb;
212            pos_y[1] = pos_y[2] = pos_y[3] = ya;
213            pos_y[4] = pos_y[5] = pos_y[6] = (ya+yb)/2;
214            pos_y[7] = pos_y[8] = pos_y[9] = yb;
215        }
216    }
217
218    serial_open("/dev/ttyUSB0");
219    serial_printf("\nIN;!MC0\n");
220
221    cx = pos_x[0];
222    cy = pos_y[0];
223    cz = z0+height;
224    move();
225
226    while ((c = getkey())) {
227        if (do_key(c))
228            break;
229        if (positioning)
230            printf("%f %f %f\r\n", cx, cy, cz);
231    }
232
233    if (!positioning)
234        up();
235
236    serial_close();
237    return 0;
238}
239

Archive Download this file

Branches:
master



interactive