def __init__(self):
ipcon = IPConnection()
ipcon.connect(HOST, PORT)
ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE, self.cb_enumerate)
ipcon.enumerate()
i = 0
while self.imu_uid == None:
time.sleep(0.1)
i += 1
if i == 20:
print "Didn't find IMU, exiting"
return
self.s = Servo('9TZyPftcTLE', ipcon)
self.i = IMU(self.imu_uid, ipcon)
self.s.set_output_voltage(self.SERVO_VOLTAGE)
for servo in self.servos.values():
self.s.set_pulse_width(servo[0], servo[1][0], servo[1][1])
self.s.set_degree(servo[0], servo[2][0], servo[2][1])
self.s.set_velocity(servo[0], 0xFFFF)
self.s.set_acceleration(servo[0], 0xFFFF)
self.s.set_position(servo[0], 0)
self.s.enable(servo[0])
time.sleep(1)
self.write_gyr_bias()
time.sleep(0.2)
self.calibrate_gyr_bias()
self.write_gyr_bias()
self.write_gyr_gain()
time.sleep(0.2)
self.calibrate_gyr_gain()
self.write_gyr_gain()
self.write_acc() # default
time.sleep(0.2)
self.calibrate_acc()
self.write_acc() # new calibration
text = """# Each line starts with "calibration type:"
# followed by the x, y and z calibration, separated by a comma.
# Multiplier and Divider are written as "mul/div"
"""
c = []
for num in range(6):
c.append(self.i.get_calibration(num))
text += '0: ' + str(c[0][0]) + '/' + str(c[0][3]) + ', ' + str(c[0][1]) + '/' + str(c[0][4]) + ', ' + str(c[0][2]) + '/' + str(c[0][5])
text += '\n'
text += '1: ' + str(c[1][0]) + ', ' + str(c[1][1]) + ', ' + str(c[1][2])
text += '\n'
text += '2: ' + str(c[2][0]) + '/' + str(c[2][3]) + ', ' + str(c[2][1]) + '/' + str(c[2][4]) + ', ' + str(c[2][2]) + '/' + str(c[2][5])
text += '\n'
text += '3: ' + str(c[3][0]) + ', ' + str(c[3][1]) + ', ' + str(c[3][2])
text += '\n'
text += '4: ' + str(c[4][0]) + '/' + str(c[4][3]) + ', ' + str(c[4][1]) + '/' + str(c[4][4]) + ', ' + str(c[4][2]) + '/' + str(c[4][5])
text += '\n'
text += '5: ' + str(c[5][0]) + ', ' + str(c[5][1]) + ', ' + str(c[5][2]) + ', ' + str(c[5][3]) + ', ' + str(c[5][4]) + ', ' + str(c[5][5]) + ', ' + str(c[5][6]) + ', ' + str(c[5][7])
f = file(self.imu_uid + '.txt', 'w')
f.write(text)
f.close()
def __init__(self):
ipcon = IPConnection()
ipcon.connect(HOST, PORT)
ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE,
self.cb_enumerate)
ipcon.enumerate()
i = 0
while self.imu_uid == None:
time.sleep(0.1)
i += 1
if i == 20:
print "Didn't find IMU, exiting"
return
self.s = Servo('9TZyPftcTLE', ipcon)
self.i = IMU(self.imu_uid, ipcon)
self.s.set_output_voltage(self.SERVO_VOLTAGE)
for servo in self.servos.values():
self.s.set_pulse_width(servo[0], servo[1][0], servo[1][1])
self.s.set_degree(servo[0], servo[2][0], servo[2][1])
self.s.set_velocity(servo[0], 0xFFFF)
self.s.set_acceleration(servo[0], 0xFFFF)
self.s.set_position(servo[0], 0)
self.s.enable(servo[0])
time.sleep(1)
self.write_gyr_bias()
time.sleep(0.2)
self.calibrate_gyr_bias()
self.write_gyr_bias()
self.write_gyr_gain()
time.sleep(0.2)
self.calibrate_gyr_gain()
self.write_gyr_gain()
self.write_acc() # default
time.sleep(0.2)
self.calibrate_acc()
self.write_acc() # new calibration
text = """# Each line starts with "calibration type:"
# followed by the x, y and z calibration, separated by a comma.
# Multiplier and Divider are written as "mul/div"
"""
c = []
for num in range(6):
c.append(self.i.get_calibration(num))
text += '0: ' + str(c[0][0]) + '/' + str(c[0][3]) + ', ' + str(
c[0][1]) + '/' + str(c[0][4]) + ', ' + str(c[0][2]) + '/' + str(
c[0][5])
text += '\n'
text += '1: ' + str(c[1][0]) + ', ' + str(c[1][1]) + ', ' + str(
c[1][2])
text += '\n'
text += '2: ' + str(c[2][0]) + '/' + str(c[2][3]) + ', ' + str(
c[2][1]) + '/' + str(c[2][4]) + ', ' + str(c[2][2]) + '/' + str(
c[2][5])
text += '\n'
text += '3: ' + str(c[3][0]) + ', ' + str(c[3][1]) + ', ' + str(
c[3][2])
text += '\n'
text += '4: ' + str(c[4][0]) + '/' + str(c[4][3]) + ', ' + str(
c[4][1]) + '/' + str(c[4][4]) + ', ' + str(c[4][2]) + '/' + str(
c[4][5])
text += '\n'
text += '5: ' + str(c[5][0]) + ', ' + str(c[5][1]) + ', ' + str(
c[5][2]) + ', ' + str(c[5][3]) + ', ' + str(c[5][4]) + ', ' + str(
c[5][5]) + ', ' + str(c[5][6]) + ', ' + str(c[5][7])
f = file(self.imu_uid + '.txt', 'w')
f.write(text)
f.close()
class IMUCal:
NUM_GYR_CAL_TESTS = 5
TYPE_ACC_GAIN = 0
TYPE_ACC_BIAS = 1
TYPE_MAG_GAIN = 2
TYPE_MAG_BIAS = 3
TYPE_GYR_GAIN = 4
TYPE_GYR_BIAS = 5
GYR_TEMP_LOW = 1600
GYR_TEMP_HIGH = 2500
MAG_CAL_VELOCITY = 40000
MAG_CAL_ACCELERATION = 30000
SERVO_VOLTAGE = 6000
servos = {
'hs-785hb_bottom': [0, (1265, 1735), (-18000, 18000)],
'hs-785hb_middle': [1, (1200, 1800), (-18975, 18975)],
'hs-645mg_top': [2, (900, 1950), (0, 18000)]
}
bot = servos['hs-785hb_bottom'][0]
mid = servos['hs-785hb_middle'][0]
top = servos['hs-645mg_top'][0]
servo_reached = [Event(), Event(), Event()]
mag_make_average = False
mag_sum_x = 0
mag_sum_y = 0
mag_sum_z = 0
mag_sum_i = 0
mag_x_last_top = 0
mag_y_last_top = 0
mag_z_last_top = 0
mag_x_last_bot = 0
mag_y_last_bot = 0
mag_z_last_bot = 0
mag_x_max = 0
mag_y_max = 0
mag_z_max = 0
acc_make_average = False
acc_sum_x = 0
acc_sum_y = 0
acc_sum_z = 0
acc_sum_i = 0
acc_x_min = 0
acc_x_max = 0
acc_y_min = 0
acc_y_max = 0
acc_z_min = 0
acc_z_max = 0
acc_x_bias = 0
acc_y_bias = 0
acc_z_bias = 0
acc_x_gain_mult = 1
acc_y_gain_mult = 1
acc_z_gain_mult = 1
acc_x_gain_div = 1
acc_y_gain_div = 1
acc_z_gain_div = 1
gyr_x_gain_mult = 1
gyr_y_gain_mult = 1
gyr_z_gain_mult = 1
gyr_x_gain_div = 1
gyr_y_gain_div = 1
gyr_z_gain_div = 1
gyr_bias_xl = 0
gyr_bias_yl = 0
gyr_bias_zl = 0
gyr_bias_tl = 0
gyr_bias_xh = 0
gyr_bias_yh = 0
gyr_bias_zh = 0
gyr_bias_th = 1
gyr_make_average = False
gyr_sum_i = 0
gyr_sum_x = 0
gyr_sum_y = 0
gyr_sum_z = 0
gyr_x_ref1 = 2970.9937199233336
gyr_x_ref2 = 2997.3047517850005
gyr_x_ref = int(round(gyr_x_ref1 + gyr_x_ref2))
gyr_y_ref1 = 1339.5480923774999
gyr_y_ref2 = 1428.6085124024999
gyr_y_ref = int(round(gyr_y_ref1 + gyr_y_ref2))
gyr_z_ref1 = 1250.442733235
gyr_z_ref2 = 1200.7509489899999
gyr_z_ref = int(round(gyr_z_ref1 + gyr_z_ref2))
imu_uid = None
def cb_reached(self, servo_num, position):
self.servo_reached[servo_num].set()
def cb_mag(self, x, y, z):
if self.mag_make_average:
self.mag_sum_i += 1
self.mag_sum_x += x
self.mag_sum_y += y
self.mag_sum_z += z
def make_mag_avg(self):
self.mag_make_average = True
time.sleep(1)
self.mag_make_average = False
time.sleep(0.05)
x = self.mag_sum_x/self.mag_sum_i
y = self.mag_sum_y/self.mag_sum_i
z = self.mag_sum_z/self.mag_sum_i
self.mag_sum_x, self.mag_sum_y, self.mag_sum_z, self.mag_sum_i = 0, 0, 0, 0
return x, y, z
def cb_acc(self, x, y, z):
if self.acc_make_average and self.acc_sum_i < 1000:
self.acc_sum_i += 1
self.acc_sum_x += x
self.acc_sum_y += y
self.acc_sum_z += z
if self.acc_sum_i == 1000:
self.acc_make_average = False
self.servo_reached[0].set()
def make_acc_avg(self, direction):
print "start calibrating", direction
self.servo_reached[0].clear()
self.i.set_acceleration_period(1)
self.acc_make_average = True
self.servo_reached[0].wait()
self.i.set_acceleration_period(0)
if direction == 'X':
if self.acc_sum_x > 0:
self.acc_x_max = self.acc_sum_x/self.acc_sum_i
print "value =", self.acc_x_max
else:
self.acc_x_min = self.acc_sum_x/self.acc_sum_i
print "value =", self.acc_x_min
elif direction == 'Y':
if self.acc_sum_y > 0:
self.acc_y_max = self.acc_sum_y/self.acc_sum_i
print "value =", self.acc_y_max
else :
self.acc_y_min = self.acc_sum_y/self.acc_sum_i
print "value =", self.acc_y_min
elif direction == 'Z':
if self.acc_sum_z > 0:
self.acc_z_max = self.acc_sum_z/self.acc_sum_i
print "value =", self.acc_z_max
else :
self.acc_z_min = self.acc_sum_z/self.acc_sum_i
print "value =", self.acc_z_min
self.acc_sum_i = 0
self.acc_sum_x = 0
self.acc_sum_y = 0
self.acc_sum_z = 0
def write_acc(self):
gain = [self.acc_x_gain_mult, self.acc_y_gain_mult, self.acc_z_gain_mult, self.acc_x_gain_div, self.acc_y_gain_div, self.acc_z_gain_div, 0, 0, 0, 0]
bias = [self.acc_x_bias, self.acc_y_bias, self.acc_z_bias, 0, 0, 0, 0, 0, 0, 0]
print 'writing acc (gain, bias):', gain, bias
self.i.set_calibration(self.TYPE_ACC_GAIN, gain)
self.i.set_calibration(self.TYPE_ACC_BIAS, bias)
def write_gyr_bias(self):
bias = [-self.gyr_bias_xl, -self.gyr_bias_yl, -self.gyr_bias_zl, self.gyr_bias_tl, -self.gyr_bias_xh, -self.gyr_bias_yh, -self.gyr_bias_zh, self.gyr_bias_th, 0, 0]
print 'writing gyr (bias):', bias
self.i.set_calibration(self.TYPE_GYR_BIAS, bias)
def write_gyr_gain(self):
gain = [self.gyr_x_gain_mult, self.gyr_y_gain_mult, self.gyr_z_gain_mult, self.gyr_x_gain_div, self.gyr_y_gain_div, self.gyr_z_gain_div, 0, 0, 0, 0]
print 'writing gyr (gain):', gain
self.i.set_calibration(self.TYPE_GYR_GAIN, gain)
def calculate_acc(self):
self.acc_x_bias = ((1000 - self.acc_x_max) + (-1000 - self.acc_x_min))/2
self.acc_y_bias = ((1000 - self.acc_y_max) + (-1000 - self.acc_y_min))/2
self.acc_z_bias = ((1000 - self.acc_z_max) + (-1000 - self.acc_z_min))/2
self.acc_x_gain_mult = 1000
self.acc_y_gain_mult = 1000
self.acc_z_gain_mult = 1000
self.acc_x_gain_div = self.acc_x_max + self.acc_x_bias
self.acc_y_gain_div = self.acc_y_max + self.acc_y_bias
self.acc_z_gain_div = self.acc_z_max + self.acc_z_bias
def calibrate_acc(self):
self.s.set_position(self.bot, 0)
self.i.register_callback(self.i.CALLBACK_ACCELERATION, self.cb_acc)
# X+
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, 7700)
time.sleep(3)
self.make_acc_avg('X')
# X-
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, -7350)
time.sleep(3.5)
self.make_acc_avg('X')
# Y+
self.s.set_position(self.top, 8800)
self.s.set_position(self.mid, 180)
time.sleep(3)
self.make_acc_avg('Y')
# Y-
self.s.set_position(self.top, 8800)
self.s.set_position(self.mid, 14900)
time.sleep(3.5)
self.make_acc_avg('Y')
# Z+
self.s.set_position(self.top, 18000)
self.s.set_position(self.mid, 14900)
time.sleep(2)
self.make_acc_avg('Z')
# Z-
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, 14900)
time.sleep(2)
self.make_acc_avg('Z')
self.calculate_acc()
print 'acc calibration:', (self.acc_x_min, self.acc_x_max, self.acc_y_min, self.acc_y_max, self.acc_z_min, self.acc_z_max)
def cb_gyr(self, x, y, z):
if self.gyr_make_average:
self.gyr_sum_i += 1
self.gyr_sum_x += x
self.gyr_sum_y += y
self.gyr_sum_z += z
def calibrate_gyr_gain(self):
def calibrate_x(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.top, pos)
time.sleep(0.2)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(0.5)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.2)
print 'value: ', self.gyr_sum_x/self.gyr_sum_i
time.sleep(0.5)
def calibrate_y(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.mid, pos)
time.sleep(1.2)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(0.5)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.1)
print 'value: ', self.gyr_sum_y/self.gyr_sum_i
time.sleep(1.2)
def calibrate_z(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.bot, pos)
time.sleep(2.5)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(1.0)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.1)
print 'value: ', self.gyr_sum_z/self.gyr_sum_i
time.sleep(0.5)
self.i.register_callback(self.i.CALLBACK_ANGULAR_VELOCITY, self.cb_gyr)
self.s.set_position(self.top, 18000)
self.s.set_position(self.mid, 14900)
self.s.set_velocity(self.top, 20000)
time.sleep(2)
val1 = 0
val2 = 0
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_x(0)
if i != 0:
val1 += self.gyr_sum_x/float(self.gyr_sum_i)
calibrate_x(18000)
if i != 0:
val2 += self.gyr_sum_x/float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_x = int(round(val1/float(self.NUM_GYR_CAL_TESTS-1) + val2/float(self.NUM_GYR_CAL_TESTS-1)))
self.s.set_velocity(self.mid, 8000)
self.s.set_position(self.mid, 10000)
val1 = 0
val2 = 0
time.sleep(2)
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_y(-10000)
if i != 0:
val1 += self.gyr_sum_y/float(self.gyr_sum_i)
calibrate_y(10000)
if i != 0:
val2 += self.gyr_sum_y/float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_y = int(round(val1/float(self.NUM_GYR_CAL_TESTS-1) + val2/float(self.NUM_GYR_CAL_TESTS-1)))
self.s.set_velocity(self.mid, 0xFFFF)
self.s.set_velocity(self.bot, 8000)
self.s.set_position(self.bot, 10000)
self.s.set_position(self.mid, 0)
time.sleep(2)
val1 = 0
val2 = 0
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_z(-18000)
if i != 0:
val1 += self.gyr_sum_z/float(self.gyr_sum_i)
calibrate_z(18000)
if i != 0:
val2 += self.gyr_sum_z/float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_z = int(round(val1/float(self.NUM_GYR_CAL_TESTS-1) + val2/float(self.NUM_GYR_CAL_TESTS-1)))
self.gyr_x_gain_mult = self.gyr_x_ref
self.gyr_y_gain_mult = self.gyr_y_ref
self.gyr_z_gain_mult = self.gyr_z_ref
self.gyr_x_gain_div = val_x
self.gyr_y_gain_div = val_y
self.gyr_z_gain_div = val_z
gyr_gain_str = ', '.join([str(self.gyr_x_ref) + '/' + str(val_x),
str(self.gyr_y_ref) + '/' + str(val_y),
str(self.gyr_z_ref) + '/' + str(val_z)])
print "gyr gain: " + gyr_gain_str
def wait(self, num, pos):
if self.s.get_current_position(num) != pos:
self.servo_reached[num].wait()
self.servo_reached[num].clear()
time.sleep(0.25)
def calibrate_mag(self):
# TODO
bot_pos = 0
top_pos = -13000
self.s.set_acceleration(self.bot, 0xFFFF)
self.s.set_acceleration(self.top, 0xFFFF)
self.s.set_velocity(self.bot, 0xFFFF)
self.s.set_velocity(self.top, 0xFFFF)
self.s.set_position(self.bot, bot_pos)
self.s.set_position(self.top, top_pos)
self.s.enable(self.bot)
self.s.enable(self.top)
direction = 1
for i in range(20):
time.sleep(1)
x, y, z = self.make_mag_avg()
print "bot", x
if x < self.mag_x_last_bot:
bot_pos += 1000
else:
bot_pos -= 1000
if bot_pos > 18000:
bot_pos = bot_pos - 36000
if bot_pos < -18000:
bot_pos = bot_pos + 36000
print bot_pos
self.mag_x_last_bot = x
self.s.set_position(self.bot, bot_pos)
self.wait(self.bot, bot_pos)
time.sleep(0.1)
# time.sleep(1)
# x, y, z = self.make_mag_avg()
# print "top", x
# if x > self.mag_x_last_top:
# top_pos += 1000
# else:
# top_pos -= 1000
#
# self.mag_x_last_top = x
#
# print 'new_pos', bot_pos
# self.s.set_position(self.top, top_pos)
# self.wait(self.top, top_pos)
# time.sleep(0.1)
def calibrate_top_mid(s, bot_pos):
for e in servo_reached:
e.clear()
self.s.set_acceleration(top, MAG_CAL_ACCELERATION)
self.s.set_acceleration(mid, MAG_CAL_ACCELERATION)
self.s.set_acceleration(bot, MAG_CAL_ACCELERATION)
self.s.set_velocity(top, MAG_CAL_VELOCITY)
self.s.set_velocity(mid, MAG_CAL_VELOCITY)
self.s.set_velocity(bot, MAG_CAL_VELOCITY)
self.s.set_position(top, 18000)
self.s.enable(top)
self.s.set_position(mid, 0)
self.s.enable(mid)
self.s.set_position(bot, bot_pos)
self.s.enable(bot)
wait(bot, bot_pos, s)
wait(top, 18000, s)
wait(mid, 0, s)
for i in range(19):
self.s.set_position(mid, i*1000)
wait(mid, i*1000, s)
if i % 2:
self.s.set_position(top, 18000)
wait(top, 18000, s)
else:
self.s.set_position(top, 0)
wait(top, 0, s)
calibrate_top_mid(s, -9000)
calibrate_top_mid(s, 9000)
def calibrate_gyr_bias(self):
self.i.register_callback(self.i.CALLBACK_ANGULAR_VELOCITY, self.cb_gyr)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(temp/100.0) + " °C (waiting to fall below " + str(self.GYR_TEMP_LOW/100.0) + " °C)"
if temp < self.GYR_TEMP_LOW:
break
time.sleep(1)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(temp/100.0) + " °C (waiting to rise above " + str(self.GYR_TEMP_LOW/100.0) + " °C)"
if temp > self.GYR_TEMP_LOW:
break
time.sleep(1)
temp1 = self.i.get_imu_temperature()
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(3)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
temp2 = self.i.get_imu_temperature()
self.gyr_bias_tl = int(round((temp1 + temp2)/2.0))
self.gyr_bias_xl, self.gyr_bias_yl, self.gyr_bias_zl = (self.gyr_sum_x/self.gyr_sum_i, self.gyr_sum_y/self.gyr_sum_i, self.gyr_sum_z/self.gyr_sum_i)
print 'value: ', (self.gyr_bias_xl, self.gyr_bias_yl, self.gyr_bias_zl, self.gyr_bias_tl)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(temp/100.0) + " °C (waiting to rise above " + str(self.GYR_TEMP_HIGH/100.0) + " °C)"
if temp > self.GYR_TEMP_HIGH:
break
time.sleep(1)
temp1 = self.i.get_imu_temperature()
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(3)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
temp2 = self.i.get_imu_temperature()
self.gyr_bias_th = int(round((temp1 + temp2)/2.0))
self.gyr_bias_xh, self.gyr_bias_yh, self.gyr_bias_zh = (self.gyr_sum_x/self.gyr_sum_i, self.gyr_sum_y/self.gyr_sum_i, self.gyr_sum_z/self.gyr_sum_i)
print 'value: ', (self.gyr_bias_xh, self.gyr_bias_yh, self.gyr_bias_zh, self.gyr_bias_th)
def cb_enumerate(self, uid, connected_uid, position, hardware_version, firmware_version, device_identifier, enumeration_type):
if device_identifier == 16:
self.imu_uid = uid
def __init__(self):
ipcon = IPConnection()
ipcon.connect(HOST, PORT)
ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE, self.cb_enumerate)
ipcon.enumerate()
i = 0
while self.imu_uid == None:
time.sleep(0.1)
i += 1
if i == 20:
print "Didn't find IMU, exiting"
return
self.s = Servo('9TZyPftcTLE', ipcon)
self.i = IMU(self.imu_uid, ipcon)
self.s.set_output_voltage(self.SERVO_VOLTAGE)
for servo in self.servos.values():
self.s.set_pulse_width(servo[0], servo[1][0], servo[1][1])
self.s.set_degree(servo[0], servo[2][0], servo[2][1])
self.s.set_velocity(servo[0], 0xFFFF)
self.s.set_acceleration(servo[0], 0xFFFF)
self.s.set_position(servo[0], 0)
self.s.enable(servo[0])
time.sleep(1)
self.write_gyr_bias()
time.sleep(0.2)
self.calibrate_gyr_bias()
self.write_gyr_bias()
self.write_gyr_gain()
time.sleep(0.2)
self.calibrate_gyr_gain()
self.write_gyr_gain()
self.write_acc() # default
time.sleep(0.2)
self.calibrate_acc()
self.write_acc() # new calibration
text = """# Each line starts with "calibration type:"
# followed by the x, y and z calibration, separated by a comma.
# Multiplier and Divider are written as "mul/div"
"""
c = []
for num in range(6):
c.append(self.i.get_calibration(num))
text += '0: ' + str(c[0][0]) + '/' + str(c[0][3]) + ', ' + str(c[0][1]) + '/' + str(c[0][4]) + ', ' + str(c[0][2]) + '/' + str(c[0][5])
text += '\n'
text += '1: ' + str(c[1][0]) + ', ' + str(c[1][1]) + ', ' + str(c[1][2])
text += '\n'
text += '2: ' + str(c[2][0]) + '/' + str(c[2][3]) + ', ' + str(c[2][1]) + '/' + str(c[2][4]) + ', ' + str(c[2][2]) + '/' + str(c[2][5])
text += '\n'
text += '3: ' + str(c[3][0]) + ', ' + str(c[3][1]) + ', ' + str(c[3][2])
text += '\n'
text += '4: ' + str(c[4][0]) + '/' + str(c[4][3]) + ', ' + str(c[4][1]) + '/' + str(c[4][4]) + ', ' + str(c[4][2]) + '/' + str(c[4][5])
text += '\n'
text += '5: ' + str(c[5][0]) + ', ' + str(c[5][1]) + ', ' + str(c[5][2]) + ', ' + str(c[5][3]) + ', ' + str(c[5][4]) + ', ' + str(c[5][5]) + ', ' + str(c[5][6]) + ', ' + str(c[5][7])
f = file(self.imu_uid + '.txt', 'w')
f.write(text)
f.close()
class IMUCal:
NUM_GYR_CAL_TESTS = 5
TYPE_ACC_GAIN = 0
TYPE_ACC_BIAS = 1
TYPE_MAG_GAIN = 2
TYPE_MAG_BIAS = 3
TYPE_GYR_GAIN = 4
TYPE_GYR_BIAS = 5
GYR_TEMP_LOW = 1600
GYR_TEMP_HIGH = 2500
MAG_CAL_VELOCITY = 40000
MAG_CAL_ACCELERATION = 30000
SERVO_VOLTAGE = 6000
servos = {
'hs-785hb_bottom': [0, (1265, 1735), (-18000, 18000)],
'hs-785hb_middle': [1, (1200, 1800), (-18975, 18975)],
'hs-645mg_top': [2, (900, 1950), (0, 18000)]
}
bot = servos['hs-785hb_bottom'][0]
mid = servos['hs-785hb_middle'][0]
top = servos['hs-645mg_top'][0]
servo_reached = [Event(), Event(), Event()]
mag_make_average = False
mag_sum_x = 0
mag_sum_y = 0
mag_sum_z = 0
mag_sum_i = 0
mag_x_last_top = 0
mag_y_last_top = 0
mag_z_last_top = 0
mag_x_last_bot = 0
mag_y_last_bot = 0
mag_z_last_bot = 0
mag_x_max = 0
mag_y_max = 0
mag_z_max = 0
acc_make_average = False
acc_sum_x = 0
acc_sum_y = 0
acc_sum_z = 0
acc_sum_i = 0
acc_x_min = 0
acc_x_max = 0
acc_y_min = 0
acc_y_max = 0
acc_z_min = 0
acc_z_max = 0
acc_x_bias = 0
acc_y_bias = 0
acc_z_bias = 0
acc_x_gain_mult = 1
acc_y_gain_mult = 1
acc_z_gain_mult = 1
acc_x_gain_div = 1
acc_y_gain_div = 1
acc_z_gain_div = 1
gyr_x_gain_mult = 1
gyr_y_gain_mult = 1
gyr_z_gain_mult = 1
gyr_x_gain_div = 1
gyr_y_gain_div = 1
gyr_z_gain_div = 1
gyr_bias_xl = 0
gyr_bias_yl = 0
gyr_bias_zl = 0
gyr_bias_tl = 0
gyr_bias_xh = 0
gyr_bias_yh = 0
gyr_bias_zh = 0
gyr_bias_th = 1
gyr_make_average = False
gyr_sum_i = 0
gyr_sum_x = 0
gyr_sum_y = 0
gyr_sum_z = 0
gyr_x_ref1 = 2970.9937199233336
gyr_x_ref2 = 2997.3047517850005
gyr_x_ref = int(round(gyr_x_ref1 + gyr_x_ref2))
gyr_y_ref1 = 1339.5480923774999
gyr_y_ref2 = 1428.6085124024999
gyr_y_ref = int(round(gyr_y_ref1 + gyr_y_ref2))
gyr_z_ref1 = 1250.442733235
gyr_z_ref2 = 1200.7509489899999
gyr_z_ref = int(round(gyr_z_ref1 + gyr_z_ref2))
imu_uid = None
def cb_reached(self, servo_num, position):
self.servo_reached[servo_num].set()
def cb_mag(self, x, y, z):
if self.mag_make_average:
self.mag_sum_i += 1
self.mag_sum_x += x
self.mag_sum_y += y
self.mag_sum_z += z
def make_mag_avg(self):
self.mag_make_average = True
time.sleep(1)
self.mag_make_average = False
time.sleep(0.05)
x = self.mag_sum_x / self.mag_sum_i
y = self.mag_sum_y / self.mag_sum_i
z = self.mag_sum_z / self.mag_sum_i
self.mag_sum_x, self.mag_sum_y, self.mag_sum_z, self.mag_sum_i = 0, 0, 0, 0
return x, y, z
def cb_acc(self, x, y, z):
if self.acc_make_average and self.acc_sum_i < 1000:
self.acc_sum_i += 1
self.acc_sum_x += x
self.acc_sum_y += y
self.acc_sum_z += z
if self.acc_sum_i == 1000:
self.acc_make_average = False
self.servo_reached[0].set()
def make_acc_avg(self, direction):
print "start calibrating", direction
self.servo_reached[0].clear()
self.i.set_acceleration_period(1)
self.acc_make_average = True
self.servo_reached[0].wait()
self.i.set_acceleration_period(0)
if direction == 'X':
if self.acc_sum_x > 0:
self.acc_x_max = self.acc_sum_x / self.acc_sum_i
print "value =", self.acc_x_max
else:
self.acc_x_min = self.acc_sum_x / self.acc_sum_i
print "value =", self.acc_x_min
elif direction == 'Y':
if self.acc_sum_y > 0:
self.acc_y_max = self.acc_sum_y / self.acc_sum_i
print "value =", self.acc_y_max
else:
self.acc_y_min = self.acc_sum_y / self.acc_sum_i
print "value =", self.acc_y_min
elif direction == 'Z':
if self.acc_sum_z > 0:
self.acc_z_max = self.acc_sum_z / self.acc_sum_i
print "value =", self.acc_z_max
else:
self.acc_z_min = self.acc_sum_z / self.acc_sum_i
print "value =", self.acc_z_min
self.acc_sum_i = 0
self.acc_sum_x = 0
self.acc_sum_y = 0
self.acc_sum_z = 0
def write_acc(self):
gain = [
self.acc_x_gain_mult, self.acc_y_gain_mult, self.acc_z_gain_mult,
self.acc_x_gain_div, self.acc_y_gain_div, self.acc_z_gain_div, 0,
0, 0, 0
]
bias = [
self.acc_x_bias, self.acc_y_bias, self.acc_z_bias, 0, 0, 0, 0, 0,
0, 0
]
print 'writing acc (gain, bias):', gain, bias
self.i.set_calibration(self.TYPE_ACC_GAIN, gain)
self.i.set_calibration(self.TYPE_ACC_BIAS, bias)
def write_gyr_bias(self):
bias = [
-self.gyr_bias_xl, -self.gyr_bias_yl, -self.gyr_bias_zl,
self.gyr_bias_tl, -self.gyr_bias_xh, -self.gyr_bias_yh,
-self.gyr_bias_zh, self.gyr_bias_th, 0, 0
]
print 'writing gyr (bias):', bias
self.i.set_calibration(self.TYPE_GYR_BIAS, bias)
def write_gyr_gain(self):
gain = [
self.gyr_x_gain_mult, self.gyr_y_gain_mult, self.gyr_z_gain_mult,
self.gyr_x_gain_div, self.gyr_y_gain_div, self.gyr_z_gain_div, 0,
0, 0, 0
]
print 'writing gyr (gain):', gain
self.i.set_calibration(self.TYPE_GYR_GAIN, gain)
def calculate_acc(self):
self.acc_x_bias = ((1000 - self.acc_x_max) +
(-1000 - self.acc_x_min)) / 2
self.acc_y_bias = ((1000 - self.acc_y_max) +
(-1000 - self.acc_y_min)) / 2
self.acc_z_bias = ((1000 - self.acc_z_max) +
(-1000 - self.acc_z_min)) / 2
self.acc_x_gain_mult = 1000
self.acc_y_gain_mult = 1000
self.acc_z_gain_mult = 1000
self.acc_x_gain_div = self.acc_x_max + self.acc_x_bias
self.acc_y_gain_div = self.acc_y_max + self.acc_y_bias
self.acc_z_gain_div = self.acc_z_max + self.acc_z_bias
def calibrate_acc(self):
self.s.set_position(self.bot, 0)
self.i.register_callback(self.i.CALLBACK_ACCELERATION, self.cb_acc)
# X+
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, 7700)
time.sleep(3)
self.make_acc_avg('X')
# X-
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, -7350)
time.sleep(3.5)
self.make_acc_avg('X')
# Y+
self.s.set_position(self.top, 8800)
self.s.set_position(self.mid, 180)
time.sleep(3)
self.make_acc_avg('Y')
# Y-
self.s.set_position(self.top, 8800)
self.s.set_position(self.mid, 14900)
time.sleep(3.5)
self.make_acc_avg('Y')
# Z+
self.s.set_position(self.top, 18000)
self.s.set_position(self.mid, 14900)
time.sleep(2)
self.make_acc_avg('Z')
# Z-
self.s.set_position(self.top, 50)
self.s.set_position(self.mid, 14900)
time.sleep(2)
self.make_acc_avg('Z')
self.calculate_acc()
print 'acc calibration:', (self.acc_x_min, self.acc_x_max,
self.acc_y_min, self.acc_y_max,
self.acc_z_min, self.acc_z_max)
def cb_gyr(self, x, y, z):
if self.gyr_make_average:
self.gyr_sum_i += 1
self.gyr_sum_x += x
self.gyr_sum_y += y
self.gyr_sum_z += z
def calibrate_gyr_gain(self):
def calibrate_x(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.top, pos)
time.sleep(0.2)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(0.5)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.2)
print 'value: ', self.gyr_sum_x / self.gyr_sum_i
time.sleep(0.5)
def calibrate_y(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.mid, pos)
time.sleep(1.2)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(0.5)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.1)
print 'value: ', self.gyr_sum_y / self.gyr_sum_i
time.sleep(1.2)
def calibrate_z(pos):
self.gyr_sum_i = 0
self.gyr_sum_x = 0
self.gyr_sum_y = 0
self.gyr_sum_z = 0
self.s.set_position(self.bot, pos)
time.sleep(2.5)
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(1.0)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
time.sleep(0.1)
print 'value: ', self.gyr_sum_z / self.gyr_sum_i
time.sleep(0.5)
self.i.register_callback(self.i.CALLBACK_ANGULAR_VELOCITY, self.cb_gyr)
self.s.set_position(self.top, 18000)
self.s.set_position(self.mid, 14900)
self.s.set_velocity(self.top, 20000)
time.sleep(2)
val1 = 0
val2 = 0
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_x(0)
if i != 0:
val1 += self.gyr_sum_x / float(self.gyr_sum_i)
calibrate_x(18000)
if i != 0:
val2 += self.gyr_sum_x / float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_x = int(
round(val1 / float(self.NUM_GYR_CAL_TESTS - 1) +
val2 / float(self.NUM_GYR_CAL_TESTS - 1)))
self.s.set_velocity(self.mid, 8000)
self.s.set_position(self.mid, 10000)
val1 = 0
val2 = 0
time.sleep(2)
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_y(-10000)
if i != 0:
val1 += self.gyr_sum_y / float(self.gyr_sum_i)
calibrate_y(10000)
if i != 0:
val2 += self.gyr_sum_y / float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_y = int(
round(val1 / float(self.NUM_GYR_CAL_TESTS - 1) +
val2 / float(self.NUM_GYR_CAL_TESTS - 1)))
self.s.set_velocity(self.mid, 0xFFFF)
self.s.set_velocity(self.bot, 8000)
self.s.set_position(self.bot, 10000)
self.s.set_position(self.mid, 0)
time.sleep(2)
val1 = 0
val2 = 0
for i in range(self.NUM_GYR_CAL_TESTS):
calibrate_z(-18000)
if i != 0:
val1 += self.gyr_sum_z / float(self.gyr_sum_i)
calibrate_z(18000)
if i != 0:
val2 += self.gyr_sum_z / float(self.gyr_sum_i)
val1 = abs(val1)
val2 = abs(val2)
val_z = int(
round(val1 / float(self.NUM_GYR_CAL_TESTS - 1) +
val2 / float(self.NUM_GYR_CAL_TESTS - 1)))
self.gyr_x_gain_mult = self.gyr_x_ref
self.gyr_y_gain_mult = self.gyr_y_ref
self.gyr_z_gain_mult = self.gyr_z_ref
self.gyr_x_gain_div = val_x
self.gyr_y_gain_div = val_y
self.gyr_z_gain_div = val_z
gyr_gain_str = ', '.join([
str(self.gyr_x_ref) + '/' + str(val_x),
str(self.gyr_y_ref) + '/' + str(val_y),
str(self.gyr_z_ref) + '/' + str(val_z)
])
print "gyr gain: " + gyr_gain_str
def wait(self, num, pos):
if self.s.get_current_position(num) != pos:
self.servo_reached[num].wait()
self.servo_reached[num].clear()
time.sleep(0.25)
def calibrate_mag(self):
# TODO
bot_pos = 0
top_pos = -13000
self.s.set_acceleration(self.bot, 0xFFFF)
self.s.set_acceleration(self.top, 0xFFFF)
self.s.set_velocity(self.bot, 0xFFFF)
self.s.set_velocity(self.top, 0xFFFF)
self.s.set_position(self.bot, bot_pos)
self.s.set_position(self.top, top_pos)
self.s.enable(self.bot)
self.s.enable(self.top)
direction = 1
for i in range(20):
time.sleep(1)
x, y, z = self.make_mag_avg()
print "bot", x
if x < self.mag_x_last_bot:
bot_pos += 1000
else:
bot_pos -= 1000
if bot_pos > 18000:
bot_pos = bot_pos - 36000
if bot_pos < -18000:
bot_pos = bot_pos + 36000
print bot_pos
self.mag_x_last_bot = x
self.s.set_position(self.bot, bot_pos)
self.wait(self.bot, bot_pos)
time.sleep(0.1)
# time.sleep(1)
# x, y, z = self.make_mag_avg()
# print "top", x
# if x > self.mag_x_last_top:
# top_pos += 1000
# else:
# top_pos -= 1000
#
# self.mag_x_last_top = x
#
# print 'new_pos', bot_pos
# self.s.set_position(self.top, top_pos)
# self.wait(self.top, top_pos)
# time.sleep(0.1)
def calibrate_top_mid(s, bot_pos):
for e in servo_reached:
e.clear()
self.s.set_acceleration(top, MAG_CAL_ACCELERATION)
self.s.set_acceleration(mid, MAG_CAL_ACCELERATION)
self.s.set_acceleration(bot, MAG_CAL_ACCELERATION)
self.s.set_velocity(top, MAG_CAL_VELOCITY)
self.s.set_velocity(mid, MAG_CAL_VELOCITY)
self.s.set_velocity(bot, MAG_CAL_VELOCITY)
self.s.set_position(top, 18000)
self.s.enable(top)
self.s.set_position(mid, 0)
self.s.enable(mid)
self.s.set_position(bot, bot_pos)
self.s.enable(bot)
wait(bot, bot_pos, s)
wait(top, 18000, s)
wait(mid, 0, s)
for i in range(19):
self.s.set_position(mid, i * 1000)
wait(mid, i * 1000, s)
if i % 2:
self.s.set_position(top, 18000)
wait(top, 18000, s)
else:
self.s.set_position(top, 0)
wait(top, 0, s)
calibrate_top_mid(s, -9000)
calibrate_top_mid(s, 9000)
def calibrate_gyr_bias(self):
self.i.register_callback(self.i.CALLBACK_ANGULAR_VELOCITY, self.cb_gyr)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(
temp / 100.0) + " °C (waiting to fall below " + str(
self.GYR_TEMP_LOW / 100.0) + " °C)"
if temp < self.GYR_TEMP_LOW:
break
time.sleep(1)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(
temp / 100.0) + " °C (waiting to rise above " + str(
self.GYR_TEMP_LOW / 100.0) + " °C)"
if temp > self.GYR_TEMP_LOW:
break
time.sleep(1)
temp1 = self.i.get_imu_temperature()
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(3)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
temp2 = self.i.get_imu_temperature()
self.gyr_bias_tl = int(round((temp1 + temp2) / 2.0))
self.gyr_bias_xl, self.gyr_bias_yl, self.gyr_bias_zl = (
self.gyr_sum_x / self.gyr_sum_i, self.gyr_sum_y / self.gyr_sum_i,
self.gyr_sum_z / self.gyr_sum_i)
print 'value: ', (self.gyr_bias_xl, self.gyr_bias_yl, self.gyr_bias_zl,
self.gyr_bias_tl)
while True:
temp = self.i.get_imu_temperature()
print "Temperature: " + str(
temp / 100.0) + " °C (waiting to rise above " + str(
self.GYR_TEMP_HIGH / 100.0) + " °C)"
if temp > self.GYR_TEMP_HIGH:
break
time.sleep(1)
temp1 = self.i.get_imu_temperature()
self.i.set_angular_velocity_period(1)
self.gyr_make_average = True
print("start")
time.sleep(3)
self.gyr_make_average = False
print("stop")
self.i.set_angular_velocity_period(0)
temp2 = self.i.get_imu_temperature()
self.gyr_bias_th = int(round((temp1 + temp2) / 2.0))
self.gyr_bias_xh, self.gyr_bias_yh, self.gyr_bias_zh = (
self.gyr_sum_x / self.gyr_sum_i, self.gyr_sum_y / self.gyr_sum_i,
self.gyr_sum_z / self.gyr_sum_i)
print 'value: ', (self.gyr_bias_xh, self.gyr_bias_yh, self.gyr_bias_zh,
self.gyr_bias_th)
def cb_enumerate(self, uid, connected_uid, position, hardware_version,
firmware_version, device_identifier, enumeration_type):
if device_identifier == 16:
self.imu_uid = uid
def __init__(self):
ipcon = IPConnection()
ipcon.connect(HOST, PORT)
ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE,
self.cb_enumerate)
ipcon.enumerate()
i = 0
while self.imu_uid == None:
time.sleep(0.1)
i += 1
if i == 20:
print "Didn't find IMU, exiting"
return
self.s = Servo('9TZyPftcTLE', ipcon)
self.i = IMU(self.imu_uid, ipcon)
self.s.set_output_voltage(self.SERVO_VOLTAGE)
for servo in self.servos.values():
self.s.set_pulse_width(servo[0], servo[1][0], servo[1][1])
self.s.set_degree(servo[0], servo[2][0], servo[2][1])
self.s.set_velocity(servo[0], 0xFFFF)
self.s.set_acceleration(servo[0], 0xFFFF)
self.s.set_position(servo[0], 0)
self.s.enable(servo[0])
time.sleep(1)
self.write_gyr_bias()
time.sleep(0.2)
self.calibrate_gyr_bias()
self.write_gyr_bias()
self.write_gyr_gain()
time.sleep(0.2)
self.calibrate_gyr_gain()
self.write_gyr_gain()
self.write_acc() # default
time.sleep(0.2)
self.calibrate_acc()
self.write_acc() # new calibration
text = """# Each line starts with "calibration type:"
# followed by the x, y and z calibration, separated by a comma.
# Multiplier and Divider are written as "mul/div"
"""
c = []
for num in range(6):
c.append(self.i.get_calibration(num))
text += '0: ' + str(c[0][0]) + '/' + str(c[0][3]) + ', ' + str(
c[0][1]) + '/' + str(c[0][4]) + ', ' + str(c[0][2]) + '/' + str(
c[0][5])
text += '\n'
text += '1: ' + str(c[1][0]) + ', ' + str(c[1][1]) + ', ' + str(
c[1][2])
text += '\n'
text += '2: ' + str(c[2][0]) + '/' + str(c[2][3]) + ', ' + str(
c[2][1]) + '/' + str(c[2][4]) + ', ' + str(c[2][2]) + '/' + str(
c[2][5])
text += '\n'
text += '3: ' + str(c[3][0]) + ', ' + str(c[3][1]) + ', ' + str(
c[3][2])
text += '\n'
text += '4: ' + str(c[4][0]) + '/' + str(c[4][3]) + ', ' + str(
c[4][1]) + '/' + str(c[4][4]) + ', ' + str(c[4][2]) + '/' + str(
c[4][5])
text += '\n'
text += '5: ' + str(c[5][0]) + ', ' + str(c[5][1]) + ', ' + str(
c[5][2]) + ', ' + str(c[5][3]) + ', ' + str(c[5][4]) + ', ' + str(
c[5][5]) + ', ' + str(c[5][6]) + ', ' + str(c[5][7])
f = file(self.imu_uid + '.txt', 'w')
f.write(text)
f.close()
