daily pastebin goal
18%
SHARE
TWEET

Untitled

a guest Mar 22nd, 2018 57 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. #include <Servo.h>
  3. #include <AFMotor.h>
  4.  
  5. #define LINE_BUFFER_LENGTH 512
  6.  
  7. char STEP = MICROSTEP ;
  8.  
  9. // Servo position for Up and Down
  10. const int penZUp = 115;
  11. const int penZDown = 83;
  12.  
  13. // Servo on PWM pin 10
  14. const int penServoPin =10 ;
  15.  
  16. // Should be right for DVD steppers, but is not too important here
  17. const int stepsPerRevolution = 48;
  18.  
  19. // create servo object to control a servo
  20. Servo penServo;  
  21.  
  22. // Initialize steppers for X- and Y-axis using this Arduino pins for the L293D H-bridge
  23. AF_Stepper myStepperY(stepsPerRevolution,1);            
  24. AF_Stepper myStepperX(stepsPerRevolution,2);  
  25.  
  26. /* Structures, global variables    */
  27. struct point {
  28.   float x;
  29.   float y;
  30.   float z;
  31. };
  32.  
  33. // Current position of plothead
  34. struct point actuatorPos;
  35.  
  36. //  Drawing settings, should be OK
  37. float StepInc = 1;
  38. int StepDelay = 0;
  39. int LineDelay =0;
  40. int penDelay = 50;
  41.  
  42. // Motor steps to go 1 millimeter.
  43. // Use test sketch to go 100 steps. Measure the length of line.
  44. // Calculate steps per mm. Enter here.
  45. float StepsPerMillimeterX = 100.0;
  46. float StepsPerMillimeterY = 100.0;
  47.  
  48. // Drawing robot limits, in mm
  49. // OK to start with. Could go up to 50 mm if calibrated well.
  50. float Xmin = 0;
  51. float Xmax = 40;
  52. float Ymin = 0;
  53. float Ymax = 40;
  54. float Zmin = 0;
  55. float Zmax = 1;
  56.  
  57. float Xpos = Xmin;
  58. float Ypos = Ymin;
  59. float Zpos = Zmax;
  60.  
  61. // Set to true to get debug output.
  62. boolean verbose = false;
  63.  
  64. //  Needs to interpret
  65. //  G1 for moving
  66. //  G4 P300 (wait 150ms)
  67. //  M300 S30 (pen down)
  68. //  M300 S50 (pen up)
  69. //  Discard anything with a (
  70. //  Discard any other command!
  71.  
  72. /**********************
  73.  * void setup() - Initialisations
  74.  ***********************/
  75. void setup() {
  76.   //  Setup
  77.  
  78.   Serial.begin( 9600 );
  79.  
  80.   penServo.attach(penServoPin);
  81.   penServo.write(penZUp);
  82.   delay(100);
  83.  
  84.   // Decrease if necessary
  85.   myStepperX.setSpeed(600);
  86.  
  87.   myStepperY.setSpeed(600);  
  88.  
  89.  
  90.   //  Set & move to initial default position
  91.   // TBD
  92.  
  93.   //  Notifications!!!
  94.   Serial.println("Mini CNC Plotter alive and kicking!");
  95.   Serial.print("X range is from ");
  96.   Serial.print(Xmin);
  97.   Serial.print(" to ");
  98.   Serial.print(Xmax);
  99.   Serial.println(" mm.");
  100.   Serial.print("Y range is from ");
  101.   Serial.print(Ymin);
  102.   Serial.print(" to ");
  103.   Serial.print(Ymax);
  104.   Serial.println(" mm.");
  105. }
  106.  
  107. /**********************
  108.  * void loop() - Main loop
  109.  ***********************/
  110. void loop()
  111. {
  112.  
  113.   delay(100);
  114.   char line[ LINE_BUFFER_LENGTH ];
  115.   char c;
  116.   int lineIndex;
  117.   bool lineIsComment, lineSemiColon;
  118.  
  119.   lineIndex = 0;
  120.   lineSemiColon = false;
  121.   lineIsComment = false;
  122.  
  123.   while (1) {
  124.  
  125.     // Serial reception - Mostly from Grbl, added semicolon support
  126.     while ( Serial.available()>0 ) {
  127.       c = Serial.read();
  128.       if (( c == '\n') || (c == '\r') ) {             // End of line reached
  129.         if ( lineIndex > 0 ) {                        // Line is complete. Then execute!
  130.           line[ lineIndex ] = '\0';                   // Terminate string
  131.           if (verbose) {
  132.             Serial.print( "Received : ");
  133.             Serial.println( line );
  134.           }
  135.           processIncomingLine( line, lineIndex );
  136.           lineIndex = 0;
  137.         }
  138.         else {
  139.           // Empty or comment line. Skip block.
  140.         }
  141.         lineIsComment = false;
  142.         lineSemiColon = false;
  143.         Serial.println("ok");    
  144.       }
  145.       else {
  146.         if ( (lineIsComment) || (lineSemiColon) ) {   // Throw away all comment characters
  147.           if ( c == ')' )  lineIsComment = false;     // End of comment. Resume line.
  148.         }
  149.         else {
  150.           if ( c <= ' ' ) {                           // Throw away whitepace and control characters
  151.           }
  152.           else if ( c == '/' ) {                    // Block delete not supported. Ignore character.
  153.           }
  154.           else if ( c == '(' ) {                    // Enable comments flag and ignore all characters until ')' or EOL.
  155.             lineIsComment = true;
  156.           }
  157.           else if ( c == ';' ) {
  158.             lineSemiColon = true;
  159.           }
  160.           else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) {
  161.             Serial.println( "ERROR - lineBuffer overflow" );
  162.             lineIsComment = false;
  163.             lineSemiColon = false;
  164.           }
  165.           else if ( c >= 'a' && c <= 'z' ) {        // Upcase lowercase
  166.             line[ lineIndex++ ] = c-'a'+'A';
  167.           }
  168.           else {
  169.             line[ lineIndex++ ] = c;
  170.           }
  171.         }
  172.       }
  173.     }
  174.   }
  175. }
  176.  
  177. void processIncomingLine( char* line, int charNB ) {
  178.   int currentIndex = 0;
  179.   char buffer[ 64 ];                                 // Hope that 64 is enough for 1 parameter
  180.   struct point newPos;
  181.  
  182.   newPos.x = 0.0;
  183.   newPos.y = 0.0;
  184.  
  185.   //  Needs to interpret
  186.   //  G1 for moving
  187.   //  G4 P300 (wait 150ms)
  188.   //  G1 X60 Y30
  189.   //  G1 X30 Y50
  190.   //  M300 S30 (pen down)
  191.   //  M300 S50 (pen up)
  192.   //  Discard anything with a (
  193.   //  Discard any other command!
  194.  
  195.   while( currentIndex < charNB ) {
  196.     switch ( line[ currentIndex++ ] ) {              // Select command, if any
  197.     case 'U':
  198.       penUp();
  199.       break;
  200.     case 'D':
  201.       penDown();
  202.       break;
  203.     case 'G':
  204.       buffer[0] = line[ currentIndex++ ];          // /!\ Dirty - Only works with 2 digit commands
  205.       //      buffer[1] = line[ currentIndex++ ];
  206.       //      buffer[2] = '\0';
  207.       buffer[1] = '\0';
  208.  
  209.       switch ( atoi( buffer ) ){                   // Select G command
  210.       case 0:                                   // G00 & G01 - Movement or fast movement. Same here
  211.       case 1:
  212.         // /!\ Dirty - Suppose that X is before Y
  213.         char* indexX = strchr( line+currentIndex, 'X' );  // Get X/Y position in the string (if any)
  214.         char* indexY = strchr( line+currentIndex, 'Y' );
  215.         if ( indexY <= 0 ) {
  216.           newPos.x = atof( indexX + 1);
  217.           newPos.y = actuatorPos.y;
  218.         }
  219.         else if ( indexX <= 0 ) {
  220.           newPos.y = atof( indexY + 1);
  221.           newPos.x = actuatorPos.x;
  222.         }
  223.         else {
  224.           newPos.y = atof( indexY + 1);
  225.           indexY = '\0';
  226.           newPos.x = atof( indexX + 1);
  227.         }
  228.         drawLine(newPos.x, newPos.y );
  229.         //        Serial.println("ok");
  230.         actuatorPos.x = newPos.x;
  231.         actuatorPos.y = newPos.y;
  232.         break;
  233.       }
  234.       break;
  235.     case 'M':
  236.       buffer[0] = line[ currentIndex++ ];        // /!\ Dirty - Only works with 3 digit commands
  237.       buffer[1] = line[ currentIndex++ ];
  238.       buffer[2] = line[ currentIndex++ ];
  239.       buffer[3] = '\0';
  240.       switch ( atoi( buffer ) ){
  241.       case 300:
  242.         {
  243.           char* indexS = strchr( line+currentIndex, 'S' );
  244.           float Spos = atof( indexS + 1);
  245.           //         Serial.println("ok");
  246.           if (Spos == 30) {
  247.             penDown();
  248.           }
  249.           if (Spos == 50) {
  250.             penUp();
  251.           }
  252.           break;
  253.         }
  254.       case 114:                                // M114 - Repport position
  255.         Serial.print( "Absolute position : X = " );
  256.         Serial.print( actuatorPos.x );
  257.         Serial.print( "  -  Y = " );
  258.         Serial.println( actuatorPos.y );
  259.         break;
  260.       default:
  261.         Serial.print( "Command not recognized : M");
  262.         Serial.println( buffer );
  263.       }
  264.     }
  265.   }
  266.  
  267.  
  268.  
  269. }
  270.  
  271.  
  272. /*********************************
  273.  * Draw a line from (x0;y0) to (x1;y1).
  274.  * int (x1;y1) : Starting coordinates
  275.  * int (x2;y2) : Ending coordinates
  276.  **********************************/
  277. void drawLine(float x1, float y1) {
  278.  
  279.   if (verbose)
  280.   {
  281.     Serial.print("fx1, fy1: ");
  282.     Serial.print(x1);
  283.     Serial.print(",");
  284.     Serial.print(y1);
  285.     Serial.println("");
  286.   }  
  287.  
  288.   //  Bring instructions within limits
  289.   if (x1 >= Xmax) {
  290.     x1 = Xmax;
  291.   }
  292.   if (x1 <= Xmin) {
  293.     x1 = Xmin;
  294.   }
  295.   if (y1 >= Ymax) {
  296.     y1 = Ymax;
  297.   }
  298.   if (y1 <= Ymin) {
  299.     y1 = Ymin;
  300.   }
  301.  
  302.   if (verbose)
  303.   {
  304.     Serial.print("Xpos, Ypos: ");
  305.     Serial.print(Xpos);
  306.     Serial.print(",");
  307.     Serial.print(Ypos);
  308.     Serial.println("");
  309.   }
  310.  
  311.   if (verbose)
  312.   {
  313.     Serial.print("x1, y1: ");
  314.     Serial.print(x1);
  315.     Serial.print(",");
  316.     Serial.print(y1);
  317.     Serial.println("");
  318.   }
  319.  
  320.   //  Convert coordinates to steps
  321.   x1 = (int)(x1*StepsPerMillimeterX);
  322.   y1 = (int)(y1*StepsPerMillimeterY);
  323.   float x0 = Xpos;
  324.   float y0 = Ypos;
  325.  
  326.   //  Let's find out the change for the coordinates
  327.   long dx = abs(x1-x0);
  328.   long dy = abs(y1-y0);
  329.   int sx = x0<x1 ? StepInc : -StepInc;
  330.   int sy = y0<y1 ? StepInc : -StepInc;
  331.  
  332.   long i;
  333.   long over = 0;
  334.  
  335.   if (dx > dy) {
  336.     for (i=0; i<dx; ++i) {
  337.       myStepperX.onestep(sx,STEP);
  338.       over+=dy;
  339.       if (over>=dx) {
  340.         over-=dx;
  341.         myStepperY.onestep(sy,STEP);
  342.       }
  343.     delay(StepDelay);
  344.     }
  345.   }
  346.   else {
  347.     for (i=0; i<dy; ++i) {
  348.       myStepperY.onestep(sy,STEP);
  349.       over+=dx;
  350.       if (over>=dy) {
  351.         over-=dy;
  352.         myStepperX.onestep(sx,STEP);
  353.       }
  354.       delay(StepDelay);
  355.     }    
  356.   }
  357.  
  358.   if (verbose)
  359.   {
  360.     Serial.print("dx, dy:");
  361.     Serial.print(dx);
  362.     Serial.print(",");
  363.     Serial.print(dy);
  364.     Serial.println("");
  365.   }
  366.  
  367.   if (verbose)
  368.   {
  369.     Serial.print("Going to (");
  370.     Serial.print(x0);
  371.     Serial.print(",");
  372.     Serial.print(y0);
  373.     Serial.println(")");
  374.   }
  375.  
  376.   //  Delay before any next lines are submitted
  377.   delay(LineDelay);
  378.   //  Update the positions
  379.   Xpos = x1;
  380.   Ypos = y1;
  381. }
  382.  
  383. //  Raises pen
  384. void penUp() {
  385.   penServo.write(penZUp);
  386.   delay(penDelay);
  387.   Zpos=Zmax;
  388.   digitalWrite(15, LOW);
  389.     digitalWrite(16, HIGH);
  390.   if (verbose) {
  391.     Serial.println("Pen up!");
  392.    
  393.   }
  394. }
  395. //  Lowers pen
  396. void penDown() {
  397.   penServo.write(penZDown);
  398.   delay(penDelay);
  399.   Zpos=Zmin;
  400.   digitalWrite(15, HIGH);
  401.     digitalWrite(16, LOW);
  402.   if (verbose) {
  403.     Serial.println("Pen down.");
  404.    
  405.    
  406.   }
  407. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top