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()
