Hi there,
I am working on Mini CNC Plotter Machine using Arduino Uno + Motor shield + GCTRL for control. Every things work fine but when the machine reaches the code M300 S30 pen up and not reaches the end of gcode. Which creates problem in servo motor for penup pendown parts in complex drawing where penup is in the middle of some large gcode. Stepper motors are totally fine, but facing problem with pen up down with servo motor.
Hardware used: Arduino Uno R3, L293D MotorDriver Shield, Stepper motor from CD/DVD drives and Micro Servo SG90 Motor
Here the codes I have used
//Arduino Code
#include <Servo.h>
#include <AFMotor.h>
#define LINE_BUFFER_LENGTH 512
char STEP = MICROSTEP ;
// Servo position for Up and Down
const int penZUp = 40;
const int penZDown = 80;
// Servo on PWM pin 10
const int penServoPin =10 ;
// Should be right for DVD steppers, but is not too important here
const int stepsPerRevolution = 48;
// create servo object to control a servo
Servo penServo;
// Initialize steppers for X- and Y-axis using this Arduino pins for the L293D H-bridge
AF_Stepper myStepperY(stepsPerRevolution,1);
AF_Stepper myStepperX(stepsPerRevolution,2);
/* Structures, global variables */
struct point {
float x;
float y;
float z;
};
// Current position of plothead
struct point actuatorPos;
// Drawing settings, should be OK
float StepInc = 1;
int StepDelay = 0;
int LineDelay =0;
int penDelay = 50;
// Motor steps to go 1 millimeter.
// Use test sketch to go 100 steps. Measure the length of line.
// Calculate steps per mm. Enter here.
float StepsPerMillimeterX = 100.0;
float StepsPerMillimeterY = 100.0;
// Drawing robot limits, in mm
// OK to start with. Could go up to 50 mm if calibrated well.
float Xmin = -20;
float Xmax = 20;
float Ymin = -20;
float Ymax = 20;
float Zmin = 0;
float Zmax = 1;
float Xpos = Xmin;
float Ypos = Ymin;
float Zpos = Zmax;
// Set to true to get debug output.
boolean verbose = false;
// Needs to interpret
// G1 for moving
// G4 P300 (wait 150ms)
// M300 S30 (pen down)
// M300 S50 (pen up)
// Discard anything with a (
// Discard any other command!
/**********************
* void setup() - Initialisations
***********************/
void setup() {
// Setup
Serial.begin( 9600 );
penServo.attach(penServoPin);
penServo.write(penZUp);
delay(100);
// Decrease if necessary
myStepperX.setSpeed(600);
myStepperY.setSpeed(600);
// Set & move to initial default position
// TBD
// Notifications!!!
Serial.println("Mini CNC Plotter alive and kicking!");
Serial.print("X range is from ");
Serial.print(Xmin);
Serial.print(" to ");
Serial.print(Xmax);
Serial.println(" mm.");
Serial.print("Y range is from ");
Serial.print(Ymin);
Serial.print(" to ");
Serial.print(Ymax);
Serial.println(" mm.");
}
/**********************
* void loop() - Main loop
***********************/
void loop()
{
delay(100);
char line[ LINE_BUFFER_LENGTH ];
char c;
int lineIndex;
bool lineIsComment, lineSemiColon;
lineIndex = 0;
lineSemiColon = false;
lineIsComment = false;
while (1) {
// Serial reception - Mostly from Grbl, added semicolon support
while ( Serial.available()>0 ) {
c = Serial.read();
if (( c == '\n') || (c == '\r') ) { // End of line reached
if ( lineIndex > 0 ) { // Line is complete. Then execute!
line[ lineIndex ] = '\0'; // Terminate string
if (verbose) {
Serial.print( "Received : ");
Serial.println( line );
}
processIncomingLine( line, lineIndex );
lineIndex = 0;
}
else {
// Empty or comment line. Skip block.
}
lineIsComment = false;
lineSemiColon = false;
Serial.println("ok");
}
else {
if ( (lineIsComment) || (lineSemiColon) ) { // Throw away all comment characters
if ( c == ')' ) lineIsComment = false; // End of comment. Resume line.
}
else {
if ( c <= ' ' ) { // Throw away whitepace and control characters
}
else if ( c == '/' ) { // Block delete not supported. Ignore character.
}
else if ( c == '(' ) { // Enable comments flag and ignore all characters until ')' or EOL.
lineIsComment = true;
}
else if ( c == ';' ) {
lineSemiColon = true;
}
else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) {
Serial.println( "ERROR - lineBuffer overflow" );
lineIsComment = false;
lineSemiColon = false;
}
else if ( c >= 'a' && c <= 'z' ) { // Upcase lowercase
line[ lineIndex++ ] = c-'a'+'A';
}
else {
line[ lineIndex++ ] = c;
}
}
}
}
}
}
void processIncomingLine( char* line, int charNB ) {
int currentIndex = 0;
char buffer[ 64 ]; // Hope that 64 is enough for 1 parameter
struct point newPos;
newPos.x = 0.0;
newPos.y = 0.0;
// Needs to interpret
// G1 for moving
// G4 P300 (wait 150ms)
// G1 X60 Y30
// G1 X30 Y50
// M300 S30 (pen down)
// M300 S50 (pen up)
// Discard anything with a (
// Discard any other command!
while( currentIndex < charNB ) {
switch ( line[ currentIndex++ ] ) { // Select command, if any
case 'U':
penUp();
break;
case 'D':
penDown();
break;
case 'G':
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 2 digit commands
// buffer[1] = line[ currentIndex++ ];
// buffer[2] = '\0';
buffer[1] = '\0';
switch ( atoi( buffer ) ){ // Select G command
case 0: // G00 & G01 - Movement or fast movement. Same here
case 1:
// /!\ Dirty - Suppose that X is before Y
char* indexX = strchr( line+currentIndex, 'X' ); // Get X/Y position in the string (if any)
char* indexY = strchr( line+currentIndex, 'Y' );
if ( indexY <= 0 ) {
newPos.x = atof( indexX + 1);
newPos.y = actuatorPos.y;
}
else if ( indexX <= 0 ) {
newPos.y = atof( indexY + 1);
newPos.x = actuatorPos.x;
}
else {
newPos.y = atof( indexY + 1);
indexY = '\0';
newPos.x = atof( indexX + 1);
}
drawLine(newPos.x, newPos.y );
// Serial.println("ok");
actuatorPos.x = newPos.x;
actuatorPos.y = newPos.y;
break;
}
break;
case 'M':
buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works with 3 digit commands
buffer[1] = line[ currentIndex++ ];
buffer[2] = line[ currentIndex++ ];
buffer[3] = '\0';
switch ( atoi( buffer ) ){
case 300:
{
char* indexS = strchr( line+currentIndex, 'S' );
float Spos = atof( indexS + 1);
// Serial.println("ok");
if (Spos == 50) {
penUp();
}
if (Spos == 30) {
penDown();
}
break;
}
case 114: // M114 - Repport position
Serial.print( "Absolute position : X = " );
Serial.print( actuatorPos.x );
Serial.print( " - Y = " );
Serial.println( actuatorPos.y );
break;
default:
if ((buffer[0] == 1) && (buffer [1] == 8)){ // M18 = drives off
Serial.println( " M18: we are done, turn drives off" );
myStepperX.release();
myStepperY.release();
}
else {
Serial.print("Command not recognised: M");
Serial.println( buffer );
}
}
}
}
}
/*********************************
* Draw a line from (x0;y0) to (x1;y1).
* int (x1;y1) : Starting coordinates
* int (x2;y2) : Ending coordinates
**********************************/
void drawLine(float x1, float y1) {
if (verbose)
{
Serial.print("fx1, fy1: ");
Serial.print(x1);
Serial.print(",");
Serial.print(y1);
Serial.println("");
}
// Bring instructions within limits
if (x1 >= Xmax) {
x1 = Xmax;
}
if (x1 <= Xmin) {
x1 = Xmin;
}
if (y1 >= Ymax) {
y1 = Ymax;
}
if (y1 <= Ymin) {
y1 = Ymin;
}
if (verbose)
{
Serial.print("Xpos, Ypos: ");
Serial.print(Xpos);
Serial.print(",");
Serial.print(Ypos);
Serial.println("");
}
if (verbose)
{
Serial.print("x1, y1: ");
Serial.print(x1);
Serial.print(",");
Serial.print(y1);
Serial.println("");
}
// Convert coordinates to steps
x1 = (int)(x1*StepsPerMillimeterX);
y1 = (int)(y1*StepsPerMillimeterY);
float x0 = Xpos;
float y0 = Ypos;
// Let's find out the change for the coordinates
long dx = abs(x1-x0);
long dy = abs(y1-y0);
int sx = x0<x1 ? StepInc : -StepInc;
int sy = y0<y1 ? StepInc : -StepInc;
long i;
long over = 0;
if (dx > dy) {
for (i=0; i<dx; ++i) {
myStepperX.onestep(sx,STEP);
over+=dy;
if (over>=dx) {
over-=dx;
myStepperY.onestep(sy,STEP);
}
delay(StepDelay);
}
}
else {
for (i=0; i<dy; ++i) {
myStepperY.onestep(sy,STEP);
over+=dx;
if (over>=dy) {
over-=dy;
myStepperX.onestep(sx,STEP);
}
delay(StepDelay);
}
}
if (verbose)
{
Serial.print("dx, dy:");
Serial.print(dx);
Serial.print(",");
Serial.print(dy);
Serial.println("");
}
if (verbose)
{
Serial.print("Going to (");
Serial.print(x0);
Serial.print(",");
Serial.print(y0);
Serial.println(")");
}
// Delay before any next lines are submitted
delay(LineDelay);
// Update the positions
Xpos = x1;
Ypos = y1;
}
// Raises pen
void penUp() {
penServo.write(penZUp);
delay(penDelay);
Zpos=Zmax;
//digitalWrite(15, LOW);
//digitalWrite(16, HIGH);
if (verbose) {
Serial.println("Pen up!");
}
}
// Lowers pen
void penDown() {
penServo.write(penZDown);
delay(penDelay);
Zpos=Zmin;
//digitalWrite(15, HIGH);
//digitalWrite(16, LOW);
if (verbose) {
Serial.println("Pen down.");
}
}
import java.awt.event.KeyEvent;
import javax.swing.JOptionPane;
import processing.serial.*;
Serial port = null;
// select and modify the appropriate line for your operating system
// leave as null to use interactive port (press 'p' in the program)
//String portname = null;
//String portname = Serial.list()[0]; // Mac OS X
//String portname = "/dev/ttyUSB0"; // Linux
String portname = "COM10"; // Windows
boolean streaming = false;
float speed = 0.001;
String[] gcode;
int i = 0;
void openSerialPort()
{
if (portname == null) return;
if (port != null) port.stop();
port = new Serial(this, portname, 9600);
port.bufferUntil('\n');
}
void selectSerialPort()
{
String result = (String) JOptionPane.showInputDialog(null,
"Select the serial port that corresponds to your Arduino board.",
"Select serial port",
JOptionPane.PLAIN_MESSAGE,
null,
Serial.list(),
0);
if (result != null) {
portname = result;
openSerialPort();
}
}
void setup()
{
size(500, 250);
openSerialPort();
}
void draw()
{
background(0);
fill(255);
int y = 24, dy = 12;
text("INSTRUCTIONS", 12, y); y += dy;
text("p: select serial port", 12, y); y += dy;
text("arrow keys: jog in x-y plane", 12, y); y += dy;
text("5 & 2: jog in z axis", 12, y); y += dy;
text("$: display grbl settings", 12, y); y+= dy;
text("h: go home", 12, y); y += dy;
text("0: zero machine (set home to the current location)", 12, y); y += dy;
text("g: stream a g-code file", 12, y); y += dy;
text("x: stop streaming g-code (this is NOT immediate)", 12, y); y += dy;
y = height - dy;
text("current jog speed: " + speed + " inches per step", 12, y); y -= dy;
text("current serial port: " + portname, 12, y); y -= dy;
}
void keyPressed()
{
if (key == '1') speed = 0.001;
if (key == '2') speed = 0.01;
if (key == '3') speed = 0.1;
if (!streaming) {
if (keyCode == LEFT) port.write("G21/G90/G1 X-10 F3500\n");
if (keyCode == RIGHT) port.write("G21/G90/G1 X10 F3500\n");
if (keyCode == UP) port.write("G21/G90/G1 Y10 F3500\n");
if (keyCode == DOWN) port.write("G21/G90/G1 Y-10 F3500\n");
if (key == '5') port.write("M300 S50\n");
if (key == '2') port.write("M300 S30\n");
if (key == 'h') port.write("G90\nG20\nG00 X0.000 Y0.000 Z0.000\n");
if (key == 'v') port.write("$0=75\n$1=74\n$2=75\n");
//if (key == 'v') port.write("$0=100\n$1=74\n$2=75\n");
if (key == 's') port.write("$3=10\n");
if (key == 'e') port.write("$16=1\n");
if (key == 'd') port.write("$16=0\n");
if (key == '0') openSerialPort();
if (key == 'p') selectSerialPort();
if (key == '$') port.write("$$\n");
}
if (!streaming && key == 'g') {
gcode = null; i = 0;
File file = null;
println("Loading file...");
selectInput("Select a file to process:", "fileSelected", file);
}
if (key == 'x') streaming = false;
}
void fileSelected(File selection) {
if (selection == null) {
println("Window was closed or the user hit cancel.");
} else {
println("User selected " + selection.getAbsolutePath());
gcode = loadStrings(selection.getAbsolutePath());
if (gcode == null) return;
streaming = true;
stream();
}
}
void stream()
{
if (!streaming) return;
while (true) {
if (i == gcode.length) {
streaming = false;
return;
}
if (gcode[i].trim().length() == 0) i++;
else break;
}
println(gcode[i]);
port.write(gcode[i] + '\n');
i++;
}
void serialEvent(Serial p)
{
String s = p.readStringUntil('\n');
println(s.trim());
if (s.trim().startsWith("ok")) stream();
if (s.trim().startsWith("error")) stream(); // XXX: really?
}
//GCTRL code which is used in Processing software
//Sample Gcode which should run till end
(Scribbled version of C:\Users\sgosw\AppData\Local\Temp\ink_ext_XXXXXX.svgIVSFN2 @ 3500.00)
( unicorn.py --tab="pen_changes" --pen-up-angle=50 --pen-down-angle=30 --start-delay=150 --stop-delay=150 --xy-feedrate=3500 --z-feedrate=150 --z-height=0 --finished-height=0 --register-pen=false --x-home=0 --y-home=0 --num-copies=1 --continuous=false --pause-on-layer-change=false C:\Users\sgosw\AppData\Local\Temp\ink_ext_XXXXXX.svgIVSFN2 )
G21 (metric ftw)
G90 (absolute mode)
G92 X0.00 Y0.00 Z0.00 (you are here)
(Polyline consisting of 1 segments.)
G1 X-7.30 Y-4.26 F3500.00
M300 S50 (pen down)
G4 P150 (wait 150ms)
G1 X-5.09 Y-4.07 F3500.00
G1 X-4.36 Y-6.29 F3500.00
G1 X-3.62 Y-7.07 F3500.00
G1 X-2.58 Y-7.69 F3500.00
G1 X0.14 Y-8.22 F3500.00
G1 X2.51 Y-7.82 F3500.00
G1 X4.04 Y-6.70 F3500.00
G1 X4.54 Y-5.16 F3500.00
G1 X4.05 Y-3.68 F3500.00
G1 X2.46 Y-2.62 F3500.00
G1 X-0.69 Y-1.74 F3500.00
G1 X-4.11 Y-0.61 F3500.00
G1 X-5.22 Y0.15 F3500.00
G1 X-6.00 Y1.07 F3500.00
G1 X-6.47 Y2.13 F3500.00
G1 X-6.62 Y3.31 F3500.00
G1 X-6.43 Y4.63 F3500.00
G1 X-5.86 Y5.86 F3500.00
G1 X-4.92 Y6.90 F3500.00
G1 X-3.64 Y7.66 F3500.00
G1 X-2.09 Y8.12 F3500.00
G1 X-0.39 Y8.28 F3500.00
G1 X1.46 Y8.12 F3500.00
G1 X3.08 Y7.63 F3500.00
G1 X4.41 Y6.83 F3500.00
G1 X5.40 Y5.73 F3500.00
G1 X6.02 Y4.39 F3500.00
G1 X6.26 Y2.89 F3500.00
G1 X4.02 Y2.72 F3500.00
G1 X3.62 Y4.22 F3500.00
G1 X2.79 Y5.30 F3500.00
G1 X1.50 Y5.96 F3500.00
G1 X-0.29 Y6.17 F3500.00
G1 X-2.13 Y5.98 F3500.00
G1 X-3.39 Y5.38 F3500.00
G1 X-4.12 Y4.51 F3500.00
G1 X-4.36 Y3.47 F3500.00
G1 X-4.19 Y2.58 F3500.00
G1 X-3.67 Y1.87 F3500.00
G1 X-2.45 Y1.24 F3500.00
G1 X-0.14 Y0.59 F3500.00
G1 X3.78 Y-0.56 F3500.00
G1 X5.12 Y-1.38 F3500.00
G1 X6.06 Y-2.40 F3500.00
G1 X6.61 Y-3.60 F3500.00
G1 X6.80 Y-4.96 F3500.00
G1 X6.59 Y-6.36 F3500.00
G1 X5.99 Y-7.67 F3500.00
G1 X5.00 Y-8.81 F3500.00
G1 X3.66 Y-9.65 F3500.00
G1 X2.05 Y-10.18 F3500.00
G1 X0.25 Y-10.36 F3500.00
G1 X-1.96 Y-10.18 F3500.00
G1 X-3.78 Y-9.65 F3500.00
G1 X-5.23 Y-8.75 F3500.00
G1 X-6.32 Y-7.50 F3500.00
G1 X-7.03 Y-5.98 F3500.00
G1 X-7.30 Y-4.26 F3500.00
G1 X-7.30 Y-4.26 F3500.00
M300 S30 (pen up)
G4 P150 (wait 150ms)
(end of print job)
M300 S50.00 (pen up)
G4 P150 (wait 150ms)
M300 S255 (turn off servo)
G1 X0 Y0 F3500.00
G1 Z0.00 F150.00 (go up to finished level)
G1 X0.00 Y0.00 F3500.00 (go home)
M18 (drives off)
//same gcode which the machine runs
Mini CNC Plotter alive and kicking!
X range is from -20.00 to 20.00 mm.
Y range is from -20.00 to 20.00 mm.
Loading file...
User selected C:\Users\sgosw\OneDrive\Desktop\S.gcode
(Scribbled version of C:\Users\sgosw\AppData\Local\Temp\ink_ext_XXXXXX.svgIVSFN2 @ 3500.00)
ok
( unicorn.py --tab="pen_changes" --pen-up-angle=50 --pen-down-angle=30 --start-delay=150 --stop-delay=150 --xy-feedrate=3500 --z-feedrate=150 --z-height=0 --finished-height=0 --register-pen=false --x-home=0 --y-home=0 --num-copies=1 --continuous=false --pause-on-layer-change=false C:\Users\sgosw\AppData\Local\Temp\ink_ext_XXXXXX.svgIVSFN2 )
ok
G21 (metric ftw)
ok
G90 (absolute mode)
ok
G92 X0.00 Y0.00 Z0.00 (you are here)
ok
(Polyline consisting of 1 segments.)
ok
G1 X-7.30 Y-4.26 F3500.00
ok
M300 S50 (pen down)
ok
G4 P150 (wait 150ms)
ok
G1 X-5.09 Y-4.07 F3500.00
ok
G1 X-4.36 Y-6.29 F3500.00
ok
G1 X-3.62 Y-7.07 F3500.00
ok
G1 X-2.58 Y-7.69 F3500.00
ok
G1 X0.14 Y-8.22 F3500.00
ok
G1 X2.51 Y-7.82 F3500.00
ok
G1 X4.04 Y-6.70 F3500.00
ok
G1 X4.54 Y-5.16 F3500.00
ok
G1 X4.05 Y-3.68 F3500.00
ok
G1 X2.46 Y-2.62 F3500.00
ok
G1 X-0.69 Y-1.74 F3500.00
ok
G1 X-4.11 Y-0.61 F3500.00
ok
G1 X-5.22 Y0.15 F3500.00
ok
G1 X-6.00 Y1.07 F3500.00
ok
G1 X-6.47 Y2.13 F3500.00
ok
G1 X-6.62 Y3.31 F3500.00
ok
G1 X-6.43 Y4.63 F3500.00
ok
G1 X-5.86 Y5.86 F3500.00
ok
G1 X-4.92 Y6.90 F3500.00
ok
G1 X-3.64 Y7.66 F3500.00
ok
G1 X-2.09 Y8.12 F3500.00
ok
G1 X-0.39 Y8.28 F3500.00
ok
G1 X1.46 Y8.12 F3500.00
ok
G1 X3.08 Y7.63 F3500.00
ok
G1 X4.41 Y6.83 F3500.00
ok
G1 X5.40 Y5.73 F3500.00
ok
G1 X6.02 Y4.39 F3500.00
ok
G1 X6.26 Y2.89 F3500.00
ok
G1 X4.02 Y2.72 F3500.00
ok
G1 X3.62 Y4.22 F3500.00
ok
G1 X2.79 Y5.30 F3500.00
ok
G1 X1.50 Y5.96 F3500.00
ok
G1 X-0.29 Y6.17 F3500.00
ok
G1 X-2.13 Y5.98 F3500.00
ok
G1 X-3.39 Y5.38 F3500.00
ok
G1 X-4.12 Y4.51 F3500.00
ok
G1 X-4.36 Y3.47 F3500.00
ok
G1 X-4.19 Y2.58 F3500.00
ok
G1 X-3.67 Y1.87 F3500.00
ok
G1 X-2.45 Y1.24 F3500.00
ok
G1 X-0.14 Y0.59 F3500.00
ok
G1 X3.78 Y-0.56 F3500.00
ok
G1 X5.12 Y-1.38 F3500.00
ok
G1 X6.06 Y-2.40 F3500.00
ok
G1 X6.61 Y-3.60 F3500.00
ok
G1 X6.80 Y-4.96 F3500.00
ok
G1 X6.59 Y-6.36 F3500.00
ok
G1 X5.99 Y-7.67 F3500.00
ok
G1 X5.00 Y-8.81 F3500.00
ok
G1 X3.66 Y-9.65 F3500.00
ok
G1 X2.05 Y-10.18 F3500.00
ok
G1 X0.25 Y-10.36 F3500.00
ok
G1 X-1.96 Y-10.18 F3500.00
ok
G1 X-3.78 Y-9.65 F3500.00
ok
G1 X-5.23 Y-8.75 F3500.00
ok
G1 X-6.32 Y-7.50 F3500.00
ok
G1 X-7.03 Y-5.98 F3500.00
ok
G1 X-7.30 Y-4.26 F3500.00
ok
G1 X-7.30 Y-4.26 F3500.00
ok
M300 S30 (pen up)
