CNC CODE

1
2 #include <Servo.h>
3 #include <Stepper.h>
4
5 #define LINE_BUFFER_LENGTH 512
6
7 const int penZUp = 80;
8 const int penZDown = 40;
9
0 const int penServoPin = 6;
1
2 const int stepsPerRevolution = 48;
3
4 Servo penServo;
5
6 Stepper myStepperY(stepsPerRevolution, 2,3,4,5);
7 Stepper myStepperX(stepsPerRevolution, 11,10,8,9);
8
9 struct point {
0 float x;
1 float y;
2 float z;
3 };
4
5 struct point actuatorPos;
6
7 float StepInc = 1;
8 int StepDelay = 0;
9 int LineDelay = 50;
0 int penDelay = 50;
1
2 float StepsPerMillimeterX = 6.0;
3 float StepsPerMillimeterY = 6.0;
4
5 float Xmin = 0;
6 float Xmax = 40;
7 float Ymin = 0;
8 float Ymax = 40;
9 float Zmin = 0;
0 float Zmax = 1;
1
2 float Xpos = Xmin;
3 float Ypos = Ymin;
4 float Zpos = Zmax;
5
6 boolean verbose = false;
7
8 void setup() {
9 Serial.begin( 9600 );
0
1 penServo.attach(penServoPin);
2 penServo.write(penZUp);
3 delay(200);
4
5 myStepperX.setSpeed(100 );
6 myStepperY.setSpeed(100);
7
8 Serial.println("Mini CNC Plotter alive and kicking!");
9 Serial.print("X range is from ");
0 Serial.print(Xmin);
1 Serial.print(" to ");
2 Serial.print(Xmax);
3 Serial.println(" mm.");
4 Serial.print("Y range is from ");
5 Serial.print(Ymin);
6 Serial.print(" to ");
7 Serial.print(Ymax);
8 Serial.println(" mm.");
9 }
0
1 void loop()
2 {
3 delay(200);
4 char line[ LINE_BUFFER_LENGTH ];
5 char c;
6 int lineIndex;
7 bool lineIsComment, lineSemiColon;
8
9 lineIndex = 0;
0 lineSemiColon = false;
1 lineIsComment = false;
2
3 while (1) {
4
5 while ( Serial.available()>0 ) {
6 c = Serial.read();
7 if (( c == ’\n’) || (c == ’\r’) ) {
8 if ( lineIndex > 0 ) {
9 line[ lineIndex ] = ’\0’;
0 if (verbose) {
1 Serial.print( "Received : ");
2 Serial.println( line );
3 }
4 processIncomingLine( line, lineIndex );
5 lineIndex = 0;
6 }
7 else {
8
9 }
0 lineIsComment = false;
1 lineSemiColon = false;
2 Serial.println("ok");
3 }
4 else {
5 if ( (lineIsComment) || (lineSemiColon) ) { // Throw away all
comment characters
6 if ( c == ’)’ ) lineIsComment = false; // End of comment.
Resume line.
7 }
8 else {
9 if ( c <= ’ ’ ) { // Throw away whitepace
and control characters
0 }
1 else if ( c == ’/’ ) { // Block delete not
supported. Ignore character.
2 }
3 else if ( c == ’(’ ) { // Enable comments flag
and ignore all characters until ’)’ or EOL.
4 lineIsComment = true;
5 }
6 else if ( c == ’;’ ) {
7 lineSemiColon = true;
8 }
9 else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) {
0 Serial.println( "ERROR - lineBuffer overflow" );
1 lineIsComment = false;
2 lineSemiColon = false;
3 }
4 else if ( c >= ’a’ && c <= ’z’ ) { // Upcase lowercase
5 line[ lineIndex++ ] = c-’a’+’A’;
6 }
7 else {
8 line[ lineIndex++ ] = c;
9 }
0 }
1 }
2 }
3 }
4 }
5
6 void processIncomingLine( char* line, int charNB ) {
7 int currentIndex = 0;
8 char buffer[ 64 ]; // Hope that 64 is enough
for 1 parameter
9 struct point newPos;
0
1 newPos.x = 0.0;
2 newPos.y = 0.0;
3
4 // Needs to interpret
5 // G1 for moving
6 // G4 P300 (wait 150ms)
7 // G1 X60 Y30
8 // G1 X30 Y50
9 // M300 S30 (pen down)
0 // M300 S50 (pen up)
1 // Discard anything with a (
2 // Discard any other command!
3
4 while( currentIndex < charNB ) {
5 switch ( line[ currentIndex++ ] ) { // Select command, if any
6 case ’U’:
7 penUp();
8 break;
9 case ’D’:
0 penDown();
1 break;
2 case ’G’:
3 buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works
with 2 digit commands
4 // buffer[1] = line[ currentIndex++ ];
5 // buffer[2] = ’\0’;
6 buffer[1] = ’\0’;
7
8 switch ( atoi( buffer ) ){ // Select G command
9 case 0: // G00 & G01 - Movement or
fast movement. Same here
0 case 1:
1 // /!\ Dirty - Suppose that X is before Y
2 char* indexX = strchr( line+currentIndex, ’X’ ); // Get X/Y position
in the string (if any)
3 char* indexY = strchr( line+currentIndex, ’Y’ );
4 if ( indexY <= 0 ) {
5 newPos.x = atof( indexX + 1);
6 newPos.y = actuatorPos.y;
7 }
8 else if ( indexX <= 0 ) {
9 newPos.y = atof( indexY + 1);
0 newPos.x = actuatorPos.x;
1 }
2 else {
3 newPos.y = atof( indexY + 1);
4 indexY = ’\0’;
5 newPos.x = atof( indexX + 1);
6 }
7 drawLine(newPos.x, newPos.y );
8 // Serial.println("ok");
9 actuatorPos.x = newPos.x;
0 actuatorPos.y = newPos.y;
1 break;
2 }
3 break;
4 case ’M’:
5 buffer[0] = line[ currentIndex++ ]; // /!\ Dirty - Only works
with 3 digit commands
6 buffer[1] = line[ currentIndex++ ];
7 buffer[2] = line[ currentIndex++ ];
8 buffer[3] = ’\0’;
9 switch ( atoi( buffer ) ){
0 case 300:
1 {
2 char* indexS = strchr( line+currentIndex, ’S’ );
3 float Spos = atof( indexS + 1);
4 // Serial.println("ok");
5 if (Spos == 30) {
6 penDown();
7 }
8 if (Spos == 50) {
9 penUp();
0 }
1 break;
2 }
3 case 114: // M114 - Repport position
4 Serial.print( "Absolute position : X = " );
5 Serial.print( actuatorPos.x );
6 Serial.print( " - Y = " );
7 Serial.println( actuatorPos.y );
8 break;
9 default:
0 Serial.print( "Command not recognized : M");
1 Serial.println( buffer );
2 }
3 }
4 }
5
6
7
8 }
9
0
1
2 void drawLine(float x1, float y1) {
3
4 if (verbose)
5 {
6 Serial.print("fx1, fy1: ");
7 Serial.print(x1);
8 Serial.print(",");
9 Serial.print(y1);
0 Serial.println("");
1 }
2
3
4 if (x1 >= Xmax) {
5 x1 = Xmax;
6 }
7 if (x1 <= Xmin) {
8 x1 = Xmin;
9 }
0 if (y1 >= Ymax) {
1 y1 = Ymax;
2 }
3 if (y1 <= Ymin) {
4 y1 = Ymin;
5 }
6
7 if (verbose)
8 {
9 Serial.print("Xpos, Ypos: ");
0 Serial.print(Xpos);
1 Serial.print(",");
2 Serial.print(Ypos);
3 Serial.println("");
4 }
5
6 if (verbose)
7 {
8 Serial.print("x1, y1: ");
9 Serial.print(x1);
0 Serial.print(",");
1 Serial.print(y1);
2 Serial.println("");
3 }
4
5 // Convert coordinates to steps
6 x1 = (int)(x1*StepsPerMillimeterX);
7 y1 = (int)(y1*StepsPerMillimeterY);
8 float x0 = Xpos;
9 float y0 = Ypos;
0
1 // Let’s find out the change for the coordinates
2 long dx = abs(x1-x0);
3 long dy = abs(y1-y0);
4 int sx = x0<x1 ? StepInc : -StepInc;
5 int sy = y0<y1 ? StepInc : -StepInc;
6
7 long i;
8 long over = 0;
9
0 if (dx > dy) {
1 for (i=0; i<dx; ++i) {
2 myStepperX.step(sx);
3 over+=dy;
4 if (over>=dx) {
5 over-=dx;
6 myStepperY.step(sy);
7 }
8 delay(StepDelay);
9 }
0 }
1 else {
2 for (i=0; i<dy; ++i) {
3 myStepperY.step(sy);
4 over+=dx;
5 if (over>=dy) {
6 over-=dy;
7 myStepperX.step(sx);
8 }
9 delay(StepDelay);
0 }
1 }
2
3 if (verbose)
4 {
5 Serial.print("dx, dy:");
6 Serial.print(dx);
7 Serial.print(",");
8 Serial.print(dy);
9 Serial.println("");
0 }
1
2 if (verbose)
3 {
4 Serial.print("Going to (");
5 Serial.print(x0);
6 Serial.print(",");
7 Serial.print(y0);
8 Serial.println(")");
9 }
0
1
2 delay(LineDelay);
3
4 Xpos = x1;
5 Ypos = y1;
6 }
7
8
9 void penUp() {
0 penServo.write(penZUp);
1 delay(LineDelay);
2 Zpos=Zmax;
3 if (verbose) {
4 Serial.println("Pen up!");
5 }
6 }
7 void penDown() {
8 penServo.write(penZDown);
9 delay(LineDelay);
0 Zpos=Zmin;
1 if (verbose) {
2 Serial.println("Pen down.");
3 }
4 }