-
Notifications
You must be signed in to change notification settings - Fork 1
/
servo.py
79 lines (66 loc) · 2.44 KB
/
servo.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
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
from kipr import msleep, get_servo_position, set_servo_position
import constants as c
import drive
def move(port: int, end_position: int, speed: int = 25):
"""
For speed, 1 is very slow, and 100 is very fast
"""
speed = abs(speed)
current_position = get_servo_position(port)
if current_position > end_position:
speed = -speed
while current_position != end_position:
if abs(speed) > abs(end_position - current_position):
current_position = end_position
else:
current_position += speed
set_servo_position(port, current_position)
msleep(25)
def move_parallel(arm_end_position: int, speed: int = 15):
"""
For speed, 1 is very slow, and 100 is very fast
"""
speed = abs(speed)
arm_current_position = get_servo_position(c.ARM)
wrist_current_position = get_servo_position(c.WRIST)
if arm_current_position > arm_end_position:
speed = -speed
while arm_current_position != arm_end_position:
if abs(speed) > abs(arm_end_position - arm_current_position):
arm_current_position = arm_end_position
else:
arm_current_position += speed
wrist_current_position -= int(speed * 1.03) # tilt the wrist slightly up
set_servo_position(c.ARM, arm_current_position)
set_servo_position(c.WRIST, wrist_current_position)
msleep(25)
def move_parallel_with_drive(arm_end_position: int, speed: int = 15):
"""
For speed, 1 is very slow, and 100 is very fast
"""
speed = abs(speed)
arm_current_position = get_servo_position(c.ARM)
wrist_current_position = get_servo_position(c.WRIST)
if arm_current_position > arm_end_position:
speed = -speed
drive.time_straight(-25, 100, False)
while arm_current_position != arm_end_position:
if abs(speed) > abs(arm_end_position - arm_current_position):
arm_current_position = arm_end_position
else:
arm_current_position += speed
wrist_current_position -= int(speed * 1.03) # tilt the wrist slightly up
set_servo_position(c.ARM, arm_current_position)
set_servo_position(c.WRIST, wrist_current_position)
msleep(25)
def self_test():
print("testing servos")
move(c.WRIST, 900, 35)
msleep(250)
move(c.ARM, 1900, 35)
msleep(250)
move(c.ARM, 0, 35)
msleep(250)
move(c.WRIST, 2000, 35)
msleep(250)
print("done testing")