4. System and programming
1. System overview
This mobile 3D printer robot interprets G-code and navigates autonomously while printing. A Raspberry Pi Pico microcontroller directly drives the motor controllers, and X/Y limit switches are used for zeroing.
System diagram
Triage
After triage on week 15, I decided not to use firmware. Basic G-code functionality can be implemented directly without it, and mecanum wheel control needs to be handled externally regardless. I also decided to give up adding input devices for absolute or global position tracking and orientation detection.
The input devices for this machine have the following requirements:
- G-code reading
- End stop (Zeroing)
- Hot end temperature management
- Obstacle detection
- Understanding accumulated travel distance
- Understanding self-position and orientation (to compensate for deviations caused by wheel slippage etc.)
I am considering placing end stops (bumpers) on all four sides as basic sensors, and also finding a way to detect and correct the robot's own position. Also, I need to consider how this machine can actually move without interfering with printed model (especially on the Z axis).
Options for position and orientation detection
For detecting and correcting position, so far, I am considering the following input devices:
- Rotary encorders
- IMU (Inertial Measurement Unit)
- Optical flow sensors
- ArUco markers
I briefly asked ChatGPT to compare these options.
Prompt: Compare the following input devices for the project:
Rotary Encoder, IMU, Optical Flow Sensor, ArUco Marker
| Feature |
Rotary Encoders |
IMU (Inertial Measurement Unit) |
Optical Flow Sensors |
ArUco Markers |
| How It Works |
Measures wheel rotation using an optical or magnetic sensor. Converts rotation into position or speed data. |
Uses accelerometers and gyroscopes to measure acceleration and angular velocity. Data is integrated over time to estimate movement. |
Captures images of the surface and tracks movement by comparing pixel shifts. Works similarly to an optical mouse. |
Uses a camera to detect square fiducial markers in the environment and calculates position based on their known size and position. |
| Measures |
Wheel rotation (position, speed) |
Rotation, acceleration |
Surface movement (X, Y) |
Global position (X, Y) |
| Accuracy |
High (precise for wheels) |
Medium (drifts over time) |
High on textured surfaces |
Very High (camera-dependent) |
| Drift |
No drift |
Yes (needs correction) |
Some drift (depends on surface) |
No drift |
| Response Time |
Fast (real-time) |
Fast (but noisy) |
Fast |
Slower (depends on camera) |
| Environment Sensitivity |
Works in any condition |
Affected by vibrations |
Fails on uniform surfaces (glass, smooth floors) |
Needs good lighting & clear markers |
| Cost |
Cheap (~$2-$10) |
Affordable (~$5-$15) |
Medium (~$10-$30) |
Camera + markers (~$30-$100) |
| Setup Complexity |
Easy |
Easy (with filtering) |
Medium (needs surface tuning) |
Hard (camera calibration) |
| Use Case |
Precise wheel movement |
Balance, rotation tracking |
Non-wheel-based tracking |
Absolute positioning |
For now, I've decided not to use input devices for position detection in my first spiral.
3. Arduino embeded programming
*Codes in this page are assisted by ChatGPT. In this case, the step-by-step process of adding features worked for me. For a detailed process of writing these codes, see Week 14.
A. Basic stepper motor control without libraries
- 1/16 microstep
- Move all motors simultaneously: move forward
Arduino code
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 | // RF motor (Wheel 1)
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
// LF motor (Wheel 2)
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
// RB motor (Wheel 3)
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
// LB motor (Wheel 4)
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping control pins (shared)
const int EN_PIN = 9;
const int MS1 = 6;
const int MS2 = 7;
const int MS3 = 8;
const int STEP_DELAY_US = 1600; // Step delay in microseconds (adjust for speed)
void setup() {
// Set all step and direction pins as outputs
pinMode(STEP_PIN_RF, OUTPUT);
pinMode(DIR_PIN_RF, OUTPUT);
pinMode(STEP_PIN_LF, OUTPUT);
pinMode(DIR_PIN_LF, OUTPUT);
pinMode(STEP_PIN_RB, OUTPUT);
pinMode(DIR_PIN_RB, OUTPUT);
pinMode(STEP_PIN_LB, OUTPUT);
pinMode(DIR_PIN_LB, OUTPUT);
// Set microstepping control and enable pins as outputs
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT);
pinMode(MS2, OUTPUT);
pinMode(MS3, OUTPUT);
// Enable all drivers
digitalWrite(EN_PIN, LOW); // LOW to enable most drivers
// Set microstepping mode to 1/16 (MS1=1, MS2=1, MS3=1)
digitalWrite(MS1, HIGH);
digitalWrite(MS2, HIGH);
digitalWrite(MS3, HIGH);
// Set directions (can be individually changed)
digitalWrite(DIR_PIN_RF, HIGH);
digitalWrite(DIR_PIN_LF, HIGH);
digitalWrite(DIR_PIN_RB, HIGH);
digitalWrite(DIR_PIN_LB, HIGH);
}
void loop() {
// Perform a single step on all motors
digitalWrite(STEP_PIN_RF, HIGH);
digitalWrite(STEP_PIN_LF, HIGH);
digitalWrite(STEP_PIN_RB, HIGH);
digitalWrite(STEP_PIN_LB, HIGH);
delayMicroseconds(STEP_DELAY_US);
digitalWrite(STEP_PIN_RF, LOW);
digitalWrite(STEP_PIN_LF, LOW);
digitalWrite(STEP_PIN_RB, LOW);
digitalWrite(STEP_PIN_LB, LOW);
delayMicroseconds(STEP_DELAY_US);
// Loop will continue indefinitely
}
|
B. Mecanum wheel control via serial
- Direction control of mecanum wheel
| void setDirection(int x, int y, int rot) {
int dir_RF = y - x - rot;
int dir_LF = y + x + rot;
int dir_RB = y + x - rot;
int dir_LB = y - x + rot;
digitalWrite(DIR_PIN_RF, dir_RF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LF, dir_LF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_RB, dir_RB >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LB, dir_LB >= 0 ? HIGH : LOW);
}
|
- Mecanum wheel kinematics
- When moving forward (
+y), all wheels = HIGH
- When strafing right (
+x):
- Front Left, Rear Right =
LOW
- Front Right, Rear Left =
HIGH
- When rotating clockwise (
`+rot):
- Left wheels =
HIGH
- Right wheels =
LOW
- Combining above in RF motor example:
int dir_RF = y - x - rot;
+y adds forward contribution-x subtracts due to strafing (RF opposes the sideways motion)-rot subtracts due to rotation (RF rotates backward to help rotate CW)
- Setting motor direction in RF motor example:
digitalWrite(DIR_PIN_RF, dir_RF >= 0 ? HIGH : LOW);
- Set the final rotation direction
DIR_PIN_RF based on the value of dir_RF
- If
dir_RF is positive or zero, set direction HIGH (e.g., clockwise).
- If
dir_RF is negative, set direction LOW (e.g., counterclockwise).
-
Map single character serial inputs to robot movement directions:
1
2
3
4
5
6
7
8
9
10
11
12
13 | void moveCommand(char cmd) {
switch (cmd) {
case 'F': Serial.println("Forward"); setDirection(0, 1, 0); break;
case 'B': Serial.println("Backward"); setDirection(0, -1, 0); break;
case 'L': Serial.println("Strafe Left"); setDirection(-1, 0, 0); break;
case 'R': Serial.println("Strafe Right"); setDirection(1, 0, 0); break;
case 'C': Serial.println("Rotate CW"); setDirection(0, 0, 1); break;
case 'A': Serial.println("Rotate CCW"); setDirection(0, 0, -1); break;
case 'S': Serial.println("Stop"); return; // no step
default: Serial.println("Unknown command"); return;
}
stepAll(NUM_STEPS);
}
|
| Command |
Meaning |
Direction Vector (x, y, rot) |
'F' |
Move Forward |
(0, 1, 0) – No strafe, forward, no spin |
'B' |
Move Backward |
(0, -1, 0) – No strafe, backward |
'L' |
Strafe Left |
(-1, 0, 0) – Strafe left, no forward |
'R' |
Strafe Right |
(1, 0, 0) – Strafe right |
'C' |
Rotate Clockwise |
(0, 0, 1) – Only spin CW |
'A' |
Rotate Counter-CW |
(0, 0, -1) – Only spin CCW |
'S' |
Stop |
Does nothing, exits early |
| default |
Unknown command |
Prints warning and exits |
Arduino code
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 | // Motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping and enable
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
const int STEP_DELAY_US = 1600;
const int NUM_STEPS = 500;
void setup() {
Serial.begin(115200);
pinMode(STEP_PIN_RF, OUTPUT); pinMode(DIR_PIN_RF, OUTPUT);
pinMode(STEP_PIN_LF, OUTPUT); pinMode(DIR_PIN_LF, OUTPUT);
pinMode(STEP_PIN_RB, OUTPUT); pinMode(DIR_PIN_RB, OUTPUT);
pinMode(STEP_PIN_LB, OUTPUT); pinMode(DIR_PIN_LB, OUTPUT);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
digitalWrite(EN_PIN, LOW); // Enable motor drivers
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH); // 1/16 microstepping
Serial.println("Ready. Send command: F/B/L/R/C/A/S");
}
void setDirection(int x, int y, int rot) {
int dir_RF = y - x - rot;
int dir_LF = y + x + rot;
int dir_RB = y + x - rot;
int dir_LB = y - x + rot;
digitalWrite(DIR_PIN_RF, dir_RF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LF, dir_LF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_RB, dir_RB >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LB, dir_LB >= 0 ? HIGH : LOW);
}
void stepAll(int steps) {
for (int i = 0; i < steps; i++) {
digitalWrite(STEP_PIN_RF, HIGH);
digitalWrite(STEP_PIN_LF, HIGH);
digitalWrite(STEP_PIN_RB, HIGH);
digitalWrite(STEP_PIN_LB, HIGH);
delayMicroseconds(STEP_DELAY_US);
digitalWrite(STEP_PIN_RF, LOW);
digitalWrite(STEP_PIN_LF, LOW);
digitalWrite(STEP_PIN_RB, LOW);
digitalWrite(STEP_PIN_LB, LOW);
delayMicroseconds(STEP_DELAY_US);
}
}
void moveCommand(char cmd) {
switch (cmd) {
case 'F': Serial.println("Forward"); setDirection(0, 1, 0); break;
case 'B': Serial.println("Backward"); setDirection(0, -1, 0); break;
case 'L': Serial.println("Strafe Left"); setDirection(-1, 0, 0); break;
case 'R': Serial.println("Strafe Right"); setDirection(1, 0, 0); break;
case 'C': Serial.println("Rotate CW"); setDirection(0, 0, 1); break;
case 'A': Serial.println("Rotate CCW"); setDirection(0, 0, -1); break;
case 'S': Serial.println("Stop"); return; // no step
default: Serial.println("Unknown command"); return;
}
stepAll(NUM_STEPS);
}
void loop() {
if (Serial.available()) {
char command = Serial.read();
moveCommand(command);
}
}
|
C. Mecanum wheel control with G-code
Parses G-code:
- Waits for a full line of text commands like:
G1 X100 Y0 R-30
- Checks if the command starts with
G1, which means "move."
- If it doesn't, the command is ignored.
- Finds the letters X, Y, and R in the command.
- If it finds
X, it reads the number after it (e.g., 100).
- Same for
Y and R (e.g., 0 and -30).
These numbers represent movement amounts:
- X = strafe (left/right)
- Y = forward/backward
- R = rotate
- Simplifies the values to just direction:
- If X is positive > move right (
1)
- If X is negative > move left (
-1)
- If X is 0 > don’t strafe (
0)
(Same for Y and R)
- Sends the direction values to the motors
- Figures out how many steps to move
- It uses the largest value from X, Y, or R
- Then converts that into a number of motor steps
- It moves all wheels together for that number of steps
Arduino code
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 | const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
const int STEP_DELAY_US = 1200; // Step delay for speed
const int MAX_STEPS = 500; // Max steps per command
String input = "";
void setup() {
Serial.begin(115200);
pinMode(STEP_PIN_RF, OUTPUT); pinMode(DIR_PIN_RF, OUTPUT);
pinMode(STEP_PIN_LF, OUTPUT); pinMode(DIR_PIN_LF, OUTPUT);
pinMode(STEP_PIN_RB, OUTPUT); pinMode(DIR_PIN_RB, OUTPUT);
pinMode(STEP_PIN_LB, OUTPUT); pinMode(DIR_PIN_LB, OUTPUT);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
digitalWrite(EN_PIN, LOW);
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);
Serial.println("Ready for G-code commands like: G1 X100 Y0 R0");
}
void setDirection(int x, int y, int rot) {
int dir_RF = y - x - rot;
int dir_LF = y + x + rot;
int dir_RB = y + x - rot;
int dir_LB = y - x + rot;
digitalWrite(DIR_PIN_RF, dir_RF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LF, dir_LF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_RB, dir_RB >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LB, dir_LB >= 0 ? HIGH : LOW);
}
void stepAll(int steps) {
for (int i = 0; i < steps; i++) {
digitalWrite(STEP_PIN_RF, HIGH);
digitalWrite(STEP_PIN_LF, HIGH);
digitalWrite(STEP_PIN_RB, HIGH);
digitalWrite(STEP_PIN_LB, HIGH);
delayMicroseconds(STEP_DELAY_US);
digitalWrite(STEP_PIN_RF, LOW);
digitalWrite(STEP_PIN_LF, LOW);
digitalWrite(STEP_PIN_RB, LOW);
digitalWrite(STEP_PIN_LB, LOW);
delayMicroseconds(STEP_DELAY_US);
}
}
void parseAndExecute(String cmd) {
cmd.trim();
cmd.toUpperCase();
if (!cmd.startsWith("G1")) {
Serial.println("Invalid or unsupported command.");
return;
}
int x = 0, y = 0, r = 0;
int xIndex = cmd.indexOf('X');
if (xIndex != -1) x = cmd.substring(xIndex + 1).toInt();
int yIndex = cmd.indexOf('Y');
if (yIndex != -1) y = cmd.substring(yIndex + 1).toInt();
int rIndex = cmd.indexOf('R');
if (rIndex != -1) r = cmd.substring(rIndex + 1).toInt();
Serial.print("Parsed -> X: "); Serial.print(x);
Serial.print(" Y: "); Serial.print(y);
Serial.print(" R: "); Serial.println(r);
// Normalize input range to direction only
int dirX = (x > 0) ? 1 : (x < 0) ? -1 : 0;
int dirY = (y > 0) ? 1 : (y < 0) ? -1 : 0;
int dirR = (r > 0) ? 1 : (r < 0) ? -1 : 0;
setDirection(dirX, dirY, dirR);
int stepCount = max(abs(x), max(abs(y), abs(r)));
stepCount = map(stepCount, 0, 100, 0, MAX_STEPS); // Cap to max steps
stepAll(stepCount);
}
void loop() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (input.length() > 0) {
parseAndExecute(input);
input = "";
}
} else {
input += c;
}
}
}
|
D. Servo motor for Z move
- Accept a
Z value from G-code to control a servo motor.
- Move each wheel with different numbers of steps for more precise mecanum motion.
- Replace the old
stepAll() with stepMecanum() for better control.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 | #include <Servo.h>
Servo servoZ; // Create servo object
const int SERVO_PIN_Z = 22; // Servo control pin
int currentZ = 0; // Current angle of servo
void setup() {
servoZ.attach(SERVO_PIN_Z, 500, 2400); // Attach servo with custom pulse range
servoZ.write(currentZ); // Move servo to initial position (0°)
}
void parseAndExecute(String cmd) {
int z = -1;
int zIndex = cmd.indexOf('Z');
if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();
if (z != -1) {
z = constrain(z, 0, 180); // Limit value to valid range
servoZ.write(z); // Move servo to Z degrees
currentZ = z; // Update current position
Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
}
}
|
Arduino code
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 | #include <Servo.h>
// Stepper motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping control
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
// Servo Z-axis
Servo servoZ;
const int SERVO_PIN_Z = 22;
int currentZ = 0;
const int STEP_DELAY_US = 1200;
const int MAX_STEPS = 500;
String input = "";
void setup() {
Serial.begin(115200);
pinMode(STEP_PIN_RF, OUTPUT); pinMode(DIR_PIN_RF, OUTPUT);
pinMode(STEP_PIN_LF, OUTPUT); pinMode(DIR_PIN_LF, OUTPUT);
pinMode(STEP_PIN_RB, OUTPUT); pinMode(DIR_PIN_RB, OUTPUT);
pinMode(STEP_PIN_LB, OUTPUT); pinMode(DIR_PIN_LB, OUTPUT);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
digitalWrite(EN_PIN, LOW);
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);
servoZ.attach(SERVO_PIN_Z, 500, 2400); // Custom pulse range
servoZ.write(currentZ); // Initial position
Serial.println("Ready for G-code like G1 X100 Y0 Z90");
}
void setDirection(int x, int y, int rot) {
int dir_RF = y - x - rot;
int dir_LF = y + x + rot;
int dir_RB = y + x - rot;
int dir_LB = y - x + rot;
digitalWrite(DIR_PIN_RF, dir_RF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LF, dir_LF >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_RB, dir_RB >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LB, dir_LB >= 0 ? HIGH : LOW);
}
void stepAll(int steps) {
for (int i = 0; i < steps; i++) {
digitalWrite(STEP_PIN_RF, HIGH);
digitalWrite(STEP_PIN_LF, HIGH);
digitalWrite(STEP_PIN_RB, HIGH);
digitalWrite(STEP_PIN_LB, HIGH);
delayMicroseconds(STEP_DELAY_US);
digitalWrite(STEP_PIN_RF, LOW);
digitalWrite(STEP_PIN_LF, LOW);
digitalWrite(STEP_PIN_RB, LOW);
digitalWrite(STEP_PIN_LB, LOW);
delayMicroseconds(STEP_DELAY_US);
}
}
void parseAndExecute(String cmd) {
cmd.trim();
cmd.toUpperCase();
if (!cmd.startsWith("G1")) {
Serial.println("Invalid or unsupported command.");
return;
}
int x = 0, y = 0, r = 0, z = -1;
int xIndex = cmd.indexOf('X');
if (xIndex != -1) x = cmd.substring(xIndex + 1).toInt();
int yIndex = cmd.indexOf('Y');
if (yIndex != -1) y = cmd.substring(yIndex + 1).toInt();
int rIndex = cmd.indexOf('R');
if (rIndex != -1) r = cmd.substring(rIndex + 1).toInt();
int zIndex = cmd.indexOf('Z');
if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();
Serial.print("Parsed -> X: "); Serial.print(x);
Serial.print(" Y: "); Serial.print(y);
Serial.print(" R: "); Serial.print(r);
if (z != -1) {
Serial.print(" Z: "); Serial.print(z);
}
Serial.println();
if (z != -1) {
z = constrain(z, 0, 180);
servoZ.write(z);
currentZ = z;
Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
}
// Mecanum wheel step calculation
int rfSteps = y - x - r;
int lfSteps = y + x + r;
int rbSteps = y + x - r;
int lbSteps = y - x + r;
stepMecanum(rfSteps, lfSteps, rbSteps, lbSteps);
}
void stepMecanum(int rf, int lf, int rb, int lb) {
int abs_rf = abs(rf);
int abs_lf = abs(lf);
int abs_rb = abs(rb);
int abs_lb = abs(lb);
// Max steps to take
int maxSteps = max(max(abs_rf, abs_lf), max(abs_rb, abs_lb));
if (maxSteps == 0) return;
// Set directions
digitalWrite(DIR_PIN_RF, rf >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LF, lf >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_RB, rb >= 0 ? HIGH : LOW);
digitalWrite(DIR_PIN_LB, lb >= 0 ? HIGH : LOW);
// Track errors for each motor
float rfErr = 0, lfErr = 0, rbErr = 0, lbErr = 0;
for (int i = 0; i < maxSteps; i++) {
rfErr += (float)abs_rf / maxSteps;
lfErr += (float)abs_lf / maxSteps;
rbErr += (float)abs_rb / maxSteps;
lbErr += (float)abs_lb / maxSteps;
if (rfErr >= 1.0) {
digitalWrite(STEP_PIN_RF, HIGH); rfErr -= 1.0;
}
if (lfErr >= 1.0) {
digitalWrite(STEP_PIN_LF, HIGH); lfErr -= 1.0;
}
if (rbErr >= 1.0) {
digitalWrite(STEP_PIN_RB, HIGH); rbErr -= 1.0;
}
if (lbErr >= 1.0) {
digitalWrite(STEP_PIN_LB, HIGH); lbErr -= 1.0;
}
delayMicroseconds(STEP_DELAY_US);
digitalWrite(STEP_PIN_RF, LOW);
digitalWrite(STEP_PIN_LF, LOW);
digitalWrite(STEP_PIN_RB, LOW);
digitalWrite(STEP_PIN_LB, LOW);
delayMicroseconds(STEP_DELAY_US);
}
}
void loop() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (input.length() > 0) {
parseAndExecute(input);
input = "";
}
} else {
input += c;
}
}
}
|
E. With AccelStepper library
AccelStepper is a library that helps control stepper motors more easily and smoothly using motor drivers like A4988.
- Smooth acceleration and deceleration
- Non-blocking motor control (runs in
loop())
- Support for multiple motors
- To make this code compatible with a motor driver like the A4988, create AccelStepper object with DRIVER interface:
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
- Go to Tools > Manage Libraries > Search for “AccelStepper” > Install
#include <AccelStepper.h>
Key lines:
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 | #include <AccelStepper.h>
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int MAX_SPEED = 800;
const int ACCEL = 600;
// Create RF motor stepper object
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
void setup() {
Serial.begin(115200);
// Set max speed and acceleration for RF motor
stepperRF.setMaxSpeed(MAX_SPEED);
stepperRF.setAcceleration(ACCEL);
}
void loop() {
int steps = 100; // Example steps to move
stepperRF.move(steps); // Queue steps
// Run motor until target is reached
while (stepperRF.isRunning()) {
stepperRF.run();
}
delay(1000); // Pause before next move
}
|
Arduino code
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 | #include <AccelStepper.h>
#include <Servo.h>
// Stepper motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping control
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
// Servo Z-axis
Servo servoZ;
const int SERVO_PIN_Z = 22;
int currentZ = 0;
const int MAX_SPEED = 800;
const int ACCEL = 600;
String input = "";
// Define AccelStepper objects (interface type 1 = driver)
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
AccelStepper stepperLF(AccelStepper::DRIVER, STEP_PIN_LF, DIR_PIN_LF);
AccelStepper stepperRB(AccelStepper::DRIVER, STEP_PIN_RB, DIR_PIN_RB);
AccelStepper stepperLB(AccelStepper::DRIVER, STEP_PIN_LB, DIR_PIN_LB);
void setup() {
Serial.begin(115200);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
digitalWrite(EN_PIN, LOW);
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);
// Setup AccelStepper parameters
stepperRF.setMaxSpeed(MAX_SPEED); stepperRF.setAcceleration(ACCEL);
stepperLF.setMaxSpeed(MAX_SPEED); stepperLF.setAcceleration(ACCEL);
stepperRB.setMaxSpeed(MAX_SPEED); stepperRB.setAcceleration(ACCEL);
stepperLB.setMaxSpeed(MAX_SPEED); stepperLB.setAcceleration(ACCEL);
servoZ.attach(SERVO_PIN_Z, 500, 2400);
servoZ.write(currentZ);
Serial.println("Ready for G-code like G1 X100 Y0 Z90");
}
void parseAndExecute(String cmd) {
cmd.trim();
cmd.toUpperCase();
if (!cmd.startsWith("G1")) {
Serial.println("Invalid or unsupported command.");
return;
}
int x = 0, y = 0, r = 0, z = -1;
int xIndex = cmd.indexOf('X');
if (xIndex != -1) x = cmd.substring(xIndex + 1).toInt();
int yIndex = cmd.indexOf('Y');
if (yIndex != -1) y = cmd.substring(yIndex + 1).toInt();
int rIndex = cmd.indexOf('R');
if (rIndex != -1) r = cmd.substring(rIndex + 1).toInt();
int zIndex = cmd.indexOf('Z');
if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();
Serial.print("Parsed -> X: "); Serial.print(x);
Serial.print(" Y: "); Serial.print(y);
Serial.print(" R: "); Serial.print(r);
if (z != -1) {
Serial.print(" Z: "); Serial.print(z);
}
Serial.println();
if (z != -1) {
z = constrain(z, 0, 180);
servoZ.write(z);
currentZ = z;
Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
}
// Mecanum step calculations
int rfSteps = y - x - r;
int lfSteps = y + x + r;
int rbSteps = y + x - r;
int lbSteps = y - x + r;
stepperRF.move(rfSteps);
stepperLF.move(lfSteps);
stepperRB.move(rbSteps);
stepperLB.move(lbSteps);
while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
stepperRF.run();
stepperLF.run();
stepperRB.run();
stepperLB.run();
}
}
void loop() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (input.length() > 0) {
parseAndExecute(input);
input = "";
}
} else {
input += c;
}
}
}
|
F. Implementation of limit switches and zeroing
-
Homing the X/Y axis
- Moves the robot in the X or Y axis direction.
- It continues until the LIMIT_X or Y switch is triggered (goes LOW).
- Once triggered, all X or Y-related stepper motors stop.
- setCurrentPosition(0) is called to set the current X position to zero.
while (digitalRead(LIMIT_X) == HIGH) {
stepperRF.move(1); stepperLF.move(-1);
stepperRB.move(-1); stepperLB.move(1);
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
-
Backing off from the limit
- After zeroing, moves slightly away from the limit switches to prevent re-triggering or staying pressed.
stepperLF.move(BACK_OFF_STEPS);
stepperRB.move(BACK_OFF_STEPS);
-
Homing is activated with the command: G28
- When received via serial, it calls:
if (cmd.startsWith("G28")) {
homeAxes();
return;
}
-
Sleep mode
- Also, implement sleep mode by setting the A4988's EN pin HIGH (EN_PIN = HIGH) when no commands are received for a specified duration.
- In this case, I set it as 5 seconds:
const unsigned long INACTIVITY_TIMEOUT = 5000; // ms
if (millis() - lastCommandTime > INACTIVITY_TIMEOUT) {
digitalWrite(EN_PIN, HIGH); // disable drivers
}
These lines significantly improve the testing experience by reducing noise and heat when the machine is idle, without needing to power it off!
Arduino code
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 | #include <AccelStepper.h>
#include <Servo.h>
// Calibration: steps required to move 1mm or 1 degree
const float STEPS_PER_MM_X = 17.95; // adjust based on mechanical setup
const float STEPS_PER_MM_Y = 17.00;
const float STEPS_PER_DEG_R = 45.50; // for rotation (if applicable)
// Stepper motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping control
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
// Servo Z-axis
Servo servoZ;
const int SERVO_PIN_Z = 22;
int currentZ = 0;
// Limit switches
const int LIMIT_X = 28;
const int LIMIT_Y = 27;
const int MAX_SPEED = 800;
const int ACCEL = 600;
const int HOMING_SPEED = 200;
const int BACK_OFF_STEPS = 500;
const unsigned long INACTIVITY_TIMEOUT = 5000; // ms
unsigned long lastCommandTime = 0;
String input = "";
// Stepper drivers
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
AccelStepper stepperLF(AccelStepper::DRIVER, STEP_PIN_LF, DIR_PIN_LF);
AccelStepper stepperRB(AccelStepper::DRIVER, STEP_PIN_RB, DIR_PIN_RB);
AccelStepper stepperLB(AccelStepper::DRIVER, STEP_PIN_LB, DIR_PIN_LB);
void setup() {
Serial.begin(115200);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
pinMode(LIMIT_X, INPUT_PULLUP);
pinMode(LIMIT_Y, INPUT_PULLUP);
digitalWrite(EN_PIN, LOW); // enable drivers
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);
stepperRF.setMaxSpeed(MAX_SPEED); stepperRF.setAcceleration(ACCEL);
stepperLF.setMaxSpeed(MAX_SPEED); stepperLF.setAcceleration(ACCEL);
stepperRB.setMaxSpeed(MAX_SPEED); stepperRB.setAcceleration(ACCEL);
stepperLB.setMaxSpeed(MAX_SPEED); stepperLB.setAcceleration(ACCEL);
servoZ.attach(SERVO_PIN_Z, 500, 2400);
servoZ.write(currentZ);
lastCommandTime = millis();
Serial.println("Ready for G-code like G1 X100 Y0 Z90 or G28");
}
void homeAxes() {
Serial.println("Starting zeroing routine...");
stepperRF.setMaxSpeed(HOMING_SPEED);
stepperLF.setMaxSpeed(HOMING_SPEED);
stepperRB.setMaxSpeed(HOMING_SPEED);
stepperLB.setMaxSpeed(HOMING_SPEED);
// --- Homing X ---
while (digitalRead(LIMIT_X) == HIGH) {
stepperRF.move(1);
stepperLF.move(-1);
stepperRB.move(-1);
stepperLB.move(1);
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
stepperRF.stop(); stepperLF.stop(); stepperRB.stop(); stepperLB.stop();
stepperRF.setCurrentPosition(0); stepperLF.setCurrentPosition(0);
stepperRB.setCurrentPosition(0); stepperLB.setCurrentPosition(0);
Serial.println("LIMIT_X triggered. X position zeroed.");
delay(200);
// --- Homing Y ---
while (digitalRead(LIMIT_Y) == HIGH) {
stepperRF.move(-1);
stepperLF.move(-1);
stepperRB.move(-1);
stepperLB.move(-1);
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
stepperRF.stop(); stepperLF.stop(); stepperRB.stop(); stepperLB.stop();
stepperRF.setCurrentPosition(0); stepperLF.setCurrentPosition(0);
stepperRB.setCurrentPosition(0); stepperLB.setCurrentPosition(0);
Serial.println("LIMIT_Y triggered. Y position zeroed.");
delay(200);
// Back off
stepperLF.move(BACK_OFF_STEPS);
stepperRB.move(BACK_OFF_STEPS);
while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
Serial.println("Zeroing complete");
}
void parseAndExecute(String cmd) {
cmd.trim();
cmd.toUpperCase();
lastCommandTime = millis(); // update activity timestamp
digitalWrite(EN_PIN, LOW); // wake up drivers if asleep
if (cmd.startsWith("G28")) {
homeAxes();
return;
}
if (!cmd.startsWith("G1")) {
Serial.println("Invalid or unsupported command.");
return;
}
int x = 0, y = 0, r = 0, z = -1;
int xIndex = cmd.indexOf('X');
if (xIndex != -1) x = cmd.substring(xIndex + 1).toInt();
int yIndex = cmd.indexOf('Y');
if (yIndex != -1) y = cmd.substring(yIndex + 1).toInt();
int rIndex = cmd.indexOf('R');
if (rIndex != -1) r = cmd.substring(rIndex + 1).toInt();
int zIndex = cmd.indexOf('Z');
if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();
Serial.print("Parsed -> X: "); Serial.print(x);
Serial.print(" Y: "); Serial.print(y);
Serial.print(" R: "); Serial.print(r);
if (z != -1) {
Serial.print(" Z: "); Serial.print(z);
}
Serial.println();
if (z != -1) {
z = constrain(z, 0, 180);
servoZ.write(z);
currentZ = z;
Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
}
long rfSteps = (y * STEPS_PER_MM_Y) - (x * STEPS_PER_MM_X) - (r * STEPS_PER_DEG_R);
long lfSteps = (y * STEPS_PER_MM_Y) + (x * STEPS_PER_MM_X) + (r * STEPS_PER_DEG_R);
long rbSteps = (y * STEPS_PER_MM_Y) + (x * STEPS_PER_MM_X) - (r * STEPS_PER_DEG_R);
long lbSteps = (y * STEPS_PER_MM_Y) - (x * STEPS_PER_MM_X) + (r * STEPS_PER_DEG_R);
stepperRF.move(rfSteps);
stepperLF.move(lfSteps);
stepperRB.move(rbSteps);
stepperLB.move(lbSteps);
while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
}
void loop() {
// Serial input
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (input.length() > 0) {
parseAndExecute(input);
input = "";
}
} else {
input += c;
}
}
// Sleep mode
if (millis() - lastCommandTime > INACTIVITY_TIMEOUT) {
digitalWrite(EN_PIN, HIGH); // disable drivers
}
}
|
G.Final Arduino code
I then added global position tracking, switched to relative movement using delta calculations, and updated position values after each move for better coordination. This enabled the implementation of relative movement commands from Processing (using W/A/S/D keys for directional control), as well as support for homing and zeroing functions.
Updates:
-
Global Position Tracking Added
- Introduced
currentX, currentY, and currentR to track absolute position.
- Tracks current position in mm/degrees for coordinated motion commands. This allows for relative to absolute motion tracking.
float currentX = 0;
float currentY = 0;
float currentR = 0;
-
Homing movement method
- Uses
runSpeed() instead of move()/run()
runSpeed() provides smoother, fixed-speed motion ideal for homing, avoiding acceleration/deceleration.
stepperRF.setSpeed(100);
stepperRF.runSpeed();
-
Position reset after homing
- Explicitly resets global position after homing
- Keeps logical (global) position variables in sync with physical homed zero.
currentX = 0;
currentY = 0;
currentR = 0;
-
G1 Command uses delta calculation
- Calculates movement delta instead of absolute movement.
- Ensures movement is made relative to current position — avoids cumulative error and allows for sequential G-code moves.
float dx = targetX - currentX;
float dy = targetY - currentY;
float dr = targetR - currentR;
-
Position updated after movement
- Updates
currentX, currentY, and currentR after motion
- Keeps software-side position tracking accurate, enabling future relative motion.
currentX = targetX;
currentY = targetY;
currentR = targetR;
Arduino code
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 | #include <AccelStepper.h>
#include <Servo.h>
// Global position
float currentX = 0;
float currentY = 0;
float currentR = 0;
// Calibration: steps required to move 1mm or 1 degree
const float STEPS_PER_MM_X = 17.95; // adjust based on mechanical setup
const float STEPS_PER_MM_Y = 17.00;
const float STEPS_PER_DEG_R = 45.50; // for rotation
// Stepper motor pins
const int STEP_PIN_RF = 19;
const int DIR_PIN_RF = 18;
const int STEP_PIN_LF = 12;
const int DIR_PIN_LF = 13;
const int STEP_PIN_RB = 17;
const int DIR_PIN_RB = 16;
const int STEP_PIN_LB = 14;
const int DIR_PIN_LB = 15;
// Microstepping control
const int EN_PIN = 9;
const int MS1 = 6, MS2 = 7, MS3 = 8;
// Servo Z-axis
Servo servoZ;
const int SERVO_PIN_Z = 22;
int currentZ = 120; //
// Limit switches
const int LIMIT_X = 28;
const int LIMIT_Y = 27;
const int MAX_SPEED = 800;
const int ACCEL = 600;
const int HOMING_SPEED = 200;
const int BACK_OFF_STEPS = 500;
const unsigned long INACTIVITY_TIMEOUT = 5000; // ms
unsigned long lastCommandTime = 0;
String input = "";
// Stepper drivers
AccelStepper stepperRF(AccelStepper::DRIVER, STEP_PIN_RF, DIR_PIN_RF);
AccelStepper stepperLF(AccelStepper::DRIVER, STEP_PIN_LF, DIR_PIN_LF);
AccelStepper stepperRB(AccelStepper::DRIVER, STEP_PIN_RB, DIR_PIN_RB);
AccelStepper stepperLB(AccelStepper::DRIVER, STEP_PIN_LB, DIR_PIN_LB);
void setup() {
Serial.begin(115200);
pinMode(EN_PIN, OUTPUT);
pinMode(MS1, OUTPUT); pinMode(MS2, OUTPUT); pinMode(MS3, OUTPUT);
pinMode(LIMIT_X, INPUT_PULLUP);
pinMode(LIMIT_Y, INPUT_PULLUP);
digitalWrite(EN_PIN, LOW); // enable drivers
digitalWrite(MS1, HIGH); digitalWrite(MS2, HIGH); digitalWrite(MS3, HIGH);
stepperRF.setMaxSpeed(MAX_SPEED); stepperRF.setAcceleration(ACCEL);
stepperLF.setMaxSpeed(MAX_SPEED); stepperLF.setAcceleration(ACCEL);
stepperRB.setMaxSpeed(MAX_SPEED); stepperRB.setAcceleration(ACCEL);
stepperLB.setMaxSpeed(MAX_SPEED); stepperLB.setAcceleration(ACCEL);
servoZ.attach(SERVO_PIN_Z, 500, 2400);
servoZ.write(currentZ);
lastCommandTime = millis();
Serial.println("Ready for G-code like G1 X100 Y0 Z90 or G28");
Serial.print("Initial position: X="); Serial.print(currentX);
Serial.print(" Y="); Serial.print(currentY);
Serial.print(" R="); Serial.println(currentR);
}
void homeAxes() {
Serial.println("Starting zeroing routine...");
stepperRF.setMaxSpeed(HOMING_SPEED);
stepperLF.setMaxSpeed(HOMING_SPEED);
stepperRB.setMaxSpeed(HOMING_SPEED);
stepperLB.setMaxSpeed(HOMING_SPEED);
// --- Homing X ---
stepperRF.setSpeed(100); // Set speed directly (positive or negative)
stepperLF.setSpeed(-100);
stepperRB.setSpeed(-100);
stepperLB.setSpeed(100);
while (digitalRead(LIMIT_X) == HIGH) {
stepperRF.runSpeed();
stepperLF.runSpeed();
stepperRB.runSpeed();
stepperLB.runSpeed();
}
Serial.println("LIMIT_X triggered. X position zeroed.");
delay(200);
// --- Homing Y ---
stepperRF.setSpeed(-100); // Set speed directly (positive or negative)
stepperLF.setSpeed(-100);
stepperRB.setSpeed(-100);
stepperLB.setSpeed(-100);
while (digitalRead(LIMIT_Y) == HIGH) {
stepperRF.runSpeed();
stepperLF.runSpeed();
stepperRB.runSpeed();
stepperLB.runSpeed();
}
// --- Stop and zero
stepperRF.stop(); stepperLF.stop(); stepperRB.stop(); stepperLB.stop();
stepperRF.setCurrentPosition(0); stepperLF.setCurrentPosition(0);
stepperRB.setCurrentPosition(0); stepperLB.setCurrentPosition(0);
Serial.println("LIMIT_Y triggered. Y position zeroed.");
delay(200);
// Reset relative position 0
currentX = 0;
currentY = 0;
currentR = 0;
// Back off
stepperLF.move(BACK_OFF_STEPS);
stepperRB.move(BACK_OFF_STEPS);
while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
Serial.println("Zeroing complete");
}
void parseAndExecute(String cmd) {
cmd.trim();
cmd.toUpperCase();
lastCommandTime = millis();
digitalWrite(EN_PIN, LOW);
if (cmd.startsWith("G28")) {
homeAxes();
return;
}
if (!cmd.startsWith("G1")) {
Serial.println("Invalid or unsupported command.");
return;
}
float targetX = currentX;
float targetY = currentY;
float targetR = currentR;
int z = -1;
int xIndex = cmd.indexOf('X');
if (xIndex != -1) targetX = cmd.substring(xIndex + 1).toFloat();
int yIndex = cmd.indexOf('Y');
if (yIndex != -1) targetY = cmd.substring(yIndex + 1).toFloat();
int rIndex = cmd.indexOf('R');
if (rIndex != -1) targetR = cmd.substring(rIndex + 1).toFloat();
int zIndex = cmd.indexOf('Z');
if (zIndex != -1) z = cmd.substring(zIndex + 1).toInt();
if (z != -1) {
z = constrain(z, 0, 180);
servoZ.write(z);
currentZ = z;
Serial.print("Servo moved to "); Serial.print(z); Serial.println(" degrees");
}
// Calculate deltas
float deltaX = targetX - currentX;
float deltaY = targetY - currentY;
float deltaR = targetR - currentR;
// Convert to steps
float deltaXSteps = deltaX * STEPS_PER_MM_X;
float deltaYSteps = deltaY * STEPS_PER_MM_Y;
float deltaRSteps = deltaR * STEPS_PER_DEG_R;
// Inverse kinematics: calculate wheel steps
long rfSteps = round(-deltaXSteps + deltaYSteps - deltaRSteps);
long lfSteps = round( deltaXSteps + deltaYSteps + deltaRSteps);
long rbSteps = round( deltaXSteps + deltaYSteps - deltaRSteps);
long lbSteps = round(-deltaXSteps + deltaYSteps + deltaRSteps);
stepperRF.move(rfSteps);
stepperLF.move(lfSteps);
stepperRB.move(rbSteps);
stepperLB.move(lbSteps);
while (stepperRF.isRunning() || stepperLF.isRunning() || stepperRB.isRunning() || stepperLB.isRunning()) {
stepperRF.run(); stepperLF.run(); stepperRB.run(); stepperLB.run();
}
// Accumulate actual movement
currentX = targetX;
currentY = targetY;
currentR = targetR;
Serial.println("Moved to:");
Serial.print("X: "); Serial.print(currentX, 2); Serial.print(" mm");
Serial.print(" | Y: "); Serial.print(currentY, 2); Serial.print(" mm");
Serial.print(" | R: "); Serial.print(currentR, 2); Serial.println(" deg");
Serial.print("Servo Z (degrees): ");
Serial.println(currentZ);
}
void loop() {
// Serial input
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (input.length() > 0) {
parseAndExecute(input);
input = "";
Serial.println("ok");
}
} else {
input += c;
}
}
// Sleep mode
if (millis() - lastCommandTime > INACTIVITY_TIMEOUT) {
digitalWrite(EN_PIN, HIGH); // disable drivers
}
}
|
4. Grasshopper G-code generator
The G-code generator definition is based on Diego Garcia Cuevas’s book and a webinar hosted by McNeel Europe.
In this Grasshopper definition, input curves are divided into points. Each point is assigned an up or down value along the Z-axis, then deconstructed into X, Y, and Z coordinates. These coordinates are formatted with the G1 command and concatenated into a G-code string. Since a servo motor is used in this case, the Z values are mapped to a range between 80 and 120 degrees.
5. Processing application programming
Building on the Processing code I developed in Week 14: Interface and Application Programming, I simplified the functionality to focus on G-code parsing while adding support for homing and zeroing features. This Processing code was written largely through so-called “vibe coding,” with reliance on ChatGPT.
-
Manual keyboard control
- It allows manual control using keyboard keys (
W, A, S, D, etc.), where each keypress adjusts the internal position and sends a G1 command. For example:
if (key == 'w' || key == 'W') { posY += 10; updatedXY = true; }
...
if (updatedXY || updatedR) {
gcode = "G1 X" + posX + " Y" + posY + " R" + posR;
}
-
Homing and zeroing
H and Z keys issue homing (G1 X0 Y0 R0) and zeroing (G28) commands, respectively. When an ok or done response is received, the position values reset:
if ((lastCommandWasHoming || lastCommandWasZeroing) &&
(inData.equalsIgnoreCase("ok") || inData.equalsIgnoreCase("done"))) {
posX = 0; posY = 0; posR = 0;
}
-
Load and send G-code
- The
L key loads a G-code file (test.gcode) and sends each line sequentially, skipping blank lines or comments.
G-code location
The test.gcode file must be located in the same directory as the .pde file.
- Sending can be stopped anytime with the
X key.
if (key == 'l' || key == 'L') {
gcodeLines = loadStrings("test.gcode");
...
sendNextLine();
}
-
Sync with serial
- Serial communication is used to sync G-code sending with device responses:
if (sendingFile && (inData.equalsIgnoreCase("ok") || inData.equalsIgnoreCase("done"))) {
sendNextLine();
}
-
UI
- Current position, last sent G-code, and last received message are displayed in the interface for basic feedback.
Processing code
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 | import processing.serial.*;
Serial port;
String[] gcodeLines;
int lineIndex = 0;
boolean sendingFile = false;
// Accumulated positions
int posX = 0;
int posY = 0;
int posR = 0;
// Flags for homing and zeroing
boolean lastCommandWasHoming = false;
boolean lastCommandWasZeroing = false;
// Display strings
String lastSentGcode = "";
String lastReceivedMessage = "";
void setup() {
size(400, 400);
port = new Serial(this, "/dev/tty.usbmodem14101", 115200);
port.clear();
port.bufferUntil('\n');
println("Connected.");
textFont(createFont("Arial Bold", 22));
}
void draw() {
background(0);
fill(255);
textSize(22);
text("Q: CCW / W: FW / E: CW / R: UP", 20, 40);
text("A: LH / S: BW / D: RH / F: DW", 20, 70);
text("H: Homing", 20, 110);
text("Z: Zeroing (G28)", 20, 140);
text("L: Load [test.gcode]", 20, 170);
text("X: Stop sending", 20, 200);
textSize(26);
fill(255, 255, 0);
text("X: " + posX + " Y: " + posY + " R: " + posR, 20, 260);
fill(255);
textSize(20);
text("Sending: " + lastSentGcode, 20, 320);
text("Received: " + lastReceivedMessage, 20, 350);
if (lastSentGcode.equals("STOPPED")) {
fill(255, 0, 0);
text("Stopped. No data is being sent.", 20, 380);
}
}
void keyPressed() {
String gcode = "";
boolean updatedXY = false;
boolean updatedR = false;
if (key == 'w' || key == 'W') { posY += 10; updatedXY = true; }
if (key == 's' || key == 'S') { posY -= 10; updatedXY = true; }
if (key == 'a' || key == 'A') { posX -= 10; updatedXY = true; }
if (key == 'd' || key == 'D') { posX += 10; updatedXY = true; }
if (key == 'q' || key == 'Q') { posR += 10; updatedR = true; }
if (key == 'e' || key == 'E') { posR -= 10; updatedR = true; }
if (updatedXY || updatedR) {
gcode = "G1 X" + posX + " Y" + posY + " R" + posR;
}
if (key == 'r' || key == 'R') gcode = "G1 Z120";
if (key == 'f' || key == 'F') gcode = "G1 Z80";
if (key == 'h' || key == 'H') {
gcode = "G1 X0 Y0 R0";
lastCommandWasHoming = true;
lastCommandWasZeroing = false;
}
if (key == 'z' || key == 'Z') {
gcode = "G28";
lastCommandWasZeroing = true;
lastCommandWasHoming = false;
}
if (key == 'x' || key == 'X') {
sendingFile = false;
println("STOPPED: G-code sending cancelled.");
lastSentGcode = "STOPPED";
lastReceivedMessage = "";
return;
}
if (key == 'l' || key == 'L') {
gcodeLines = loadStrings("test.gcode");
if (gcodeLines != null) {
println("Loaded " + gcodeLines.length + " lines.");
lineIndex = 0;
sendingFile = true;
sendNextLine();
} else {
println("Failed to load test.gcode.");
}
return;
}
if (!gcode.equals("")) {
println("Sending: " + gcode);
lastSentGcode = gcode;
port.write(gcode + "\n");
}
}
void sendNextLine() {
while (lineIndex < gcodeLines.length) {
String line = trim(gcodeLines[lineIndex]);
lineIndex++;
if (line.length() == 0 || line.startsWith(";")) continue;
println("Sending: " + line);
lastSentGcode = line;
port.write(line + "\n");
break;
}
if (lineIndex >= gcodeLines.length) {
println("All G-code lines sent.");
sendingFile = false;
}
}
void serialEvent(Serial port) {
String inData = trim(port.readStringUntil('\n'));
println("Received: " + inData);
lastReceivedMessage = inData;
if (sendingFile && (inData.equalsIgnoreCase("ok") || inData.equalsIgnoreCase("done"))) {
sendNextLine();
return;
}
if ((lastCommandWasHoming || lastCommandWasZeroing) &&
(inData.equalsIgnoreCase("ok") || inData.equalsIgnoreCase("done"))) {
posX = 0;
posY = 0;
posR = 0;
lastCommandWasHoming = false;
lastCommandWasZeroing = false;
}
}
|
6. How to use them
- Right-click the leftmost “Curve” component and select “Set Multiple Curves”.
- Set the following parameters:
- Resolution
- Servo remapping (from
Down angle to Up angle)
- After slicing the curve:
- In the rightmost panel, select “Copy Data Only”
- Paste the data into a text editor and save it as
test.gcode
- Place the
test.gcode at the same directory as the Processing file later.
Please refer to BOM and Files | Codes for the final Arduino, Processing, and Grasshopper.