def keep_position(target,pose):
print("target= "),
print(target),
print("pose= "),
print(pose)
if abs(pose[0] - target[0])< 3:
pitchAdd = 0
else:
pitchAdd = target[0] - pose[0]
if abs(pose[2] - target[2])< 3:
yawAdd = 0
else:
yawAdd = target[2] - pose[2]
global current_pitch
global current_yaw
offset = pid.calc_pid(target,pose)#得到输出的偏移值
print(offset)
if (abs(current_pitch) < head_client.soft_limit):
current_pitch -= offset[0]#修改pitch输出
if (abs(current_yaw) < head_client.soft_limit):
current_yaw -= offset[2]#修改yaw输出
print("command yaw:"),
print(current_yaw),
print("command pitch"),
print(current_pitch)
# sync_write_angel(1,current_yaw,2,current_pitch)
head_client.sync_write_angel_client(current_yaw,current_pitch,0)
pub_servo.publish(current_pitch,current_yaw)
def handle_head_control(req): pitch = (req.pitch)/2 yaw = (req.yaw)/2 head_client.sync_write_angel_client(yaw,pitch,0) pub_servo.publish(pitch,yaw) init_PID = req.PID return 0
def keep_position(target, pose):
print("target= "),
print(target),
print("pose= "),
print(pose)
if abs(pose[0] - target[0]) < 3:
pitchAdd = 0
else:
pitchAdd = target[0] - pose[0]
if abs(pose[2] - target[2]) < 3:
yawAdd = 0
else:
yawAdd = target[2] - pose[2]
global current_pitch
global current_yaw
offset = pid.calc_pid(target, pose) #得到输出的偏移值
print(offset)
if (abs(current_pitch) < head_client.soft_limit):
current_pitch -= offset[0] #修改pitch输出
if (abs(current_yaw) < head_client.soft_limit):
current_yaw -= offset[2] #修改yaw输出
print("command yaw:"),
print(current_yaw),
print("command pitch"),
print(current_pitch)
# sync_write_angel(1,current_yaw,2,current_pitch)
head_client.sync_write_angel_client(current_yaw, current_pitch, 0)
pub_servo.publish(current_pitch, current_yaw)
def handle_head_control(req):
pitch = (req.pitch) / 2
yaw = (req.yaw) / 2
head_client.sync_write_angel_client(yaw, pitch, 0)
pub_servo.publish(pitch, yaw)
init_PID = req.PID
return 0
def handle_head_control(req):
pitch = (req.pitch)
yaw = (req.yaw)
head_client.sync_write_angel_client(yaw, pitch, 0)
return 0
# 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)