-
Notifications
You must be signed in to change notification settings - Fork 0
/
robot_arm.py
48 lines (35 loc) · 1.61 KB
/
robot_arm.py
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
#!/usr/bin/python3
from libraries.pca9685 import PCA9685
class RobotArm:
# Servo-Positionen
SERVO_MIN_DEGREES = 0
SERVO_MAX_DEGREES = 180
SERVO_DEFAULT_DEGREES = (SERVO_MAX_DEGREES - SERVO_MIN_DEGREES) / 2
# Die PWM-Frequenz muss für den Servo-Betrieb immer 50 Hz betragen
__SERVO_PWM_FREQ = 50
def __init__(self, servo_count):
self.__pwm = PCA9685()
self.__pwm.setPWMFreq(RobotArm.__SERVO_PWM_FREQ)
self.__servo_count = servo_count
# Servos in Startposition bringen
self.__servo_positions = [RobotArm.SERVO_DEFAULT_DEGREES] * servo_count
for num_servo in range(servo_count):
self.set_servo_position(num_servo, self.__servo_positions[num_servo])
def __convert_degrees_to_pulse(self, degrees):
return degrees * (100 / 9) + 500
def set_servo_position(self, num_servo, degrees):
if (num_servo < 0 or num_servo >= self.__servo_count):
raise ValueError("invalid servo number")
if (degrees < RobotArm.SERVO_MIN_DEGREES or degrees > RobotArm.SERVO_MAX_DEGREES):
raise ValueError("invalid position")
pulse = self.__convert_degrees_to_pulse(degrees)
self.__pwm.setServoPulse(num_servo, pulse)
# Neue Position im Array übernehmen
self.__servo_positions[num_servo] = degrees
def get_servo_position(self, num_servo):
if (num_servo < 0 or num_servo >= self.__servo_count):
raise ValueError("invalid servo number")
return self.__servo_positions[num_servo]
@property
def servo_count(self):
return self.__servo_count