#include <Servo.
h>
#include <AFMotor.h>
#define LINE_BUFFER_LENGTH 512
char STEP = MICROSTEP ;
// Servo position for Up and Down
const int penZUp = 115;
const int penZDown = 83;
// 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 = 0;
float Xmax = 40;
float Ymin = 0;
float Ymax = 40;
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
[Link]( 9600 );
[Link](penServoPin);
[Link](penZUp);
delay(100);
// Decrease if necessary
[Link](600);
[Link](600);
// Set & move to initial default position
// TBD
// Notifications!!!
[Link]("Mini CNC Plotter alive and kicking!");
[Link]("X range is from ");
[Link](Xmin);
[Link](" to ");
[Link](Xmax);
[Link](" mm.");
[Link]("Y range is from ");
[Link](Ymin);
[Link](" to ");
[Link](Ymax);
[Link](" 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 ( [Link]()>0 ) {
c = [Link]();
if (( c == '\n') || (c == '\r') ) { // End of line reached
if ( lineIndex > 0 ) { // Line is complete. Then
execute!
line[ lineIndex ] = '\0'; // Terminate string
if (verbose) {
[Link]( "Received : ");
[Link]( line );
}
processIncomingLine( line, lineIndex );
lineIndex = 0;
}
else {
// Empty or comment line. Skip block.
}
lineIsComment = false;
lineSemiColon = false;
[Link]("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 ) {
[Link]( "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 );
// [Link]("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);
// [Link]("ok");
if (Spos == 30) {
penDown();
}
if (Spos == 50) {
penUp();
}
break;
}
case 114: // M114 - Repport position
[Link]( "Absolute position : X = " );
[Link]( actuatorPos.x );
[Link]( " - Y = " );
[Link]( actuatorPos.y );
break;
default:
[Link]( "Command not recognized : M");
[Link]( 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)
{
[Link]("fx1, fy1: ");
[Link](x1);
[Link](",");
[Link](y1);
[Link]("");
}
// 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)
{
[Link]("Xpos, Ypos: ");
[Link](Xpos);
[Link](",");
[Link](Ypos);
[Link]("");
}
if (verbose)
{
[Link]("x1, y1: ");
[Link](x1);
[Link](",");
[Link](y1);
[Link]("");
}
// 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) {
[Link](sx,STEP);
over+=dy;
if (over>=dx) {
over-=dx;
[Link](sy,STEP);
}
delay(StepDelay);
}
}
else {
for (i=0; i<dy; ++i) {
[Link](sy,STEP);
over+=dx;
if (over>=dy) {
over-=dy;
[Link](sx,STEP);
}
delay(StepDelay);
}
}
if (verbose)
{
[Link]("dx, dy:");
[Link](dx);
[Link](",");
[Link](dy);
[Link]("");
}
if (verbose)
{
[Link]("Going to (");
[Link](x0);
[Link](",");
[Link](y0);
[Link](")");
}
// Delay before any next lines are submitted
delay(LineDelay);
// Update the positions
Xpos = x1;
Ypos = y1;
}
// Raises pen
void penUp() {
[Link](penZUp);
delay(penDelay);
Zpos=Zmax;
digitalWrite(15, LOW);
digitalWrite(16, HIGH);
if (verbose) {
[Link]("Pen up!");
}
}
// Lowers pen
void penDown() {
[Link](penZDown);
delay(penDelay);
Zpos=Zmin;
digitalWrite(15, HIGH);
digitalWrite(16, LOW);
if (verbose) {
[Link]("Pen down.");
}
}