forked from hzeller/beagleg
-
Notifications
You must be signed in to change notification settings - Fork 0
/
gcode-machine-control.c
1239 lines (1112 loc) · 47.7 KB
/
gcode-machine-control.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/* -*- mode: c; c-basic-offset: 2; indent-tabs-mode: nil; -*-
* (c) 2013, 2014 Henner Zeller <[email protected]>
*
* This file is part of BeagleG. http://github.com/hzeller/beagleg
*
* BeagleG 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 3 of the License, or
* (at your option) any later version.
*
* BeagleG is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with BeagleG. If not, see <http://www.gnu.org/licenses/>.
*/
// TODO: this is motion planner and 'other stuff printers do' in one. Separate.
// TODO: this is somewhat work-in-progress.
#include "gcode-machine-control.h"
#include <assert.h>
#include <ctype.h>
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <sys/types.h>
#include <unistd.h>
#include "motor-operations.h"
#include "gcode-parser.h"
#include "generic-gpio.h"
// In case we get a zero feedrate, send this frequency to motors instead.
#define ZERO_FEEDRATE_OVERRIDE_HZ 5
#define VERSION_STRING "PROTOCOL_VERSION:0.1 FIRMWARE_NAME:BeagleG " \
"FIRMWARE_URL:http%3A//github.com/hzeller/beagleg"
// aux_bits
#define AUX_BIT_MIST (1 << 0)
#define AUX_BIT_FLOOD (1 << 1)
#define AUX_BIT_VACUUM (1 << 2)
#define AUX_BIT_SPINDLE_ON (1 << 3)
#define AUX_BIT_SPINDLE_DIR (1 << 4)
#define MAX_AUX_PIN 4
#define NUM_ENDSTOPS 6
// Some default settings. These are most likely overrridden via flags by user.
// All these settings are in sequence of enum GCodeParserAxes: XYZEABCUVW. Axes not
// used by default are initialized to 0
static const float kMaxFeedrate[GCODE_NUM_AXES] = { 200, 200, 90, 10, 1 };
static const float kDefaultAccel[GCODE_NUM_AXES]= { 4000, 4000, 1000, 10000, 1 };
static const float kStepsPerMM[GCODE_NUM_AXES] = { 160, 160, 160, 40, 1 };
// Output mapping of Axis to motor connectors from left to right.
static const char kAxisMapping[] = "XYZEA";
// Default order in which axes should be homed.
static const char kHomeOrder[] = "ZXY";
// The vector is essentially a position in the GCODE_NUM_AXES dimensional
// space. An AxisTarget has a position vector, in absolute machine coordinates, and a
// speed when arriving at that position.
// The speed is generally an aimed goal; if it cannot be reached, the AxisTarget will
// be modified
struct AxisTarget {
int position_steps[GCODE_NUM_AXES]; // Absolute position at end of segment. In steps.
// Derived values
int delta_steps[GCODE_NUM_AXES]; // Difference to previous position.
enum GCodeParserAxis defining_axis; // index into defining axis.
float speed; // (desired) speed in steps/s on defining axis.
unsigned int aux_bits; // Auxiliarry bits in this segment; set with M42
};
// TargetBuffer implementation
struct TargetBuffer {
unsigned write_pos;
unsigned read_pos;
struct AxisTarget ring_buffer[4];
};
// Initialize a new buffer.
static void target_buffer_init(struct TargetBuffer *b) {
bzero(b, sizeof(*b));
}
// Returns number of available read items.
static int buffer_available(struct TargetBuffer *b) {
return (b->write_pos + 4 - b->read_pos) % 4;
}
// Add a new target and return pointer. Makes sure that we never override
static struct AxisTarget *buffer_add_next_target(struct TargetBuffer *b) {
assert(buffer_available(b) < 3);
struct AxisTarget *result = &b->ring_buffer[b->write_pos];
b->write_pos = (b->write_pos + 1) % 4;
return result;
}
// Get the last item written.
static struct AxisTarget *buffer_get_last_written(struct TargetBuffer *b) {
assert(buffer_available(b) > 0);
return &b->ring_buffer[(b->write_pos + 3) % 4];
}
// Get the given target, delta 0 up to buffer_peek_available();
static struct AxisTarget *buffer_at(struct TargetBuffer *b, int delta) {
assert(buffer_available(b) > delta);
return &b->ring_buffer[(b->read_pos + delta) % 4];
}
// Move on to the next read position.
static void buffer_next(struct TargetBuffer *b) {
assert(buffer_available(b) > 0);
b->read_pos = (b->read_pos + 1) % 4;
}
typedef uint8_t DriverBitmap;
// Compact representation of an enstop configuration.
struct EndstopConfig {
unsigned char trigger_value : 1; // 0: trigged low 1: triggered high.
unsigned char homing_use : 1; // 0: no 1: yes.
unsigned char endstop_number : 6; // 0: no mapping; or (1..NUM_ENDSTOPS+1)
};
// The three levels of homing confidence. If we ever switch off
// power to the motors after homing, we can't be sure.
enum HomingState {
HOMING_STATE_NEVER_HOMED,
HOMING_STATE_HOMED_BUT_MOTORS_UNPOWERED,
HOMING_STATE_HOMED,
};
struct GCodeMachineControl {
struct GCodeParserCb event_input;
struct MotorOperations *motor_ops;
const struct MachineControlConfig cfg;
// Derived configuration
float g0_feedrate_mm_per_sec; // Highest of all axes; used for G0
// (will be trimmed if needed)
// Pre-calculated per axis limits in steps, steps/s, steps/s^2
// All arrays are indexed by axis.
float max_axis_speed[GCODE_NUM_AXES]; // max travel speed hz
float max_axis_accel[GCODE_NUM_AXES]; // acceleration hz/s
float highest_accel; // hightest accel of all axes.
// "axis_to_driver": Which axis is mapped to which physical output drivers.
// This allows to have a logical axis (e.g. X, Y, Z) output to any physical
// or a set of multiple drivers (mirroring).
// Bitmap of drivers output should go.
DriverBitmap axis_to_driver[GCODE_NUM_AXES];
int axis_flip[GCODE_NUM_AXES]; // 1 or -1 for direction flip of axis
int driver_flip[BEAGLEG_NUM_MOTORS]; // 1 or -1 for for individual driver
// Mapping of Axis to which endstop it affects.
struct EndstopConfig min_endstop[GCODE_NUM_AXES];
struct EndstopConfig max_endstop[GCODE_NUM_AXES];
// Current machine configuration
float coordinate_display_origin[GCODE_NUM_AXES]; // parser tells us
float current_feedrate_mm_per_sec; // Set via Fxxx and remembered
float prog_speed_factor; // Speed factor set by program (M220)
unsigned int aux_bits; // Set via M42.
unsigned int spindle_rpm; // Set via Sxxx of M3/M4 and remembered
// Next buffered positions. Written by incoming gcode, read by outgoing
// motor movements.
struct TargetBuffer buffer;
enum HomingState homing_state;
FILE *msg_stream;
};
static uint32_t get_endstop_gpio_descriptor(struct EndstopConfig config);
static inline int round2int(float x) { return (int) roundf(x); }
static void bring_path_to_halt(GCodeMachineControl_t *state);
// machine-printf. Only prints if there is a msg-stream.
static void mprintf(GCodeMachineControl_t *state, const char *format, ...) {
va_list ap;
va_start(ap, format);
if (state->msg_stream) vfprintf(state->msg_stream, format, ap);
va_end(ap);
}
// Dummy implementations of callbacks not yet handled.
static void dummy_set_temperature(void *userdata, float f) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
mprintf(state, "// BeagleG: set_temperature(%.1f) not implemented.\n", f);
}
static void dummy_set_fanspeed(void *userdata, float speed) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
mprintf(state, "// BeagleG: set_fanspeed(%.0f) not implemented.\n", speed);
}
static void dummy_wait_temperature(void *userdata) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
mprintf(state, "// BeagleG: wait_temperature() not implemented.\n");
}
static void motors_enable(void *userdata, char b) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
bring_path_to_halt(state);
state->motor_ops->motor_enable(state->motor_ops->user_data, b);
if (state->homing_state == HOMING_STATE_HOMED) {
state->homing_state = HOMING_STATE_HOMED_BUT_MOTORS_UNPOWERED;
}
}
static void gcode_send_ok(void *userdata, char l, float v) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
mprintf(state, "ok\n");
}
static void inform_origin_offset(void *userdata, const float *origin) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
memcpy(state->coordinate_display_origin, origin,
sizeof(state->coordinate_display_origin));
}
static const char *special_commands(void *userdata, char letter, float value,
const char *remaining) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
const int code = (int)value;
if (letter == 'M') {
int pin = -1;
int aux_bit = -1;
switch (code) {
case 0: set_gpio(ESTOP_SW_GPIO); break;
case 3:
case 4:
for (;;) {
const char* after_pair = gcodep_parse_pair(remaining, &letter, &value,
state->msg_stream);
if (after_pair == NULL) break;
else if (letter == 'S') state->spindle_rpm = round2int(value);
else break;
remaining = after_pair;
}
if (state->spindle_rpm) {
state->aux_bits |= AUX_BIT_SPINDLE_ON;
if (code == 3) state->aux_bits &= ~AUX_BIT_SPINDLE_DIR;
else state->aux_bits |= AUX_BIT_SPINDLE_DIR;
}
break;
case 5: state->aux_bits &= ~(AUX_BIT_SPINDLE_ON | AUX_BIT_SPINDLE_DIR); break;
case 7: state->aux_bits |= AUX_BIT_MIST; break;
case 8: state->aux_bits |= AUX_BIT_FLOOD; break;
case 9: state->aux_bits &= ~(AUX_BIT_MIST | AUX_BIT_FLOOD); break;
case 10: state->aux_bits |= AUX_BIT_VACUUM; break;
case 11: state->aux_bits &= ~AUX_BIT_VACUUM; break;
case 42:
case 62:
case 63:
case 64:
case 65:
for (;;) {
const char* after_pair = gcodep_parse_pair(remaining, &letter, &value,
state->msg_stream);
if (after_pair == NULL) break;
if (letter == 'P') pin = round2int(value);
else if (letter == 'S' && code == 42) aux_bit = round2int(value);
else break;
remaining = after_pair;
}
if (code == 62 || code == 64)
aux_bit = 1;
else if (code == 63 || code == 65)
aux_bit = 0;
if (pin >= 0 && pin <= MAX_AUX_PIN) {
if (aux_bit >= 0 && aux_bit <= 1) {
if (aux_bit) state->aux_bits |= 1 << pin;
else state->aux_bits &= ~(1 << pin);
} else if (code == 42 && state->msg_stream) { // Just read operation.
mprintf(state, "%d\n", (state->aux_bits >> pin) & 1);
}
}
break;
case 80: set_gpio(MACHINE_PWR_GPIO); break;
case 81: clr_gpio(MACHINE_PWR_GPIO); break;
case 105: mprintf(state, "T-300\n"); break; // no temp yet.
case 114:
if (buffer_available(&state->buffer)) {
struct AxisTarget *current = buffer_at(&state->buffer, 0);
const int *mpos = current->position_steps;
const float x = 1.0f * mpos[AXIS_X] / state->cfg.steps_per_mm[AXIS_X];
const float y = 1.0f * mpos[AXIS_Y] / state->cfg.steps_per_mm[AXIS_Y];
const float z = 1.0f * mpos[AXIS_Z] / state->cfg.steps_per_mm[AXIS_Z];
const float e = 1.0f * mpos[AXIS_E] / state->cfg.steps_per_mm[AXIS_E];
const float *origin = state->coordinate_display_origin;
mprintf(state, "X:%.3f Y:%.3f Z:%.3f E:%.3f",
x - origin[AXIS_X], y - origin[AXIS_Y], z - origin[AXIS_Z],
e - origin[AXIS_E]);
mprintf(state, " [ABS. MACHINE CUBE X:%.3f Y:%.3f Z:%.3f]", x, y, z);
switch (state->homing_state) {
case HOMING_STATE_NEVER_HOMED:
mprintf(state, " (Unsure: machine never homed!)\n");
break;
case HOMING_STATE_HOMED_BUT_MOTORS_UNPOWERED:
mprintf(state, " (Lower confidence: motor power off at "
"least once after homing)\n");
break;
case HOMING_STATE_HOMED:
mprintf(state, " (confident: machine was homed)\n");
break;
}
} else {
mprintf(state, "// no current pos\n");
}
break;
case 115: mprintf(state, "%s\n", VERSION_STRING); break;
case 117:
mprintf(state, "// Msg: %s\n", remaining); // TODO: different output ?
remaining = NULL; // consume the full line.
break;
case 119: {
char any_enstops_found = 0;
for (int axis = 0; axis < GCODE_NUM_AXES; ++axis) {
struct EndstopConfig config = state->min_endstop[axis];
if (config.endstop_number) {
int value = get_gpio(get_endstop_gpio_descriptor(config));
mprintf(state, "%c_min:%s ",
tolower(gcodep_axis2letter(axis)),
value == config.trigger_value ? "TRIGGERED" : "open");
any_enstops_found = 1;
}
config = state->max_endstop[axis];
if (config.endstop_number) {
int value = get_gpio(get_endstop_gpio_descriptor(config));
mprintf(state, "%c_max:%s ",
tolower(gcodep_axis2letter(axis)),
value == config.trigger_value ? "TRIGGERED" : "open");
any_enstops_found = 1;
}
}
if (any_enstops_found) {
mprintf(state, "\n");
} else {
mprintf(state, "// This machine has no endstops configured.\n");
}
}
break;
case 999: clr_gpio(ESTOP_SW_GPIO); break;
default:
mprintf(state, "// BeagleG: didn't understand ('%c', %d, '%s')\n",
letter, code, remaining);
remaining = NULL; // In this case, let's just discard remainig block.
break;
}
}
return remaining;
}
static void finish_machine_control(void *userdata) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
bring_path_to_halt(state);
}
static float euclid_distance(float x, float y, float z) {
return sqrtf(x*x + y*y + z*z);
}
// Number of steps to accelerate or decelerate (negative "a") from speed
// v0 to speed v1. Modifies v1 if we can't reach the speed with the allocated
// number of steps.
static float steps_for_speed_change(float a, float v0, float *v1, int max_steps) {
// s = v0 * t + a/2 * t^2
// v1 = v0 + a*t
const float t = (*v1 - v0) / a;
// TODO:
if (t < 0) fprintf(stderr, "Error condition: t=%.1f INSUFFICIENT LOOKAHEAD\n", t);
float steps = a/2 * t*t + v0 * t;
if (steps <= max_steps) return steps;
// Ok, we would need more steps than we have available. We correct the speed to what
// we actually can reach.
*v1 = sqrtf(v0*v0 + 2 * a * max_steps);
return max_steps;
}
static float acceleration_for_move(const GCodeMachineControl_t *state,
const int *axis_steps,
enum GCodeParserAxis defining_axis) {
return state->max_axis_accel[defining_axis];
// TODO: we need to scale the acceleration if one of the other axes could't
// deal with it. Look at axis steps for that.
}
// Given that we want to travel "s" steps, start with speed "v0",
// accelerate peak speed v1 and slow down to "v2" with acceleration "a",
// what is v1 ?
static float get_peak_speed(float s, float v0, float v2, float a) {
return sqrtf(v2*v2 + v0*v0 + 2 * a * s) / sqrtf(2);
}
// Speed relative to defining axis
static float get_speed_factor_for_axis(const struct AxisTarget *t,
enum GCodeParserAxis request_axis) {
if (t->delta_steps[t->defining_axis] == 0) return 0.0f;
return 1.0f * t->delta_steps[request_axis] / t->delta_steps[t->defining_axis];
}
// Get the speed for a particular axis. Depending on the direction, this can
// be positive or negative.
static float get_speed_for_axis(const struct AxisTarget *target,
enum GCodeParserAxis request_axis) {
return target->speed * get_speed_factor_for_axis(target, request_axis);
}
static char within_acceptable_range(float new, float old, float fraction) {
const float max_diff = fraction * old;
if (new < old - max_diff) return 0;
if (new > old + max_diff) return 0;
return 1;
}
// Determine the fraction of the speed that "from" should decelerate
// to at the end of its travel.
// The way trapezoidal moves work, be still have to decelerate to zero in
// most times, which is inconvenient. TODO(hzeller): speed matching is not
// cutting it :)
static float determine_joining_speed(const struct AxisTarget *from,
const struct AxisTarget *to) {
// Our goal is to figure out what our from defining speed should
// be at the end of the move.
char is_first = 1;
float from_defining_speed = from->speed;
for (int axis = 0; axis < GCODE_NUM_AXES; ++axis) {
const int from_delta = from->delta_steps[axis];
const int to_delta = to->delta_steps[axis];
// Quick integer decisions
if (from_delta == 0 && to_delta == 0) continue; // uninteresting: no move.
if (from_delta == 0 || to_delta == 0) return 0.0f; // accel from/to zero
if ((from_delta < 0 && to_delta > 0) || (from_delta > 0 && to_delta < 0))
return 0.0f; // turing around
float to_speed = get_speed_for_axis(to, axis);
// What would this speed translated to our defining axis be ?
float speed_conversion = 1.0f * from->delta_steps[from->defining_axis] / from->delta_steps[axis];
float goal = to_speed * speed_conversion;
if (goal < 0.0f) return 0.0f;
if (is_first || within_acceptable_range(goal, from_defining_speed, 1e-5)) {
if (goal < from_defining_speed) from_defining_speed = goal;
is_first = 0;
} else {
return 0.0f; // Too far off.
}
}
return from_defining_speed;
}
// Assign steps to all the motors responsible for given axis.
static void assign_steps_to_motors(GCodeMachineControl_t *state,
struct MotorMovement *command,
enum GCodeParserAxis axis,
int steps) {
const DriverBitmap motormap_for_axis = state->axis_to_driver[axis];
for (int motor = 0; motor < BEAGLEG_NUM_MOTORS; ++motor) {
if (motormap_for_axis & (1 << motor)) {
command->steps[motor] =
state->axis_flip[axis] * state->driver_flip[motor] * steps;
}
}
}
// Returns true, if all results in zero movement
static uint8_t substract_steps(struct MotorMovement *value,
const struct MotorMovement *substract) {
uint8_t has_nonzero = 0;
for (int i = 0; i < BEAGLEG_NUM_MOTORS; ++i) {
value->steps[i] -= substract->steps[i];
has_nonzero |= (value->steps[i] != 0);
}
return has_nonzero;
}
// Move the given number of machine steps for each axis.
//
// This will be up to three segments: accelerating from last_pos speed to
// target speed, regular travel, and decelerating to the speed that the
// next segment is never forced to decelerate, but stays at speed or accelerate.
//
// The segments are sent to the motor operations backend.
//
// Since we calculate the deceleration, this modifies the speed of target_pos
// to reflect what the last speed was at the end of the move.
static void move_machine_steps(GCodeMachineControl_t *state,
const struct AxisTarget *last_pos,
struct AxisTarget *target_pos,
const struct AxisTarget *upcoming) {
if (target_pos->delta_steps[target_pos->defining_axis] == 0) {
return;
}
struct MotorMovement accel_command = {0};
struct MotorMovement move_command = {0};
struct MotorMovement decel_command = {0};
assert(target_pos->speed > 0); // Speed is always a positive scalar.
// Aux bits are set synchronously with what we need.
move_command.aux_bits = target_pos->aux_bits;
const enum GCodeParserAxis defining_axis = target_pos->defining_axis;
// Common settings.
memcpy(&accel_command, &move_command, sizeof(accel_command));
memcpy(&decel_command, &move_command, sizeof(decel_command));
move_command.v0 = target_pos->speed;
move_command.v1 = target_pos->speed;
// Let's see what our defining axis had as speed in the previous segment. The
// last segment might have had a different defining axis, so we calculate
// what the the fraction of the speed that our _current_ defining axis had.
const float last_speed = fabsf(get_speed_for_axis(last_pos, defining_axis));
// We need to arrive at a speed that the upcoming move does not have
// to decelerate further (after all, it has a fixed feed-rate it should not
// go over).
float next_speed = determine_joining_speed(target_pos, upcoming);
const int *axis_steps = target_pos->delta_steps; // shortcut.
const int abs_defining_axis_steps = abs(axis_steps[defining_axis]);
const float a = acceleration_for_move(state, axis_steps, defining_axis);
const float peak_speed = get_peak_speed(abs_defining_axis_steps,
last_speed, next_speed, a);
assert(peak_speed > 0);
// TODO: if we only have < 5 steps or so, we should not even consider
// accelerating or decelerating, but just do one speed.
if (peak_speed < target_pos->speed) {
target_pos->speed = peak_speed; // Don't manage to accelerate to desired v
}
const float accel_fraction =
(last_speed < target_pos->speed)
? steps_for_speed_change(a, last_speed, &target_pos->speed,
abs_defining_axis_steps) / abs_defining_axis_steps
: 0;
// We only decelerate if the upcoming speed is _slower_
float dummy_next_speed = next_speed; // Don't care to modify; we don't have
const float decel_fraction =
(next_speed < target_pos->speed)
? steps_for_speed_change(-a, target_pos->speed, &dummy_next_speed,
abs_defining_axis_steps) / abs_defining_axis_steps
: 0;
assert(accel_fraction + decel_fraction <= 1.0 + 1e-4);
#if 1
// fudging: if we have tiny acceleration segments, don't do these at all
// but only do speed; otherwise we have a lot of rattling due to many little
// segments of acceleration/deceleration (e.g. for G2/G3).
// This is not optimal. Ideally, we would actually calculate in terms of
// jerk and optimize to stay within that constraint.
const int accel_decel_steps
= (accel_fraction + decel_fraction) * abs_defining_axis_steps;
const float accel_decel_mm
= (accel_decel_steps / state->cfg.steps_per_mm[defining_axis]);
const char do_accel = (accel_decel_mm > 2 || accel_decel_steps > 16);
#else
const char do_accel = 1;
#endif
char has_accel = 0;
char has_move = 0;
char has_decel = 0;
if (do_accel && accel_fraction * abs_defining_axis_steps > 0) {
has_accel = 1;
accel_command.v0 = last_speed; // Last speed of defining axis
accel_command.v1 = target_pos->speed; // New speed of defining axis
// Now map axis steps to actual motor driver
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
const int accel_steps = round2int(accel_fraction * axis_steps[i]);
assign_steps_to_motors(state, &accel_command, i, accel_steps);
}
}
if (do_accel && decel_fraction * abs_defining_axis_steps > 0) {
has_decel = 1;
decel_command.v0 = target_pos->speed;
decel_command.v1 = next_speed;
target_pos->speed = next_speed;
// Now map axis steps to actual motor driver
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
const int decel_steps = round2int(decel_fraction * axis_steps[i]);
assign_steps_to_motors(state, &decel_command, i, decel_steps);
}
}
// Move is everything that hasn't been covered in speed changes.
// So we start with all steps and substract steps done in acceleration and
// deceleration.
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
assign_steps_to_motors(state, &move_command, i, axis_steps[i]);
}
substract_steps(&move_command, &accel_command);
has_move = substract_steps(&move_command, &decel_command);
if (state->cfg.synchronous) {
state->motor_ops->wait_queue_empty(state->motor_ops->user_data);
}
if (has_accel) state->motor_ops->enqueue(state->motor_ops->user_data,
&accel_command, state->msg_stream);
if (has_move) state->motor_ops->enqueue(state->motor_ops->user_data,
&move_command, state->msg_stream);
if (has_decel) state->motor_ops->enqueue(state->motor_ops->user_data,
&decel_command, state->msg_stream);
}
// If we have enough data in the queue, issue motor move.
static void issue_motor_move_if_possible(GCodeMachineControl_t *state) {
struct TargetBuffer *buffer = &state->buffer;
if (buffer_available(buffer) >= 3) {
move_machine_steps(state,
buffer_at(buffer, 0), // Current established position.
buffer_at(buffer, 1), // Position we want to move to.
buffer_at(buffer, 2)); // Next upcoming.
buffer_next(buffer);
}
}
static void machine_move(void *userdata, float feedrate, const float axis[]) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
struct TargetBuffer *buffer = &state->buffer;
// We always have a previous position.
struct AxisTarget *previous = buffer_get_last_written(buffer);
struct AxisTarget *new_pos = buffer_add_next_target(buffer);
int max_steps = -1;
enum GCodeParserAxis defining_axis = AXIS_X;
// Real world -> machine coordinates. Here, we are rounding to the next full
// step, but we never accumulate the error, as we always use the absolute
// position as reference.
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
new_pos->position_steps[i] = round2int(axis[i] * state->cfg.steps_per_mm[i]);
new_pos->delta_steps[i] = new_pos->position_steps[i] - previous->position_steps[i];
// The defining axis is the one that has to travel the most steps. It defines
// the frequency to go.
// All the other axes are doing a fraction of the defining axis.
if (abs(new_pos->delta_steps[i]) > max_steps) {
max_steps = abs(new_pos->delta_steps[i]);
defining_axis = (enum GCodeParserAxis) i;
}
}
new_pos->aux_bits = state->aux_bits;
new_pos->defining_axis = defining_axis;
// Now let's calculate the travel speed in steps/s on the defining axis.
if (max_steps > 0) {
float travel_speed = feedrate * state->cfg.steps_per_mm[defining_axis];
// If we're in the euclidian space, choose the step-frequency according to
// the relative feedrate of the defining axis.
// (A straight 200mm/s should be the same as a diagnoal 200mm/s)
if (defining_axis == AXIS_X || defining_axis == AXIS_Y || defining_axis == AXIS_Z) {
// We need to calculate the feedrate in real-world coordinates as each
// axis can have a different amount of steps/mm
const float total_xyz_len_mm =
euclid_distance(new_pos->delta_steps[AXIS_X] / state->cfg.steps_per_mm[AXIS_X],
new_pos->delta_steps[AXIS_Y] / state->cfg.steps_per_mm[AXIS_Y],
new_pos->delta_steps[AXIS_Z] / state->cfg.steps_per_mm[AXIS_Z]);
const float steps_per_mm = state->cfg.steps_per_mm[defining_axis];
const float defining_axis_len_mm = new_pos->delta_steps[defining_axis] / steps_per_mm;
const float euclid_fraction = fabsf(defining_axis_len_mm) / total_xyz_len_mm;
travel_speed *= euclid_fraction;
}
if (travel_speed > state->max_axis_speed[defining_axis]) {
travel_speed = state->max_axis_speed[defining_axis];
}
new_pos->speed = travel_speed;
} else {
new_pos->speed = 0;
}
issue_motor_move_if_possible(state);
}
static void bring_path_to_halt(GCodeMachineControl_t *state) {
// Enqueue a new position that is the same position as the last
// one seen, but zero speed. That will allow the previous segment to
// slow down. Enqueue.
struct AxisTarget *previous = buffer_get_last_written(&state->buffer);
struct AxisTarget *new_pos = buffer_add_next_target(&state->buffer);
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
new_pos->position_steps[i] = previous->position_steps[i];
new_pos->delta_steps[i] = 0;
}
new_pos->defining_axis = AXIS_X;
new_pos->speed = 0;
new_pos->aux_bits = state->aux_bits;
issue_motor_move_if_possible(state);
}
static char test_homing_status_ok(GCodeMachineControl_t *state) {
if (!state->cfg.require_homing)
return 1;
if (state->homing_state > HOMING_STATE_NEVER_HOMED)
return 1;
mprintf(state, "// ERROR: please home machine first (G28).\n");
return 0;
}
static char test_within_machine_limits(GCodeMachineControl_t *state,
const float *axis) {
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
// Min range ...
if (axis[i] < 0) {
// Machine cube must be in positive range.
if (state->coordinate_display_origin[i] != 0) {
mprintf(state,
"// ERROR outside machine limit: Axis %c < min allowed "
"%+.1fmm in current coordinate system. Ignoring move!\n",
gcodep_axis2letter(i), -state->coordinate_display_origin[i]);
} else {
// No relative G92 or similar set. Display in simpler form.
mprintf(state, "// ERROR outside machine limit: Axis %c < 0. "
"Ignoring move!\n", gcodep_axis2letter(i));
}
return 0;
}
// Max range ..
if (state->cfg.move_range_mm[i] <= 0)
continue; // max range not configured.
const float max_limit = state->cfg.move_range_mm[i];
if (axis[i] > max_limit) {
// Machine cube must be within machine limits if defined.
if (state->coordinate_display_origin[i] != 0) {
mprintf(state,
"// ERROR outside machine limit: Axis %c > max allowed %+.1fmm "
"in current coordinate system (=%.1fmm machine absolute). "
"Ignoring move!\n",
gcodep_axis2letter(i),
max_limit - state->coordinate_display_origin[i], max_limit);
} else {
// No relative G92 or similar set. Display in simpler form.
mprintf(state, "// ERROR outside machine limit: Axis %c > %.1fmm. "
"Ignoring move!\n", gcodep_axis2letter(i), max_limit);
}
return 0;
}
}
return 1;
}
static char machine_G1(void *userdata, float feed, const float *axis) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
if (!test_homing_status_ok(state))
return 0;
if (!test_within_machine_limits(state, axis))
return 0;
if (feed > 0) {
state->current_feedrate_mm_per_sec = state->cfg.speed_factor * feed;
}
float feedrate = state->prog_speed_factor * state->current_feedrate_mm_per_sec;
machine_move(userdata, feedrate, axis);
return 1;
}
static char machine_G0(void *userdata, float feed, const float *axis) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
if (!test_homing_status_ok(state))
return 0;
if (!test_within_machine_limits(state, axis))
return 0;
float rapid_feed = state->g0_feedrate_mm_per_sec;
const float given = state->cfg.speed_factor * state->prog_speed_factor * feed;
machine_move(userdata, given > 0 ? given : rapid_feed, axis);
return 1;
}
static void machine_dwell(void *userdata, float value) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
bring_path_to_halt(state);
state->motor_ops->wait_queue_empty(state->motor_ops->user_data);
usleep((int) (value * 1000));
}
static void gcode_input_idle(void *userdata) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
bring_path_to_halt(state);
}
static void machine_set_speed_factor(void *userdata, float value) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
if (value < 0) {
value = 1.0f + value; // M220 S-10 interpreted as: 90%
}
if (value < 0.005) {
mprintf(state, "// M220: Not accepting speed factors < 0.5%% (got %.1f%%)\n",
100.0f * value);
return;
}
state->prog_speed_factor = value;
}
static uint32_t get_endstop_gpio_descriptor(struct EndstopConfig config) {
switch (config.endstop_number) {
case 1: return END_1_GPIO;
case 2: return END_2_GPIO;
case 3: return END_3_GPIO;
case 4: return END_4_GPIO;
case 5: return END_5_GPIO;
case 6: return END_6_GPIO;
}
return 0;
}
// Get the endstop for the axis.
static uint32_t get_home_endstop(GCodeMachineControl_t *state,
enum GCodeParserAxis axis,
int *dir, int *trigger_value) {
*dir = 1;
struct EndstopConfig config = state->max_endstop[axis];
if (state->min_endstop[axis].endstop_number
&& state->min_endstop[axis].homing_use) {
*dir = -1;
config = state->min_endstop[axis];
}
if (!config.homing_use)
return 0;
*trigger_value = config.trigger_value;
return get_endstop_gpio_descriptor(config);
}
// Moves to endstop and returns how many steps it moved in the process.
static int move_to_endstop(GCodeMachineControl_t *state,
enum GCodeParserAxis axis,
float feedrate, int backoff,
int dir, int trigger_value, uint32_t gpio_def) {
int total_movement = 0;
struct MotorMovement move_command = {0};
const float steps_per_mm = state->cfg.steps_per_mm[axis];
float target_speed = feedrate * steps_per_mm;
if (target_speed > state->max_axis_speed[axis]) {
target_speed = state->max_axis_speed[axis];
}
move_command.v0 = 0;
move_command.v1 = target_speed;
// move axis until endstop is hit
int segment_move_steps = 0.5 * steps_per_mm * dir;
assign_steps_to_motors(state, &move_command, axis, segment_move_steps);
while (get_gpio(gpio_def) != trigger_value) {
state->motor_ops->enqueue(state->motor_ops->user_data,
&move_command, state->msg_stream);
state->motor_ops->wait_queue_empty(state->motor_ops->user_data);
total_movement += segment_move_steps;
// TODO: possibly acceleration over multiple segments.
move_command.v0 = move_command.v1;
}
if (backoff) {
// move axis off endstop
segment_move_steps = 0.1 * steps_per_mm * -dir;
assign_steps_to_motors(state, &move_command, axis, segment_move_steps);
while (get_gpio(gpio_def) == trigger_value) {
state->motor_ops->enqueue(state->motor_ops->user_data,
&move_command, state->msg_stream);
state->motor_ops->wait_queue_empty(state->motor_ops->user_data);
total_movement += segment_move_steps;
}
}
return total_movement;
}
static void home_axis(GCodeMachineControl_t *state, enum GCodeParserAxis axis) {
const struct MachineControlConfig *cfg = &state->cfg;
struct AxisTarget *last = buffer_get_last_written(&state->buffer);
float home_pos = 0; // assume HOME_POS_ORIGIN
int dir;
int trigger_value;
uint32_t gpio_def = get_home_endstop(state, axis, &dir, &trigger_value);
if (!gpio_def)
return;
move_to_endstop(state, axis, 15, 1, dir, trigger_value, gpio_def);
home_pos = (dir < 0) ? 0 : cfg->move_range_mm[axis];
last->position_steps[axis] = round2int(home_pos * cfg->steps_per_mm[axis]);
}
static void machine_home(void *userdata, AxisBitmap_t axes_bitmap) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
const struct MachineControlConfig *cfg = &state->cfg;
bring_path_to_halt(state);
for (const char *order = cfg->home_order; *order; order++) {
const enum GCodeParserAxis axis = gcodep_letter2axis(*order);
if (axis == GCODE_NUM_AXES || !(axes_bitmap & (1 << axis)))
continue;
home_axis(state, axis);
}
state->homing_state = HOMING_STATE_HOMED;
}
static char machine_probe(void *userdata, float feedrate,
enum GCodeParserAxis axis,
float *probe_result) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
const struct MachineControlConfig *cfg = &state->cfg;
if (!test_homing_status_ok(state))
return 0;
bring_path_to_halt(state);
int dir = 1;
// -- somewhat hackish
// We try to find the axis that is _not_ used for homing.
// this is not yet 100% the way it should be. We should actually
// define the probe-'endstops' somewhat differently.
// For now, we just do the simple thing
struct EndstopConfig config = state->max_endstop[axis];
if (state->min_endstop[axis].endstop_number
&& !state->min_endstop[axis].homing_use) {
dir = -1;
config = state->min_endstop[axis];
}
uint32_t gpio_def = get_endstop_gpio_descriptor(config);
if (!gpio_def || config.homing_use) {
// We are only looking for probes that are _not_ used for homing.
mprintf(state,
"// BeagleG: No probe - axis %c does not have a travel endstop\n",
gcodep_axis2letter(axis));
return 0;
}
struct AxisTarget *last = buffer_get_last_written(&state->buffer);
if (feedrate <= 0) feedrate = 20;
// TODO: if the probe fails to trigger, there is no mechanism to stop
// it right now...
int total_steps = move_to_endstop(state, axis, feedrate, 0, dir,
config.trigger_value, gpio_def);
last->position_steps[axis] += total_steps;
*probe_result = 1.0f * last->position_steps[axis] / cfg->steps_per_mm[axis];
return 1;
}
// Cleanup whatever is allocated. Return NULL for convenience.
static GCodeMachineControl_t *cleanup_state(GCodeMachineControl_t *object) {
free(object);
return NULL;
}
GCodeMachineControl_t *gcode_machine_control_new(const struct MachineControlConfig *config_in,
struct MotorOperations *motor_ops,
FILE *msg_stream) {
GCodeMachineControl_t *result;
// Initialize basic state and derived configuration.
result = (GCodeMachineControl_t*) malloc(sizeof(GCodeMachineControl_t));
bzero(result, sizeof(*result));
result->motor_ops = motor_ops;
result->msg_stream = msg_stream;
// Always keep the steps_per_mm positive, but extract direction for
// final assignment to motor.
struct MachineControlConfig cfg = *config_in;
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
result->axis_flip[i] = cfg.steps_per_mm[i] < 0 ? -1 : 1;
cfg.steps_per_mm[i] = fabsf(cfg.steps_per_mm[i]);
if (cfg.max_feedrate[i] < 0) {
fprintf(stderr, "Invalid negative feedrate %.1f for axis %c\n",
cfg.max_feedrate[i], gcodep_axis2letter(i));
return cleanup_state(result);
}
if (cfg.acceleration[i] < 0) {
fprintf(stderr, "Invalid negative acceleration %.1f for axis %c\n",
cfg.acceleration[i], gcodep_axis2letter(i));
return cleanup_state(result);
}
}
// Here we assign it to the 'const' cfg, all other accesses will check for
// the readonly ness. So some nasty override here: we know what we're doing.
*((struct MachineControlConfig*) &result->cfg) = cfg;
result->current_feedrate_mm_per_sec = cfg.max_feedrate[AXIS_X] / 10;
float lowest_accel = cfg.max_feedrate[AXIS_X] * cfg.steps_per_mm[AXIS_X];
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
if (cfg.max_feedrate[i] > result->g0_feedrate_mm_per_sec) {