-
Notifications
You must be signed in to change notification settings - Fork 4
Expand file tree
/
Copy pathRobotArm.py
More file actions
122 lines (95 loc) · 3.75 KB
/
RobotArm.py
File metadata and controls
122 lines (95 loc) · 3.75 KB
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
import pyfirmata
import time
class RobotArm(object):
theta0 = 90.0;
theta1 = 90.0;
theta2 = 90.0;
theta3 = 0.0;
board = None
servo0 = None
servo1 = None
servo2 = None
servo3 = None
servo4 = None
def __init__(self):
print("in __init__")
self.board = pyfirmata.Arduino('/dev/ttyACM0')
self.board.servo_config(3, angle = self.theta0)
self.servo0 = self.board.get_pin('d:3:s')
self.servo0.write(self.theta0)
self.board.servo_config(5, angle = self.theta1)
self.servo1 = self.board.get_pin('d:5:s')
self.servo1.write(self.theta1)
self.board.servo_config(6, angle = self.theta2)
self.servo2 = self.board.get_pin('d:6:s')
self.servo2.write(self.theta2)
self.board.servo_config(9, angle = self.theta3)
self.servo3 = self.board.get_pin('d:9:s')
self.servo3.write(self.theta3)
self.board.servo_config(10, angle = 90.0)
self.servo4 = self.board.get_pin('d:10:s')
self.servo4.write(90.0)
def __del__(self):
print("in __del__")
self.goto(90, 90 ,90)
self.closeHand()
self.closeGate()
self.board.exit()
def set0(self, theta0_desired):
theta0_desired = max(theta0_desired, 0.0)
theta0_desired = min(theta0_desired, 180.0)
while abs(self.theta0-theta0_desired)>0.1:
if self.theta0 <= theta0_desired:
self.theta0 = self.theta0 + 1.0
elif self.theta0 > theta0_desired:
self.theta0 = self.theta0 - 1.0
self.servo0.write(self.theta0)
time.sleep(0.015)
print(str(self.theta0) + "," + str(self.theta1) + "," + str(self.theta2))
def set1(self, theta1_desired):
theta1_desired = max(theta1_desired, 0.0)
theta1_desired = min(theta1_desired, 180.0)
while abs(self.theta1-theta1_desired)>0.1:
if self.theta1 <= theta1_desired:
self.theta1 = self.theta1 + 1.0
elif self.theta1 > theta1_desired:
self.theta1 = self.theta1 - 1.0
self.servo1.write(self.theta1)
time.sleep(0.015)
print(str(self.theta0) + "," + str(self.theta1) + "," + str(self.theta2))
def set2(self, theta2_desired):
theta2_desired = max(theta2_desired, 0.0)
theta2_desired = min(theta2_desired, 180.0)
while abs(self.theta2-theta2_desired)>0.1:
if self.theta2 <= theta2_desired:
self.theta2 = self.theta2 + 1.0
elif self.theta2 > theta2_desired:
self.theta2 = self.theta2 - 1.0
self.servo2.write(self.theta2)
time.sleep(0.015)
print(str(self.theta0) + "," + str(self.theta1) + "," + str(self.theta2))
def set3(self, theta3_desired):
theta3_desired = max(theta3_desired, 0.0)
theta3_desired = min(theta3_desired, 90.0)
while abs(self.theta3-theta3_desired)>0.1:
if self.theta3 <= theta3_desired:
self.theta3 = self.theta3 + 1.0
elif self.theta3 > theta3_desired:
self.theta3 = self.theta3 - 1.0
self.servo3.write(self.theta3)
time.sleep(0.015)
def goto(self, theta0_desired, theta1_desired, theta2_desired):
self.set0(theta0_desired)
self.set1(theta1_desired)
self.set2(theta2_desired)
print(str(self.theta0) + "," + str(self.theta1) + "," + str(self.theta2))
time.sleep(0.2)
def openHand(self):
self.set3(30.0)
def closeHand(self):
self.set3(0)
def openGate(self):
self.servo4.write(0)
time.sleep(0.500)
def closeGate(self):
self.servo4.write(90)