CNC Flatbed Pen Plotter

1/*
2  CNC Flatbed Pen Plotter
3  https://create.arduino.cc/vintproykt/cnc-flatbed-pen-plotter-1780c9
4*/
5
6#include  <AccelStepper.h>
7#include <MultiStepper.h>
8
9#define MOTORS_MAXSPEED 1500
10#define  MOTORS_ACCELERATION 100
11#define MOTORS_STEPS_PER_REVOLUTION 4096 // 28BYJ-48  in half-step mode
12
13AccelStepper motorX(AccelStepper::HALF4WIRE, 2, 3, 4, 5);
14AccelStepper  motorY(AccelStepper::HALF4WIRE, 6, 7, 8, 9);
15AccelStepper motorZ(AccelStepper::HALF4WIRE,  10, 11, 12, 13);
16
17MultiStepper motors;
18
19short motors_dir = 1;
20int  motors_avgSpeed = MOTORS_MAXSPEED;// Average speed
21
22String input = "";
23
24void  doCommand(String command, String param) {// Serial commands hub
25  if (command  == "go") {
26    long steps = param.toInt();
27    if (steps == 0) steps = MOTORS_STEPS_PER_REVOLUTION;//  Make full revolution by default
28    _go(steps);
29  }
30  else if (command  == "stop") _stop();
31  else if (command == "dir") _dir();
32  else if (command  == "speed") {
33    float speed = param.toInt();
34    if (speed == 0) speed  = MOTORS_MAXSPEED;// Average speed equals maximum speed by default
35    _speed(speed);
36  }
37}
38
39void setup() {
40  Serial.begin(115200);// Maximum baud rate for  Arduino Uno
41  Serial.println("Запуск");
42  
43  input.reserve(128);
44  
45  motorX.setMaxSpeed(MOTORS_MAXSPEED);
46  motorY.setMaxSpeed(MOTORS_MAXSPEED);
47  motorZ.setMaxSpeed(MOTORS_MAXSPEED);
48  motorX.setAcceleration(MOTORS_ACCELERATION);
49  motorY.setAcceleration(MOTORS_ACCELERATION);
50  motorZ.setAcceleration(MOTORS_ACCELERATION);
51  motors.addStepper(motorX);
52  motors.addStepper(motorY);
53  motors.addStepper(motorZ);
54}
55
56void  loop() {
57  while (Serial.available()) {
58    input = Serial.readStringUntil('\n');
59    input.trim();
60    int input_sp = input.indexOf(' ');// Checking for space  character in input string
61    String input_command = input;
62    String input_param;
63    if (input_sp > 0) {
64      input_command = input.substring(0, input_sp);
65      input_param = input.substring(input_sp + 1);
66      input_param.trim();
67    }
68    doCommand(input_command, input_param);
69    input = "";
70  }
71  
72  measureSteps();
73  
74  motors.run();
75}
76
77void _go(long increment)  {
78  if (motorX.speed() == 0) {
79    motorX.setSpeed(motors_avgSpeed);
80    motorY.setSpeed(motors_avgSpeed);
81    motorZ.setSpeed(motors_avgSpeed);
82    motors.moveTo(motorX.currentPosition()  + motors_dir * increment);
83    Serial.print("Move to ");
84    Serial.print(motorX.targetPosition());
85    Serial.print(" (rotate by ");
86    Serial.print((float)increment / (float)MOTORS_STEPS_PER_REVOLUTION  * 360);
87    Serial.print("° ");
88    if (motors_dir < 0) Serial.print("counter");
89    Serial.print("clockwise");
90    Serial.println(")");
91  }
92}
93
94void  _stop() {
95  motors.moveTo(motorX.currentPosition());
96  motorX.setSpeed(0);
97  motorY.setSpeed(0);
98  motorZ.setSpeed(0);
99  motorX.stop();
100  motorY.stop();
101  motorZ.stop();
102  Serial.println("Stopped");
103}
104
105void _dir() {
106  motors_dir = -motors_dir;
107  if (motors_dir > 0) Serial.println("CW");
108  else Serial.println("CCW");
109}
110
111void _speed(float speed) {
112  motors_avgSpeed  = speed;
113  Serial.print("Average speed changed to ");
114  Serial.println(speed);
115}
116
117void  measureSteps() {
118  if (motorX.speed() != 0) {
119    Serial.print("Position:  ");
120    Serial.print(motorX.currentPosition());
121    Serial.print(" | Speed:  ");
122    Serial.print(motorX.speed());
123    Serial.println(" steps/s");
124  }
125}
126