-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathservoCalibrate.py
87 lines (71 loc) · 3.01 KB
/
servoCalibrate.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
80
81
82
83
84
85
86
87
import numpy as np
import Adafruit_BBIO.UART as UART
import serial
import time
UART.setup("UART1")
ser = serial.Serial(port="/dev/ttyO1", baudrate=115200)
ser.close()
ser.open()
if ser.isOpen():
print "Serial is open!"
leg1 = np.array([16, 17, 19])
leg2 = np.array([0, 1, 2])
leg3 = np.array([28, 30, 31])
leg4 = np.array([12, 13, 14])
leg_1_center = np.array([1560, 1550, 1560])
leg_2_center = np.array([1560, 1755, 1770])
leg_3_center = np.array([1780, 1700, 1850])
leg_4_center = np.array([1500, 1490, 1680])
while True:
cmd1 = "#0 P%d #1 P%d #2 P%d " % (leg_2_center[0], leg_2_center[1], leg_2_center[2])
cmd2 = "#12 P%d #13 P%d #14 P%d" % (leg_4_center[0], leg_4_center[1], leg_4_center[2])
cmd3 = "#16 P%d #17 P%d #19 P%d" % (leg_1_center[0], leg_1_center[1], leg_1_center[2])
cmd4 = "#28 P%d #30 P%d #31 P%d \r" % (leg_3_center[0], leg_3_center[1], leg_3_center[2])
final_cmd = " ".join((cmd1, cmd2, cmd3, cmd4))
print "Final command: ", final_cmd
ser.write(final_cmd)
user_leg_sel = int(input("Choose a servo leg, 1, 2, 3, 4. Press 9 to EXIT: "))
if user_leg_sel == 9:
break
user_servo_sel = int(input("Choose a servo index, 0, 1 or 2: "))
user_input = int(input("Enter the step: "))
if user_input > 100 or user_input < -100:
print "Error: Enter smaller number"
continue
if user_leg_sel == 1:
leg_1_center[user_servo_sel] = leg_1_center[user_servo_sel] + user_input
# if user_input == 1:
# leg_1_center[user_servo_sel] = leg_1_center[user_servo_sel] + 1
# elif user_input == 0:
# leg_1_center[user_servo_sel] = leg_1_center[user_servo_sel] - 1
# else:
# print "ERROR: Wrong value entered!"
elif user_leg_sel == 2:
leg_2_center[user_servo_sel] = leg_2_center[user_servo_sel] + user_input
# if user_input == 1:
# leg_2_center[user_servo_sel] = leg_2_center[user_servo_sel] + 1
# elif user_input == 0:
# leg_2_center[user_servo_sel] = leg_2_center[user_servo_sel] - 1
# else:
# print "ERROR: Wrong value entered!"
elif user_leg_sel == 3:
leg_3_center[user_servo_sel] = leg_3_center[user_servo_sel] + user_input
# if user_input == 1:
# leg_3_center[user_servo_sel] = leg_3_center[user_servo_sel] + 1
# elif user_input == 0:
# leg_3_center[user_servo_sel] = leg_3_center[user_servo_sel] - 1
# else:
# print "ERROR: Wrong value entered!"
elif user_leg_sel == 4:
leg_4_center[user_servo_sel] = leg_4_center[user_servo_sel] + user_input
# if user_input == 1:
# leg_4_center[user_servo_sel] = leg_4_center[user_servo_sel] + 1
# elif user_input == 0:
# leg_4_center[user_servo_sel] = leg_4_center[user_servo_sel] - 1
# else:
# print "ERROR: Wrong value entered!"
else:
print "ERROR: Wrong leg entered"
print "DONE"
print leg_1_center, leg_2_center, leg_3_center, leg_4_center
ser.close()