ペンプロッタ自作（３）～コード編

１．Arduino

ペンプロッタではモータの制御にArduinoを使用しています。構成はこちらのとおり

Arduinoのプログラムを以下に載せます。A4988ライブラリを追加しています。

#include <Servo.h>
#include <A4988.h>

#define LINE_BUFFER_LENGTH 512

// Servo position for Up and Down 
const int penZUp = 30;
const int penZDown = 90;

// Servo on PWM pin 6
const int penServoPin = 6;

// Should be right for DVD steppers, but is not too important here
//const int stepsPerRevolution = 20; 
const int stepsPerRevolution = 10; 

// create servo object to control a servo 
Servo penServo;

const int MOTOR_STEPS = 20;
A4988 stepperX(MOTOR_STEPS,2,3);
A4988 stepperY(MOTOR_STEPS,8,9);

/* 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;
float StepInc = 50;
int StepDelay = 0;
int LineDelay = 50;
int penDelay = 50;
int rpm = 5000;

// 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 = 6.0;
//float StepsPerMillimeterY = 6.0;
float StepsPerMillimeterX = 1.4;
float StepsPerMillimeterY = 1.4;

// 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 Xmin = 0;
float Xmax = 100;
float Ymin = 0;
float Ymax = 100;
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 );

 stepperX.begin(rpm, 00);
 stepperY.begin(rpm, 90);
 
 penServo.attach(penServoPin);
 penServo.write(penZUp);
 delay(100);

 //  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);
         if (Spos == 30) { 
           penDown(); 
         }
         if (Spos == 50) { 
           penUp(); 
         }
         break;
       }
     case 114:                                // M114 - Repport position
       Serial.print( "Absolute position : X = " );
       Serial.print( actuatorPos.x );
       Serial.print( "  -  Y = " );
       Serial.println( actuatorPos.y );
       break;
     default:
       Serial.print( "Command not recognized : M");
       Serial.println( buffer );
     }
   }
 }
}


/*********************************
* Draw a line from (x0;y0) to (x1;y1). 
**********************************/
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) {
     stepperX.rotate(sx*5);
     over+=dy;
     if (over>=dx) {
       over-=dx;
       stepperY.rotate(sy*5);
     }
     delay(StepDelay);
   }
 }
 else {
   for (i=0; i<dy; ++i) {
     //myStepperY.step(sy);
     stepperY.rotate(sy*5);
     over+=dx;
     if (over>=dy) {
       over-=dy;
       stepperX.rotate(sx*5);
     }
     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(LineDelay); 
 Zpos=Zmax; 
 if (verbose) { 
   Serial.println("Pen up!"); 
 } 
}
//  Lowers pen
void penDown() { 
 penServo.write(penZDown); 
 delay(LineDelay); 
 Zpos=Zmin; 
 if (verbose) { 
   Serial.println("Pen down."); 
 } 
}

いろいろなところのつぎはぎで作っているので、文法とかめちゃくちゃですが見逃してください。

rpmの値を変えると描画速度が変わります。
Xmax, Ymaxは100にしないと図が切れてしまいます。実測では40mm程度でした。そのぶんStepsPerMillimeterX, StepsPerMillimeterYで図の大きさを調整しています。

これをArduinoに送ったあとに、Processingで図のG-codeデータを送ります。

２．Processing

次にProcessingでArduinoにG-codeを送ります。Processingで使用したプログラムは下のサイトのgctrl.pdeというものを使用しました。

How to Make Mini CNC 2D Plotter Using Scrap DVD Drive, L293d Motor Shield & Arduino

Processingで実行するとウインドウが開いて、「p」でポートを選択、「g」でGcodeファイルを選択して送り、描画を開始します。

Processing3.5.4では動きましたが、ver.4.0では動きませんでした。Processingの旧verを探すのが一番時間かかったかもしれません。