1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
| #include <Servo.h>
#include <AccelStepper.h>
// 定义舵机对象及其参数
Servo myServo;
const int servoPin = 21;
const int openAngle = 150;
const int closeAngle = 20;
const int step = 5;
const int delayTime = 20;
// 步进电机引脚宏定义
#define Y_STEP_PIN 3
#define Y_DIR_PIN 6
#define ENABLE_PIN 8
#define X_STEP_PIN 2 // 光轴步进脉冲引脚
#define X_DIR_PIN 5 // 光轴方向控制引脚
// 运动参数定义
#define STEPS_PER_CM 1580
#define MAX_SPEED 30000
#define ACCELERATION 17000
// 创建步进电机对象
AccelStepper module(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper lightAxis(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
// 记录模块和光轴当前位置，初始化为原点
long modulePos = 0;
long lightPos = 0;
// 角度限制函数
int constrainAngle(int angle) {
return (angle < 0) ? 0 : ((angle > 180) ? 180 : angle);
}
// 机械臂打开函数
void openArm() {
int currentAngle = myServo.read();
int targetAngle = constrainAngle(openAngle);
if (currentAngle < targetAngle) {
for (int angle = currentAngle; angle <= targetAngle; angle += step) {
myServo.write(angle);
delay(delayTime);
}
}
}
// 机械臂闭合函数
void closeArm() {
int currentAngle = myServo.read();
int targetAngle = constrainAngle(closeAngle);
if (currentAngle > targetAngle) {
for (int angle = currentAngle; angle >= targetAngle; angle -= step) {
myServo.write(angle);
delay(delayTime);
}
}
}
// 执行单个步骤函数（移除指针，直接操作全局变量）
void runStep(AccelStepper& motor, long distance, long& position, unsigned long delayTime) {
long target = (position + distance) * STEPS_PER_CM;
motor.moveTo(target);
while (motor.distanceToGo()) {
motor.run();
}
position += distance;
delay(delayTime);
}
// 整合所有运动的函数
void executeMotionSequence() {
// 1. 模组向前运动12cm
runStep(module, 12, modulePos, 1500);
Serial.println("模组向前运动12厘米完成");
// 2. 机械臂关闭
openArm();
Serial.println("机械臂关闭完成");
// 3. 光轴向上8cm（原代码中写的是8，保持与loop中一致）
runStep(lightAxis, 8, lightPos, 1500);
Serial.println("光轴向上8厘米完成");
// 4. 模组向后运动12cm
runStep(module, -12, modulePos, 1500);
Serial.println("模组向后运动12厘米完成");
// 5. 机械臂关闭
closeArm();
Serial.println("机械臂打开");
// 6. 光轴向下8cm
runStep(lightAxis, -8, lightPos, 1500);
Serial.println("光轴向下8厘米完成");
}
// 初始化设置
void setup() {
pinMode(ENABLE_PIN, OUTPUT);
digitalWrite(ENABLE_PIN, LOW);
module.setMaxSpeed(MAX_SPEED);
module.setAcceleration(ACCELERATION);
//module.setCurrentPosition(0);
lightAxis.setMaxSpeed(MAX_SPEED);
lightAxis.setAcceleration(ACCELERATION);
//lightAxis.setCurrentPosition(0);
myServo.attach(servoPin);
//closeArm(); // 初始时机械臂关闭
Serial.begin(9600);
Serial1.begin(9600);
}
// 主循环：执行一次整合函数
void loop() {
if(Serial1.read() == 'A'){
Serial.println('1');
executeMotionSequence();
}
}
|
