def get_average_IMU(num):
pose = [0, 0, 0]
for num in range(1, num + 1):
pose = map(add, pose, readIMU.readData())
pose[:] = [x / num for x in pose]
return pose
def get_average_IMU(num): pose = [0,0,0] for num in range(1,num+1): pose = map(add, pose, readIMU.readData()) pose[:] = [x / num for x in pose] return pose
# sync_write_angel(1,current_yaw,2,current_pitch)
#读num组数据做平均做出初始值
def get_average_IMU(num):
pose = [0, 0, 0]
for num in range(1, num + 1):
pose = map(add, pose, readIMU.readData())
pose[:] = [x / num for x in pose]
return pose
#主函数
if __name__ == '__main__':
target = [0, 0, 0] #pitch roll yaw,其中roll没用
#EX106.syncWrite(0x1E,generate_servo(1,init_pitch),generate_servo(2,init_yaw))
#sync_write_angel(1,init_yaw,2,init_pitch)
head_client.sync_write_angel_client(init_yaw, init_pitch, 0)
for num in range(1, 100): #读200组数据扔掉
pose = readIMU.readData()
target = get_average_IMU(10) #读10组数据做平均做出初始值
while True:
temp = get_average_IMU(2) #读4组数据做平均作为当前姿态
pub.publish(head_pose(temp[2], temp[0]))
readIMU.flush()
keep_position(target, temp)
print("command pitch"),
print(current_pitch)
# sync_write_angel(1,current_yaw,2,current_pitch)
#读num组数据做平均做出初始值
def get_average_IMU(num):
pose = [0,0,0]
for num in range(1,num+1):
pose = map(add, pose, readIMU.readData())
pose[:] = [x / num for x in pose]
return pose
#主函数
if __name__ == '__main__':
target = [0,0,0]#pitch roll yaw,其中roll没用
#EX106.syncWrite(0x1E,generate_servo(1,init_pitch),generate_servo(2,init_yaw))
#sync_write_angel(1,init_yaw,2,init_pitch)
head_client.sync_write_angel_client(init_yaw,init_pitch,0)
for num in range(1,100): #读200组数据扔掉
pose = readIMU.readData()
target = get_average_IMU(10) #读10组数据做平均做出初始值
while True:
temp = get_average_IMU(2) #读4组数据做平均作为当前姿态
pub.publish(head_pose(temp[2],temp[0]))
readIMU.flush()
keep_position(target,temp)