版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-09 https://www.robotway.com/

  ------------------------------*/

#include <Servo.h>

/*

* en：定义步进电机使能引脚

* servo_pin：定义舵机引脚

* stepper_count：定义步进电机数量

* stepperPulse_delay：定义步进电机脉冲生成间隔

* LINE_BUFFER_LENGTH：定义串口接收数据长度

*/

#define en 8

#define servo_pin 11

#define stepper_count 4

#define stepperPulse_delay 850

#define LINE_BUFFER_LENGTH 512

/*

* positive_x：定义正向 X 轴

* positive_y：定义正向 Y 轴

* negative_x：定义负向 X 轴

* negative_y：定义负向 Y 轴

*/

#define positive_x 0

#define positive_y 1

#define negative_x 2

#define negative_y 3

/*

* stepperDir_pin：定义步进电机方向引脚数组

* stepperStp_pin：定义步进电机步进引脚数组

* dir: x: 5, y: 6, z: 7, a: 13

* stp: x: 2, y: 3, z: 4, a: 12

*/

int stepperDir_pin[4] = {5, 6, 7, 13};

int stepperStp_pin[4] = {2, 3, 4, 12};

Servo myServo;

const int stepsPerRevolution = 3200;   //定义步进电机每圈转动的步数，此处为16细分，每圈 3200 步

int penZup = 145;                      //定义舵机抬起角度

int penZdown = 150;                    //定义舵机放下角度

float LEAD = sqrt(2) * 58 * PI;        //定义步进电机转动 1 圈，小车前进的距离，单位 mm

struct point {

  float x;

  float y;  

};

struct point actuatorPos;             // Current position of plothead

float Xmin = -60;                     //定义绘图范围，长120mm , 宽120mm

float Xmax = 60;

float Ymin = -60;

float Ymax = 60;

float Xpos = 0;                       

float Ypos = 0;

boolean verbose = true;

void setup() {

  Serial.begin(9600);               //开启串口通信，波特率为 9600

  myServo.attach(servo_pin);

  myServo.write(penZup);

  for(int i=0;i<stepper_count;i++)

  {

    pinMode(stepperDir_pin[i], OUTPUT);

    pinMode(stepperStp_pin[i], OUTPUT);  

  }

  pinMode(en, OUTPUT);

  digitalWrite(en, LOW);

  delay(1000);

}

void loop()

{

  delay(200);

  char line[ LINE_BUFFER_LENGTH ];

  char c;

  int lineIndex;

  bool lineIsComment, lineSemiColon;

  lineIndex = 0;

  lineSemiColon = false;

  lineIsComment = false;

  while (1) {

    // 接受来自Grbl的串口数据

    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 == 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 );

      }

    }

  }

}

//直线插补函数，参数为点坐标值

void drawLine(float x1, float y1)

{

  int dx, dy, n, k, i, f, stepInc;

  if (x1 >= Xmax) {

    x1 = Xmax;

  }

  if (x1 <= Xmin) {

    x1 = Xmin;

  }

  if (y1 >= Ymax) {

    y1 = Ymax;

  }

  if (y1 <= Ymin) {

    y1 = Ymin;

  }

  x1 = (int)(x1/LEAD*stepsPerRevolution);

  y1 = (int)(y1/LEAD*stepsPerRevolution);

  float x0 = Xpos;

  float y0 = Ypos;

  Serial.print("X = ");

  Serial.println(Xpos);

  Serial.print("Y = ");

  Serial.println(Ypos);

  dx = abs(x1-x0);

  dy = abs(y1-y0);

  n = abs(dx+dy);

  if(x1 >= x0)

  {

    k = y1 >= y0 ? 1:4;

  }

  else

  {

    k = y1 >= y0 ? 2:3;

  }

  for(i=0,f=0;i<n;i+=1)

  {

    if(f>=0)

    {

      switch(k)

      {

         case 1:

         stepper_move(positive_x, 1);

         f = f - dy;

         //Serial.println("+x");

         break;

         case 2:

         stepper_move(negative_x, 1);

         f = f - dy;

         //Serial.println("-x");

         break;

         case 3:

         stepper_move(negative_x, 1);

         f = f - dy;

         //Serial.println("-x");

         break;

         case 4:

         stepper_move(positive_x, 1);

         f = f - dy;

         //Serial.println("+x");

         break;

         default:break;

      }

    }

    else

    {

      switch(k)

      {

        case 1:

        stepper_move(positive_y, 1);

        f = f + dx;

        //Serial.println("+y");

        break;

        case 2:

        stepper_move(positive_y, 1);

        f = f + dx;

        //Serial.println("+y");

        break;

        case 3:

        stepper_move(negative_y, 1);

        f = f + dx;

        //Serial.println("-y");

        break;

        case 4:

        stepper_move(negative_y, 1);

        f = f +dx;

        //Serial.println("-y");

        break;

        default:break;

      }

    }

  }

  Xpos = x1;

  Ypos = y1;

}

//小车行进方向控制函数

void stepper_dir(int positiveDir_x, int positiveDir_y, int negativeDir_x, int negativeDir_y)

{

  int dir_value[] = {positiveDir_x, positiveDir_y, negativeDir_x, negativeDir_y};

  for(int i=0;i<stepper_count;i++)

  {

    //Serial.print(dir_value[i]);

    //Serial.print(",");

    digitalWrite(stepperDir_pin[i], dir_value[i]);

  }

  //Serial.println();

  for(int j=0;j<stepper_count;j++)

  {

    digitalWrite(stepperStp_pin[j], HIGH);

  }  

  delayMicroseconds(stepperPulse_delay);

  for(int j=0;j<stepper_count;j++)

  {

    digitalWrite(stepperStp_pin[j], LOW);

  }

  delayMicroseconds(stepperPulse_delay);

}

//步进电机转动函数，参数 dir_xy：步进电机转动方向，steps：步进电机转动步数

void stepper_move(int dir_xy, int steps)

{

  for(int i=0;i<abs(steps);i++)

  {

    switch(dir_xy)

    {

      case 0:

      stepper_dir(1, 1, 0, 0);   // X 正方向

      break;

      case 1:

      stepper_dir(1, 0, 1, 0);   // Y 正方向

      break;

      case 2:

      stepper_dir(0, 0, 1, 1);   // X 负方向

      break;

      case 3:

      stepper_dir(0, 1, 0, 1);   // Y 负方向

      break;

      default:break;  

    }  

  }

}

//舵机抬笔函数

void penUp()

{

  myServo.write(penZup);

}

//舵机落笔函数

void penDown()

{

  myServo.write(penZdown);

}