-
Notifications
You must be signed in to change notification settings - Fork 591
/
calibrate_servos.py
193 lines (155 loc) · 6.7 KB
/
calibrate_servos.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
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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
from pupper.HardwareInterface import HardwareInterface
from pupper.Config import PWMParams, ServoParams
import numpy as np
import re
def get_motor_name(i, j):
motor_type = {0: "abduction", 1: "inner", 2: "outer"} # Top # Bottom
leg_pos = {0: "front-right", 1: "front-left", 2: "back-right", 3: "back-left"}
final_name = motor_type[i] + " " + leg_pos[j]
return final_name
def get_motor_setpoint(i, j):
data = np.array([[0, 0, 0, 0], [45, 45, 45, 45], [45, 45, 45, 45]])
return data[i, j]
def degrees_to_radians(input_array):
"""Converts degrees to radians.
Parameters
----------
input_array : Numpy array or float
Degrees
Returns
-------
Numpy array or float
Radians
"""
return input_array * np.pi / 180.0
def radians_to_degrees(input_array):
"""Converts degrees to radians.
Parameters
----------
input_array : Numpy array or float
Radians
Returns
-------
Numpy array or float
Degrees
"""
return input_array * 180.0 / np.pi
def step_until(hardware_interface, axis, leg, set_point):
"""Returns the angle offset needed to correct a given link by asking the user for input.
Returns
-------
Float
Angle offset needed to correct the link.
"""
found_position = False
set_names = ["horizontal", "horizontal", "vertical"]
offset = 0
while not found_position:
move_input = str(
input("Enter 'a' or 'b' to move the link until it is **" + set_names[axis] + "**. Enter 'd' when done. Input: "
)
)
if move_input == "a":
offset += 1.0
hardware_interface.set_actuator_position(
degrees_to_radians(set_point + offset),
axis,
leg,
)
elif move_input == "b":
offset -= 1.0
hardware_interface.set_actuator_position(
degrees_to_radians(set_point + offset),
axis,
leg,
)
elif move_input == "d":
found_position = True
print("Offset: ", offset)
return offset
def calibrate_angle_offset(hardware_interface):
"""Calibrate the angle offset for the twelve motors on the robot. Note that servo_params is modified in-place.
Parameters
----------
servo_params : ServoParams
Servo parameters. This variable is updated in-place.
pi_board : Pi
RaspberryPi object.
pwm_params : PWMParams
PWMParams object.
"""
# Found K value of (11.4)
print("The scaling constant for your servo represents how much you have to increase\nthe pwm pulse width (in microseconds) to rotate the servo output 1 degree.")
print("This value is currently set to: {:.3f}".format(degrees_to_radians(hardware_interface.servo_params.micros_per_rad)))
print("For newer CLS6336 and CLS6327 servos the value should be 11.333.")
ks = input("Press <Enter> to keep the current value, or enter a new value: ")
if ks != '':
k = float(ks)
hardware_interface.servo_params.micros_per_rad = k * 180 / np.pi
hardware_interface.servo_params.neutral_angle_degrees = np.zeros((3, 4))
for leg_index in range(4):
for axis in range(3):
# Loop until we're satisfied with the calibration
completed = False
while not completed:
motor_name = get_motor_name(axis, leg_index)
print("\n\nCalibrating the **" + motor_name + " motor **")
set_point = get_motor_setpoint(axis, leg_index)
# Zero out the neutral angle
hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] = 0
# Move servo to set_point angle
hardware_interface.set_actuator_position(
degrees_to_radians(set_point),
axis,
leg_index,
)
# Adjust the angle using keyboard input until it matches the reference angle
offset = step_until(
hardware_interface, axis, leg_index, set_point
)
print("Final offset: ", offset)
# The upper leg link has a different equation because we're calibrating to make it horizontal, not vertical
if axis == 1:
hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] = set_point - offset
else:
hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] = -(set_point + offset)
print("Calibrated neutral angle: ", hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index])
# Send the servo command using the new beta value and check that it's ok
hardware_interface.set_actuator_position(
degrees_to_radians([0, 45, -45][axis]),
axis,
leg_index,
)
okay = ""
prompt = "The leg should be at exactly **" + ["horizontal", "45 degrees", "45 degrees"][axis] + "**. Are you satisfied? Enter 'yes' or 'no': "
while okay not in ["y", "n", "yes", "no"]:
okay = str(
input(prompt)
)
completed = okay == "y" or okay == "yes"
def overwrite_ServoCalibration_file(servo_params):
preamble = """# WARNING: This file is machine generated. Edit at your own risk.
import numpy as np
"""
# Format array object string for np.array
p1 = re.compile("([0-9]\.) ( *)") # pattern to replace the space that follows each number with a comma
partially_formatted_matrix = p1.sub(r"\1,\2", str(servo_params.neutral_angle_degrees))
p2 = re.compile("(\]\n)") # pattern to add a comma at the end of the first two lines
formatted_matrix_with_required_commas = p2.sub("],\n", partially_formatted_matrix)
# Overwrite pupper/ServoCalibration.py file with modified values
with open("pupper/ServoCalibration.py", "w") as f:
print(preamble, file = f)
print("MICROS_PER_RAD = {:.3f} * 180.0 / np.pi".format(degrees_to_radians(servo_params.micros_per_rad)), file = f)
print("NEUTRAL_ANGLE_DEGREES = np.array(", file = f)
print(formatted_matrix_with_required_commas, file = f)
print(")", file = f)
def main():
"""Main program
"""
hardware_interface = HardwareInterface()
calibrate_angle_offset(hardware_interface)
overwrite_ServoCalibration_file(hardware_interface.servo_params)
print("\n\n CALIBRATION COMPLETE!\n")
print("Calibrated neutral angles:")
print(hardware_interface.servo_params.neutral_angle_degrees)
main()