#include <M5Unified.h>
#include <HardwareSerial.h>
#include <vector>
#include <SD.h>
// --- Work Area Settings ---
// The drawing area (540px) is mapped to the physical work area in mm.
const float WORK_AREA_WIDTH_MM = 30.0; // Set work area width to 30mm
const float WORK_AREA_HEIGHT_MM = 30.0; // Set work area height to 30mm
// --------------------------
HardwareSerial SerialNano(2); // Use UART2
#define DRAW_AREA_X 10
#define DRAW_AREA_Y 10
#define DRAW_AREA_SIZE 540
#define BUTTON_WIDTH 140
#define BUTTON_HEIGHT 40
#define BUTTON_Y (DRAW_AREA_Y + DRAW_AREA_SIZE + 10)
#define REDRAW_X 10
#define PLOT_X 170
#define SET_ZERO_X 330
#define BUTTON_ROW2_Y (BUTTON_Y + BUTTON_HEIGHT + 10)
#define BUTTON_ROW3_Y (BUTTON_ROW2_Y + BUTTON_HEIGHT + 10)
#define BUTTON_ROW4_Y (BUTTON_ROW3_Y + BUTTON_HEIGHT + 10)
#define JOG_STEP_LARGE 5
#define JOG_STEP_SMALL 1
#define JOG_STEP_Z 10
#define JOG_BTN_WIDTH 100
#define JOG_XY_C1 20
#define JOG_XY_C2 140
#define JOG_XY_C3 260
#define JOG_XY_C4 380
#define JOG_Z_C1 160
#define JOG_Z_C2 280
#define DARKGRAY 0x7BEF
#define WHITE 0xFFFF
#define BLUE 0x001F
#define RED 0xF800
const char* gcode_path = "/CNC/temp.gcode";
struct Point {
int x, y;
};
struct rect_t {
int x, y, w, h;
bool contain(int tx, int ty) {
return x <= tx && tx < (x + w) && y <= ty && ty < (y + h);
}
};
std::vector<std::vector<Point>> strokes;
std::vector<Point> currentStroke;
bool isDrawing = false;
rect_t drawArea = {DRAW_AREA_X, DRAW_AREA_Y, DRAW_AREA_SIZE, DRAW_AREA_SIZE};
rect_t redrawButton = {REDRAW_X, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT};
rect_t plotButton = {PLOT_X, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT};
rect_t setZeroButton = {SET_ZERO_X, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT};
// --- Plotting state machine variables ---
bool grblReadyForNextCommand = true;
bool isPlotting = false;
int plottingStrokeIndex = 0;
int plottingPointIndex = 0;
enum PlottingState { PLOT_IDLE, PLOT_START_STROKE, PLOT_PEN_DOWN, PLOT_DRAWING, PLOT_PEN_UP, PLOT_GO_HOME };
PlottingState plottingState = PLOT_IDLE;
// --- Communication and Timeout variables ---
String grblResponse = "";
bool responseReady = false;
unsigned long commandSentTime = 0;
const unsigned long commandTimeout = 5000; // 5 seconds
rect_t x_minus_large_b = {JOG_XY_C1, BUTTON_ROW2_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t x_minus_small_b = {JOG_XY_C2, BUTTON_ROW2_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t x_plus_small_b = {JOG_XY_C3, BUTTON_ROW2_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t x_plus_large_b = {JOG_XY_C4, BUTTON_ROW2_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t y_minus_large_b = {JOG_XY_C1, BUTTON_ROW3_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t y_minus_small_b = {JOG_XY_C2, BUTTON_ROW3_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t y_plus_small_b = {JOG_XY_C3, BUTTON_ROW3_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t y_plus_large_b = {JOG_XY_C4, BUTTON_ROW3_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t z_minus_button = {JOG_Z_C1, BUTTON_ROW4_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
rect_t z_plus_button = {JOG_Z_C2, BUTTON_ROW4_Y, JOG_BTN_WIDTH, BUTTON_HEIGHT};
void setup() {
auto cfg = M5.config();
M5.begin(cfg);
SerialNano.begin(115200, SERIAL_8N1, 50, 49); // GPIO50=TX, GPIO49=RX
M5.Display.clear();
drawLayout();
}
void drawLayout() {
M5.Display.drawRect(drawArea.x, drawArea.y, drawArea.w, drawArea.h, WHITE);
M5.Display.fillRect(redrawButton.x, redrawButton.y, redrawButton.w, redrawButton.h, DARKGRAY);
M5.Display.setTextColor(WHITE);
M5.Display.setCursor(redrawButton.x + 30, redrawButton.y + 10);
M5.Display.print("Redraw");
M5.Display.fillRect(plotButton.x, plotButton.y, plotButton.w, plotButton.h, DARKGRAY);
M5.Display.setCursor(plotButton.x + 40, plotButton.y + 10);
M5.Display.print("Plot");
M5.Display.fillRect(setZeroButton.x, setZeroButton.y, setZeroButton.w, setZeroButton.h, DARKGRAY);
M5.Display.setCursor(setZeroButton.x + 25, setZeroButton.y + 10);
M5.Display.print("Set Zero");
// Clear status area to the right of the Set Zero button
M5.Display.fillRect(setZeroButton.x + setZeroButton.w + 10, setZeroButton.y, 200, BUTTON_HEIGHT, 0);
// X Jog Buttons
M5.Display.fillRect(x_minus_large_b.x, x_minus_large_b.y, x_minus_large_b.w, x_minus_large_b.h, DARKGRAY);
M5.Display.setCursor(x_minus_large_b.x + 30, x_minus_large_b.y + 10);
M5.Display.print("X- 5");
M5.Display.fillRect(x_minus_small_b.x, x_minus_small_b.y, x_minus_small_b.w, x_minus_small_b.h, DARKGRAY);
M5.Display.setCursor(x_minus_small_b.x + 30, x_minus_small_b.y + 10);
M5.Display.print("X- 1");
M5.Display.fillRect(x_plus_small_b.x, x_plus_small_b.y, x_plus_small_b.w, x_plus_small_b.h, DARKGRAY);
M5.Display.setCursor(x_plus_small_b.x + 30, x_plus_small_b.y + 10);
M5.Display.print("X+ 1");
M5.Display.fillRect(x_plus_large_b.x, x_plus_large_b.y, x_plus_large_b.w, x_plus_large_b.h, DARKGRAY);
M5.Display.setCursor(x_plus_large_b.x + 30, x_plus_large_b.y + 10);
M5.Display.print("X+ 5");
// Y Jog Buttons
M5.Display.fillRect(y_minus_large_b.x, y_minus_large_b.y, y_minus_large_b.w, y_minus_large_b.h, DARKGRAY);
M5.Display.setCursor(y_minus_large_b.x + 30, y_minus_large_b.y + 10);
M5.Display.print("Y- 5");
M5.Display.fillRect(y_minus_small_b.x, y_minus_small_b.y, y_minus_small_b.w, y_minus_small_b.h, DARKGRAY);
M5.Display.setCursor(y_minus_small_b.x + 30, y_minus_small_b.y + 10);
M5.Display.print("Y- 1");
M5.Display.fillRect(y_plus_small_b.x, y_plus_small_b.y, y_plus_small_b.w, y_plus_small_b.h, DARKGRAY);
M5.Display.setCursor(y_plus_small_b.x + 30, y_plus_small_b.y + 10);
M5.Display.print("Y+ 1");
M5.Display.fillRect(y_plus_large_b.x, y_plus_large_b.y, y_plus_large_b.w, y_plus_large_b.h, DARKGRAY);
M5.Display.setCursor(y_plus_large_b.x + 30, y_plus_large_b.y + 10);
M5.Display.print("Y+ 5");
// Z Jog Buttons
M5.Display.fillRect(z_minus_button.x, z_minus_button.y, z_minus_button.w, z_minus_button.h, DARKGRAY);
M5.Display.setCursor(z_minus_button.x + 30, z_minus_button.y + 10);
M5.Display.print("Z- 10");
M5.Display.fillRect(z_plus_button.x, z_plus_button.y, z_plus_button.w, z_plus_button.h, DARKGRAY);
M5.Display.setCursor(z_plus_button.x + 30, z_plus_button.y + 10);
M5.Display.print("Z+ 10");
}
// Robust, line-based serial handling inspired by UGS
void handleSerial() {
while (SerialNano.available() > 0) {
char inChar = SerialNano.read();
if (inChar == '\n') {
responseReady = true;
} else if (inChar != '\r') { // Ignore carriage returns
grblResponse += inChar;
}
}
if (responseReady) {
grblResponse.trim(); // Remove leading/trailing whitespace
// Display the processed line for debugging
M5.Display.fillRect(0, BUTTON_ROW4_Y + BUTTON_HEIGHT + 10, M5.Display.width(), 40, 0); // Clear debug area
M5.Display.setCursor(10, BUTTON_ROW4_Y + BUTTON_HEIGHT + 10);
M5.Display.setTextColor(WHITE);
M5.Display.print("GRBL: ");
M5.Display.print(grblResponse);
if (grblResponse == "ok") {
grblReadyForNextCommand = true;
commandSentTime = millis(); // Reset timeout timer on successful command
} else if (grblResponse.startsWith("error")) {
// An error occurred. Stop the plot and reset.
isPlotting = false;
grblReadyForNextCommand = true;
commandSentTime = 0;
}
grblResponse = "";
responseReady = false;
}
}
void checkTimeout() {
if (!grblReadyForNextCommand && (isPlotting || commandSentTime != 0) && (millis() - commandSentTime > commandTimeout)) {
M5.Display.fillRect(0, BUTTON_ROW4_Y + BUTTON_HEIGHT + 10, M5.Display.width(), 40, RED);
M5.Display.setCursor(10, BUTTON_ROW4_Y + BUTTON_HEIGHT + 10);
M5.Display.setTextColor(WHITE);
M5.Display.print("TIMEOUT: No 'ok' from GRBL!");
isPlotting = false; // Stop the plot
grblReadyForNextCommand = true; // Allow user to try again
commandSentTime = 0; // Reset timer
}
}
void sendCommand(const char* command) {
SerialNano.print(command);
grblReadyForNextCommand = false;
commandSentTime = millis();
}
void handlePlotting() {
if (!isPlotting || !grblReadyForNextCommand) {
return;
}
switch (plottingState) {
case PLOT_START_STROKE: {
if (plottingStrokeIndex >= strokes.size()) {
plottingState = PLOT_GO_HOME;
return;
}
auto& stroke = strokes[plottingStrokeIndex];
if (stroke.empty()) {
plottingStrokeIndex++;
return;
}
Point start_px = stroke[0];
// Convert screen coordinates to G-code coordinates
float gcode_x = (float)(start_px.x - drawArea.x) / drawArea.w * WORK_AREA_WIDTH_MM;
float gcode_y = -1.0 * (float)(start_px.y - drawArea.y) / drawArea.h * WORK_AREA_HEIGHT_MM;
char gcode_buffer[50];
sprintf(gcode_buffer, "G0 X%.2f Y%.2f\n", gcode_x, gcode_y);
sendCommand(gcode_buffer);
plottingPointIndex = 1;
plottingState = PLOT_PEN_DOWN;
break;
}
case PLOT_PEN_DOWN: {
sendCommand("M3 S1000\n");
plottingState = PLOT_DRAWING;
break;
}
case PLOT_DRAWING: {
auto& stroke = strokes[plottingStrokeIndex];
if (plottingPointIndex > 1) {
Point p1 = stroke[plottingPointIndex - 2];
Point p2 = stroke[plottingPointIndex - 1];
M5.Display.drawLine(p1.x, p1.y, p2.x, p2.y, RED);
}
if (plottingPointIndex >= stroke.size()) {
plottingState = PLOT_PEN_UP;
return;
}
Point p_px = stroke[plottingPointIndex];
// Convert screen coordinates to G-code coordinates
float gcode_x = (float)(p_px.x - drawArea.x) / drawArea.w * WORK_AREA_WIDTH_MM;
float gcode_y = -1.0 * (float)(p_px.y - drawArea.y) / drawArea.h * WORK_AREA_HEIGHT_MM;
char gcode_buffer[50];
// ADDED F1000 FEED RATE TO FIX ERROR 22
sprintf(gcode_buffer, "G1 X%.2f Y%.2f F1000\n", gcode_x, gcode_y);
sendCommand(gcode_buffer);
plottingPointIndex++;
break;
}
case PLOT_PEN_UP: {
auto& stroke = strokes[plottingStrokeIndex];
if (stroke.size() > 1) {
Point p1 = stroke[stroke.size() - 2];
Point p2 = stroke[stroke.size() - 1];
M5.Display.drawLine(p1.x, p1.y, p2.x, p2.y, RED);
}
sendCommand("M5\n");
plottingStrokeIndex++;
plottingState = PLOT_START_STROKE;
break;
}
case PLOT_GO_HOME: {
sendCommand("G0 X0 Y0\n");
plottingState = PLOT_IDLE;
isPlotting = false;
break;
}
case PLOT_IDLE:
default:
isPlotting = false;
break;
}
}
void handleJogging(int x, int y) {
if (isPlotting) return; // Don't jog while plotting
char jog_buffer[50];
bool jogCommandSent = false;
if (x_minus_large_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 X-%d\nG90\n", JOG_STEP_LARGE);
jogCommandSent = true;
} else if (x_minus_small_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 X-%d\nG90\n", JOG_STEP_SMALL);
jogCommandSent = true;
} else if (x_plus_small_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 X%d\nG90\n", JOG_STEP_SMALL);
jogCommandSent = true;
} else if (x_plus_large_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 X%d\nG90\n", JOG_STEP_LARGE);
jogCommandSent = true;
} else if (y_minus_large_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Y-%d\nG90\n", JOG_STEP_LARGE);
jogCommandSent = true;
} else if (y_minus_small_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Y-%d\nG90\n", JOG_STEP_SMALL);
jogCommandSent = true;
} else if (y_plus_small_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Y%d\nG90\n", JOG_STEP_SMALL);
jogCommandSent = true;
} else if (y_plus_large_b.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Y%d\nG90\n", JOG_STEP_LARGE);
jogCommandSent = true;
} else if (z_minus_button.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Z-%d\nG90\n", JOG_STEP_Z);
jogCommandSent = true;
} else if (z_plus_button.contain(x, y)) {
sprintf(jog_buffer, "G91 G0 Z%d\nG90\n", JOG_STEP_Z);
jogCommandSent = true;
}
if (jogCommandSent) {
SerialNano.print(jog_buffer);
delay(150); // Debounce to prevent flooding
}
}
void loop() {
M5.update();
handleSerial();
checkTimeout();
if(isPlotting) {
handlePlotting();
}
auto touch = M5.Touch.getDetail();
if (touch.isPressed()) {
int x = touch.x;
int y = touch.y;
if (drawArea.contain(x, y)) {
if (!isDrawing) {
currentStroke.clear();
isDrawing = true;
}
if (currentStroke.empty() || currentStroke.back().x != x || currentStroke.back().y != y) {
currentStroke.push_back({x, y});
if (currentStroke.size() > 1) {
Point p1 = currentStroke[currentStroke.size() - 2];
Point p2 = currentStroke.back();
M5.Display.drawLine(p1.x, p1.y, p2.x, p2.y, WHITE);
}
}
} else if (redrawButton.contain(x, y)) {
if(isPlotting) return;
strokes.clear();
currentStroke.clear();
M5.Display.clear();
drawLayout();
} else if (plotButton.contain(x, y)) {
if (isDrawing && !currentStroke.empty()) {
strokes.push_back(currentStroke);
currentStroke.clear();
}
isDrawing = false;
if (!isPlotting && !strokes.empty()) {
drawStrokes(BLUE);
isPlotting = true;
plottingState = PLOT_START_STROKE;
plottingStrokeIndex = 0;
plottingPointIndex = 0;
grblReadyForNextCommand = true;
}
} else if (setZeroButton.contain(x, y)) {
if (grblReadyForNextCommand && !isPlotting) {
sendCommand("G10 P0 L20 X0 Y0 Z0\n");
}
} else {
// Handle Jogging if no other button was pressed
handleJogging(x, y);
}
} else {
if (isDrawing && !currentStroke.empty()) {
strokes.push_back(currentStroke);
currentStroke.clear();
}
isDrawing = false;
}
}
void drawStrokes(uint16_t color) {
Point lastPoint = {drawArea.x, drawArea.y};
for (auto& stroke : strokes) {
if (stroke.empty()) continue;
Point startOfCurrentStroke = stroke[0];
M5.Display.drawLine(lastPoint.x, lastPoint.y, startOfCurrentStroke.x, startOfCurrentStroke.y, DARKGRAY);
if (stroke.size() >= 2) {
for (size_t i = 0; i < stroke.size() - 1; ++i) {
Point p1 = stroke[i];
Point p2 = stroke[i+1];
M5.Display.drawLine(p1.x, p1.y, p2.x, p2.y, color);
}
}
lastPoint = stroke.back();
}
M5.Display.drawLine(lastPoint.x, lastPoint.y, drawArea.x, drawArea.y, DARKGRAY);
}
Comments