1
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
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;
21
22String input = "";
23
24void doCommand(String command, String param) {
25 if (command == "go") {
26 long steps = param.toInt();
27 if (steps == 0) steps = MOTORS_STEPS_PER_REVOLUTION;
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;
35 _speed(speed);
36 }
37}
38
39void setup() {
40 Serial.begin(115200);
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(' ');
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