-
Notifications
You must be signed in to change notification settings - Fork 0
/
servo.py
44 lines (38 loc) · 1.22 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
from kipr import set_servo_position, get_servo_position, disable_servo, enable_servo
from utilities import msleep
from drive import stop_motors
def move(new_position, step_time=10):
servo = new_position.port
if step_time == 0:
set_servo_position(servo, new_position)
return
temp = get_servo_position(servo)
if temp < new_position:
while temp < new_position:
set_servo_position(servo, temp)
temp += 5
msleep(step_time)
else:
while temp > new_position:
set_servo_position(servo, temp)
temp -= 5
msleep(step_time)
def move_servo_lego(new_position, step_time=10, stop=True):
if stop:
stop_motors(0)
servo = new_position.port
temp = get_servo_position(servo)
# if servo == BackClaw.port:
# enable_servo(BackClaw.port)
if temp < new_position:
while temp < new_position:
set_servo_position(servo, temp)
temp += 5
msleep(step_time)
else:
while temp > new_position:
set_servo_position(servo, temp)
temp -= 5
msleep(step_time)
# if servo == BackClaw.port:
# disable_servo(BackClaw.port)