-
Notifications
You must be signed in to change notification settings - Fork 0
/
sample_code.ino
54 lines (45 loc) · 1.49 KB
/
sample_code.ino
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
#include <Servo.h>
// Define servo pins
#define BASE_SERVO_PIN 9
#define SHOULDER_SERVO_PIN 10
#define ELBOW_SERVO_PIN 11
#define GRIPPER_SERVO_PIN 6
// Define servo angles limits
#define BASE_MIN_ANGLE 0
#define BASE_MAX_ANGLE 180
#define SHOULDER_MIN_ANGLE 0
#define SHOULDER_MAX_ANGLE 180
#define ELBOW_MIN_ANGLE 0
#define ELBOW_MAX_ANGLE 180
#define GRIPPER_MIN_ANGLE 0
#define GRIPPER_MAX_ANGLE 180
// Create servo objects
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo gripperServo;
void setup() {
// Attach servos to pins
baseServo.attach(BASE_SERVO_PIN);
shoulderServo.attach(SHOULDER_SERVO_PIN);
elbowServo.attach(ELBOW_SERVO_PIN);
gripperServo.attach(GRIPPER_SERVO_PIN);
}
void loop() {
// Move the arm to various positions
moveArm(90, 90, 90, 90); // Example position, adjust as needed
delay(1000); // Wait for 1 second
// Add more positions and delays here as needed
}
void moveArm(int baseAngle, int shoulderAngle, int elbowAngle, int gripperAngle) {
// Constrain angles within limits
baseAngle = constrain(baseAngle, BASE_MIN_ANGLE, BASE_MAX_ANGLE);
shoulderAngle = constrain(shoulderAngle, SHOULDER_MIN_ANGLE, SHOULDER_MAX_ANGLE);
elbowAngle = constrain(elbowAngle, ELBOW_MIN_ANGLE, ELBOW_MAX_ANGLE);
gripperAngle = constrain(gripperAngle, GRIPPER_MIN_ANGLE, GRIPPER_MAX_ANGLE);
// Move servos to specified angles
baseServo.write(baseAngle);
shoulderServo.write(shoulderAngle);
elbowServo.write(elbowAngle);
gripperServo.write(gripperAngle);
}