|
@@ -18,7 +18,7 @@ type Word struct {
|
18
|
18
|
}
|
19
|
19
|
|
20
|
20
|
type GCodeFile struct {
|
21
|
|
- Blocks map[int]*Block
|
|
21
|
+ Blocks []*Block
|
22
|
22
|
}
|
23
|
23
|
|
24
|
24
|
type Block struct {
|
|
@@ -95,96 +95,110 @@ func main() {
|
95
|
95
|
mode := ""
|
96
|
96
|
var x, y float64
|
97
|
97
|
var lastPoints []point
|
98
|
|
- arcs := []*arc{}
|
|
98
|
+
|
99
|
99
|
scale := ScaleMM
|
100
|
100
|
|
101
|
|
- for lineNumber := 0; lineNumber < len(gc.Blocks); lineNumber++ {
|
102
|
|
- block := gc.Blocks[lineNumber]
|
103
|
|
- xyChanged, zChanged, modeChanged := false, false, false
|
104
|
|
- for _, cmd := range block.Words {
|
105
|
|
- oldMode := mode
|
106
|
|
- switch cmd.Address {
|
107
|
|
- case "G":
|
108
|
|
- switch cmd.Number {
|
109
|
|
- case 0:
|
110
|
|
- mode = "g0"
|
111
|
|
- case 1:
|
112
|
|
- mode = "g1"
|
113
|
|
- case 2:
|
114
|
|
- mode = "g2"
|
115
|
|
- case 3:
|
116
|
|
- mode = "g3"
|
117
|
|
- case 20:
|
118
|
|
- scale = ScaleInch
|
119
|
|
- case 21:
|
120
|
|
- scale = ScaleMM
|
|
101
|
+ done := false
|
|
102
|
+ startBlock := 0
|
|
103
|
+
|
|
104
|
+ for !done {
|
|
105
|
+ lastPoints = nil
|
|
106
|
+ for lineNumber := startBlock; lineNumber < len(gc.Blocks); lineNumber++ {
|
|
107
|
+ block := gc.Blocks[lineNumber]
|
|
108
|
+ xyChanged, zChanged, modeChanged := false, false, false
|
|
109
|
+ for _, cmd := range block.Words {
|
|
110
|
+ oldMode := mode
|
|
111
|
+ switch cmd.Address {
|
|
112
|
+ case "G":
|
|
113
|
+ switch cmd.Number {
|
|
114
|
+ case 0:
|
|
115
|
+ mode = "g0"
|
|
116
|
+ case 1:
|
|
117
|
+ mode = "g1"
|
|
118
|
+ case 2:
|
|
119
|
+ mode = "g2"
|
|
120
|
+ case 3:
|
|
121
|
+ mode = "g3"
|
|
122
|
+ case 20:
|
|
123
|
+ scale = ScaleInch
|
|
124
|
+ case 21:
|
|
125
|
+ scale = ScaleMM
|
|
126
|
+ }
|
|
127
|
+ case "X":
|
|
128
|
+ x = cmd.Number
|
|
129
|
+ xyChanged = true
|
|
130
|
+ case "Y":
|
|
131
|
+ y = cmd.Number
|
|
132
|
+ xyChanged = true
|
|
133
|
+ case "Z":
|
|
134
|
+ //z = cmd.Number
|
|
135
|
+ zChanged = true
|
|
136
|
+ }
|
|
137
|
+ if mode != oldMode {
|
|
138
|
+ modeChanged = true
|
121
|
139
|
}
|
122
|
|
- case "X":
|
123
|
|
- x = cmd.Number
|
124
|
|
- xyChanged = true
|
125
|
|
- case "Y":
|
126
|
|
- y = cmd.Number
|
127
|
|
- xyChanged = true
|
128
|
|
- case "Z":
|
129
|
|
- //z = cmd.Number
|
130
|
|
- zChanged = true
|
131
|
|
- }
|
132
|
|
- if mode != oldMode {
|
133
|
|
- modeChanged = true
|
134
|
140
|
}
|
135
|
|
- }
|
136
|
141
|
|
137
|
|
- newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}
|
|
142
|
+ newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}
|
138
|
143
|
|
139
|
|
- if zChanged || modeChanged {
|
140
|
|
- if len(lastPoints) > 0 {
|
141
|
|
- arcs = append(arcs, findArcs(lastPoints, scale)...)
|
142
|
|
- lastPoints = make([]point, 0)
|
|
144
|
+ if zChanged || modeChanged {
|
|
145
|
+ if len(lastPoints) > 0 {
|
|
146
|
+ break
|
|
147
|
+ }
|
|
148
|
+ } else if mode == "g1" && xyChanged {
|
|
149
|
+ lastPoints = append(lastPoints, newPt)
|
143
|
150
|
}
|
144
|
|
- } else if mode == "g1" && xyChanged {
|
145
|
|
- lastPoints = append(lastPoints, newPt)
|
|
151
|
+
|
146
|
152
|
}
|
147
|
153
|
|
148
|
|
- }
|
|
154
|
+ if len(lastPoints) == 0 {
|
|
155
|
+ done = true
|
|
156
|
+ break
|
|
157
|
+ }
|
149
|
158
|
|
150
|
|
- if len(lastPoints) > 0 {
|
151
|
|
- arcs = append(arcs, findArcs(lastPoints, scale)...)
|
152
|
|
- }
|
|
159
|
+ arc := findArc(lastPoints, scale)
|
153
|
160
|
|
154
|
|
- for _, arc := range arcs {
|
|
161
|
+ if arc == nil {
|
|
162
|
+ if len(gc.Blocks) == startBlock+len(lastPoints) {
|
|
163
|
+ break
|
|
164
|
+ }
|
|
165
|
+ continue
|
|
166
|
+ }
|
155
|
167
|
|
156
|
168
|
i := (arc.centre.x - arc.startPt.x)
|
157
|
169
|
j := (arc.centre.y - arc.startPt.y)
|
158
|
170
|
|
159
|
|
- // clear out old commands
|
160
|
|
- for i := arc.startBlock; i <= arc.endBlock; i++ {
|
161
|
|
- delete(gc.Blocks, i)
|
162
|
|
- }
|
|
171
|
+ spliced := gc.Blocks[:arc.startBlock]
|
163
|
172
|
|
164
|
173
|
// G1 to move to the beginning point of the arc
|
165
|
|
- gc.Blocks[arc.startBlock] = &Block{Words: []Word{
|
|
174
|
+ spliced = append(spliced, &Block{Words: []Word{
|
166
|
175
|
{Address: "G", Number: 1},
|
167
|
176
|
{Address: "X", Number: arc.startPt.x},
|
168
|
177
|
{Address: "Y", Number: arc.startPt.y},
|
169
|
|
- }}
|
|
178
|
+ }})
|
170
|
179
|
|
171
|
180
|
// G2/G3 to do the arc
|
172
|
181
|
dir := 3.0
|
173
|
182
|
if arc.clockwise {
|
174
|
183
|
dir = 2.0
|
175
|
184
|
}
|
176
|
|
- gc.Blocks[arc.startBlock+1] = &Block{Words: []Word{
|
|
185
|
+ spliced = append(spliced, &Block{Words: []Word{
|
177
|
186
|
{Address: "G", Number: dir},
|
178
|
187
|
{Address: "I", Number: i},
|
179
|
188
|
{Address: "J", Number: j},
|
180
|
189
|
{Address: "X", Number: arc.endPt.x},
|
181
|
190
|
{Address: "Y", Number: arc.endPt.y},
|
182
|
|
- }}
|
|
191
|
+ }})
|
183
|
192
|
|
184
|
193
|
// shove in a G1 for the surviving X and Y commands after the arc
|
185
|
|
- gc.Blocks[arc.startBlock+2] = &Block{Words: []Word{
|
|
194
|
+ spliced = append(spliced, &Block{Words: []Word{
|
186
|
195
|
{Address: "G", Number: 1},
|
187
|
|
- }}
|
|
196
|
+ }})
|
|
197
|
+ fmt.Println("len(gc.Blocks) =", len(gc.Blocks))
|
|
198
|
+ fmt.Println("arc.startBlock =", arc.startBlock)
|
|
199
|
+ fmt.Println("arc.endBlock =", arc.endBlock)
|
|
200
|
+ gc.Blocks = append(spliced, gc.Blocks[arc.endBlock+1:]...)
|
|
201
|
+ startBlock = len(spliced) + 1
|
188
|
202
|
}
|
189
|
203
|
|
190
|
204
|
gc.Write(os.Stdout)
|
|
@@ -200,7 +214,7 @@ func ReadGCodeFile(filename string) (*GCodeFile, error) {
|
200
|
214
|
|
201
|
215
|
r := regexp.MustCompile(`([A-Z])(\-?[0-9]+\.?[0-9]*)`)
|
202
|
216
|
|
203
|
|
- lines := make(map[int]*Block)
|
|
217
|
+ blocks := make([]*Block, 0)
|
204
|
218
|
scanner := bufio.NewScanner(file)
|
205
|
219
|
lineNumber := 0
|
206
|
220
|
for scanner.Scan() {
|
|
@@ -215,17 +229,17 @@ func ReadGCodeFile(filename string) (*GCodeFile, error) {
|
215
|
229
|
}
|
216
|
230
|
l.Words = append(l.Words, Word{Address: address, Number: number})
|
217
|
231
|
}
|
218
|
|
- lines[lineNumber] = l
|
|
232
|
+ blocks = append(blocks, l)
|
219
|
233
|
lineNumber++
|
220
|
234
|
|
221
|
235
|
}
|
222
|
236
|
|
223
|
|
- log.Printf("Read %d lines.", len(lines))
|
|
237
|
+ log.Printf("Read %d lines.", len(blocks))
|
224
|
238
|
if err := scanner.Err(); err != nil {
|
225
|
239
|
return nil, err
|
226
|
240
|
}
|
227
|
241
|
|
228
|
|
- return &GCodeFile{Blocks: lines}, nil
|
|
242
|
+ return &GCodeFile{Blocks: blocks}, nil
|
229
|
243
|
}
|
230
|
244
|
|
231
|
245
|
func (g *GCodeFile) Write(w io.Writer) error {
|
|
@@ -283,13 +297,13 @@ type arcMidpoint struct {
|
283
|
297
|
// then the arc is grown from the midpoint until it becomes too un-arc-like to
|
284
|
298
|
// be considered an arc.
|
285
|
299
|
|
286
|
|
-func findArcs(points []point, scale float64) []*arc {
|
|
300
|
+func findArc(points []point, scale float64) *arc {
|
287
|
301
|
|
288
|
302
|
numTests := 4 // how many circles to test
|
289
|
303
|
thresh := 5.0 // 5.0 is definitely not an arc. This value was determined empirically because I'm a slob
|
290
|
304
|
growThresh := 0.01 // while in an arc, anything that causes the sd to increase above growThresh terminates the arc
|
291
|
|
- growThresh = 0.05
|
292
|
|
- arcs := []*arc{}
|
|
305
|
+ // growThresh = 0.01
|
|
306
|
+ // arcs := []*arc{}
|
293
|
307
|
|
294
|
308
|
// first find things that aren't an arc
|
295
|
309
|
inArc := false
|
|
@@ -308,10 +322,16 @@ func findArcs(points []point, scale float64) []*arc {
|
308
|
322
|
if inArc && (maxDev >= thresh || i == numTestPoints-1) {
|
309
|
323
|
fmt.Println("=====")
|
310
|
324
|
inArc = false
|
|
325
|
+ //arcStart-=2*numTests
|
|
326
|
+ arcStart += 6
|
311
|
327
|
mid := arcStart + ((i - arcStart) / 2)
|
312
|
328
|
|
313
|
|
- midpoint := mid + (3*numTests)/2 // hand tuned compensation for algorithm lag
|
314
|
|
- width := (i - arcStart) + (5 * numTests) // hand tuned compensation for algorithm sensitivity
|
|
329
|
+ if mid < 0 {
|
|
330
|
+ continue
|
|
331
|
+ }
|
|
332
|
+
|
|
333
|
+ midpoint := mid //+ (3*numTests)/2 // hand tuned compensation for algorithm lag
|
|
334
|
+ width := (i - arcStart) + (4 * numTests) // hand tuned compensation for algorithm sensitivity
|
315
|
335
|
|
316
|
336
|
bestStart, bestEnd, bestCenter, bestRadius, bestSD := growArc(points, midpoint, width, growThresh)
|
317
|
337
|
fmt.Printf("TEST %d 0 %d %d %0.8f\n", points[midpoint].lineNumber, bestStart, bestEnd, bestSD)
|
|
@@ -344,7 +364,7 @@ func findArcs(points []point, scale float64) []*arc {
|
344
|
364
|
clockwise: clockwise,
|
345
|
365
|
scale: ScaleMM,
|
346
|
366
|
}).scaleTo(scale)
|
347
|
|
- arcs = append(arcs, arc)
|
|
367
|
+ return arc
|
348
|
368
|
|
349
|
369
|
} else if !inArc && maxDev < thresh {
|
350
|
370
|
inArc = true
|
|
@@ -353,7 +373,7 @@ func findArcs(points []point, scale float64) []*arc {
|
353
|
373
|
|
354
|
374
|
}
|
355
|
375
|
|
356
|
|
- return arcs
|
|
376
|
+ return nil
|
357
|
377
|
}
|
358
|
378
|
|
359
|
379
|
func growArc(points []point, midpoint int, width int, thresh float64) (int, int, point, float64, float64) {
|