yanshee机器人代码

最新推荐文章于 2023-10-20 14:42:00 发布
阿巴em0
最新推荐文章于 2023-10-20 14:42:00 发布
阅读量2k
点赞数 5
文章标签： python
本文链接：https://blog.csdn.net/qq_44827929/article/details/127712425
版权
import YanAPI
import time
#初始化
ip_addr = "192.168.137.148" # please change to your yanshee robot IP
YanAPI.yan_api_init(ip_addr)
#数据定义区
dong=[{"RightAnkleUD":100,"LeftAnkleUD":100},
      {'RightAnkleFB': 110, 'RightAnkleUD': 105, 'RightHipFB': 14, 'RightHipLR': 60, 'RightKneeFlex': 33},
      {'LeftAnkleFB': 35, 'LeftAnkleUD': 113, 'LeftHipFB': 145, 'LeftHipLR': 81, 'LeftKneeFlex': 165, 'RightAnkleFB': 0, 'RightAnkleUD': 90, 'RightHipFB': 30, 'RightHipLR': 23, 'RightKneeFlex': 94,'RightElbowFlex': 126, 'RightShoulderFlex': 57, 'RightShoulderRoll': 90},
      {'LeftAnkleFB': 65, 'LeftAnkleUD': 104, 'LeftHipFB': 127, 'LeftHipLR': 82, 'LeftKneeFlex': 121, 'RightAnkleFB': 0, 'RightAnkleUD': 112, 'RightHipFB': 17, 'RightHipLR': 60, 'RightKneeFlex': 158},
      {'LeftAnkleFB': 34, 'LeftAnkleUD': 105, 'LeftHipFB': 149, 'LeftHipLR': 97, 'LeftKneeFlex': 177, 'RightAnkleFB': 100, 'RightAnkleUD': 112, 'RightHipFB': 25, 'RightHipLR': 90, 'RightKneeFlex': 150},
      {'LeftAnkleFB': 16, 'LeftAnkleUD': 100, 'LeftHipFB': 110, 'LeftHipLR': 87, 'LeftKneeFlex': 153, 'RightAnkleFB': 100, 'RightAnkleUD': 85, 'RightHipFB': 55, 'RightHipLR': 82, 'RightKneeFlex': 145},
      {'LeftAnkleFB': 17, 'LeftAnkleUD': 89, 'LeftHipFB': 53, 'LeftHipLR': 88, 'LeftKneeFlex': 86, 'RightAnkleFB': 41, 'RightAnkleUD': 95, 'RightHipFB': 32, 'RightHipLR': 85, 'RightKneeFlex': 118},
      {'LeftAnkleFB': 90, 'LeftAnkleUD': 70, 'LeftHipFB': 78, 'LeftHipLR': 80, 'LeftKneeFlex': 69, 'RightAnkleFB': 82, 'RightAnkleUD': 88, 'RightHipFB': 17, 'RightHipLR': 80, 'RightKneeFlex': 76, 'RightElbowFlex': 165, 'RightShoulderFlex': 140, 'RightShoulderRoll': 90},
      {'LeftAnkleFB': 90, 'LeftAnkleUD': 70, 'LeftHipFB': 60, 'LeftHipLR': 80, 'LeftKneeFlex': 70, 'RightAnkleFB': 100, 'RightAnkleUD': 75, 'RightHipFB': 30, 'RightHipLR': 88, 'RightKneeFlex': 80},
      {'LeftAnkleFB': 90, 'LeftAnkleUD': 77, 'LeftHipFB': 45, 'LeftHipLR': 93, 'LeftKneeFlex': 69, 'RightAnkleFB': 100, 'RightAnkleUD': 75, 'RightHipFB': 30, 'RightHipLR': 88, 'RightKneeFlex': 75},
      {'LeftAnkleFB': 177, 'LeftAnkleUD': 88, 'LeftHipFB': 156, 'LeftHipLR': 168, 'LeftKneeFlex': 109, 'RightAnkleFB': 97, 'RightAnkleUD': 68, 'RightHipFB': 40, 'RightHipLR': 94, 'RightKneeFlex': 73, 'LeftElbowFlex': 64, 'LeftShoulderFlex': 146, 'LeftShoulderRoll': 90},
      {'LeftAnkleFB': 98, 'LeftAnkleUD': 70, 'LeftHipFB': 134, 'LeftHipLR': 103, 'LeftKneeFlex': 84,'RightAnkleFB': 110, 'RightAnkleUD': 72, 'RightHipFB': 60, 'RightHipLR': 95, 'RightKneeFlex': 76,'LeftElbowFlex': 38, 'LeftShoulderFlex': 35, 'LeftShoulderRoll': 84},
      {'LeftAnkleFB': 71,'LeftAnkleUD': 90,'LeftElbowFlex': 15,'LeftHipFB': 120,'LeftHipLR': 90,'LeftKneeFlex': 104,'LeftShoulderFlex': 40,'LeftShoulderRoll': 90,'RightAnkleFB': 110,'RightAnkleUD': 90,'RightElbowFlex': 165,'RightHipFB': 60,'RightHipLR': 90,'RightKneeFlex': 75,'RightShoulderFlex': 140,'RightShoulderRoll': 90}]
you=[
    {'LeftAnkleFB': 66, 'LeftAnkleUD': 82, 'LeftHipFB': 113, 'LeftHipLR': 93, 'LeftKneeFlex': 102,'RightAnkleFB': 110, 'RightAnkleUD': 79, 'RightHipFB': 60, 'RightHipLR': 95, 'RightKneeFlex': 75},
    {'LeftAnkleFB': 84, 'LeftAnkleUD': 84, 'LeftHipFB': 132, 'LeftHipLR': 90, 'LeftKneeFlex': 101,'RightAnkleFB': 110, 'RightAnkleUD': 80, 'RightHipFB': 60, 'RightHipLR': 95, 'RightKneeFlex': 75},
    {'LeftAnkleFB': 66, 'LeftAnkleUD': 82, 'LeftHipFB': 113, 'LeftHipLR': 93, 'LeftKneeFlex': 102,'RightAnkleFB': 110, 'RightAnkleUD': 80, 'RightHipFB': 60, 'RightHipLR': 95, 'RightKneeFlex': 75}
]
zuo=[
    {'LeftAnkleFB': 70, 'LeftAnkleUD': 95, 'LeftElbowFlex': 15, 'LeftHipFB': 120, 'LeftHipLR': 86, 'LeftKneeFlex': 105, 'LeftShoulderFlex': 40, 'LeftShoulderRoll': 90, 'NeckLR': 90, 'RightAnkleFB': 110, 'RightAnkleUD': 90, 'RightElbowFlex': 165, 'RightHipFB': 60, 'RightHipLR': 89, 'RightKneeFlex': 75, 'RightShoulderFlex': 140, 'RightShoulderRoll': 90},
    {'LeftAnkleFB': 70, 'LeftAnkleUD': 95, 'LeftElbowFlex': 15, 'LeftHipFB': 120, 'LeftHipLR': 86, 'LeftKneeFlex': 105, 'LeftShoulderFlex': 40, 'LeftShoulderRoll': 90, 'NeckLR': 90, 'RightAnkleFB': 97, 'RightAnkleUD': 90, 'RightElbowFlex': 165, 'RightHipFB': 48, 'RightHipLR': 87, 'RightKneeFlex': 76, 'RightShoulderFlex': 140, 'RightShoulderRoll': 90},
    {'LeftAnkleFB': 70, 'LeftAnkleUD': 92, 'LeftElbowFlex': 15, 'LeftHipFB': 120, 'LeftHipLR': 86, 'LeftKneeFlex': 104, 'LeftShoulderFlex': 40, 'LeftShoulderRoll': 90, 'NeckLR': 90, 'RightAnkleFB': 120, 'RightAnkleUD': 92, 'RightElbowFlex': 165, 'RightHipFB': 67, 'RightHipLR': 87, 'RightKneeFlex': 73, 'RightShoulderFlex': 140, 'RightShoulderRoll': 90}
  ]
def run(flag): 
    while 1:
        now=YanAPI.get_sensors_gyro()['data']['gyro'][0]['euler-z']
        now= now if now>0 else now+360
        print(now)
        err=now-flag
        print(err)  
        if abs(err)<10:
            break
        elif err<0:
            play(zuo,200)
            # YanAPI.sync_play_motion(name="左转01")
            # time.sleep(1)
        elif err>0:
            play(you,200)
            # YanAPI.sync_play_motion(name="右转01")
            # time.sleep(1)
        

def play(dong,speed):
    for i in range(len(dong)):
        YanAPI.set_servos_angles(dong[i],speed)
        time.sleep(1)



def avold(bu,dire,flag):
    for i in range(bu):
        run(flag)
        YanAPI.sync_play_motion(name="walk",direction=dire,repeat=1)
        
def color():
    res = YanAPI.sync_do_color_recognition()
    color_dict = {"no color detected":"没有检测到颜色","pink":"粉","red":"红","green":"绿","blue":"蓝","yellow":"黄","cyan":"青","magenta":"洋红","orange":"橙","violet":"紫","brown":"棕","black":"黑","white":"白","gray":"灰"}
    result = res["data"]["color"][0]["name"]
    for key in color_dict:
        if(key == result):
            color_res = color_dict[key]
            return(color_res+"色")

# flag=YanAPI.get_sensors_gyro()['data']['gyro'][0]['euler-z']
# YanAPI.start_voice_tts("开始人脸识别",False)  
# YanAPI.start_play_music('pai.mp3')
# res=YanAPI.sync_do_face_recognition_value('recognition')    
# YanAPI.start_voice_tts("这个人是"+res,False)   
# YanAPI.start_voice_tts("以完成人脸识别是否前进",False)
# while 1:
#     if YanAPI.sync_do_voice_asr_value()['question']=="开始":
#         break
# YanAPI.sync_play_motion(name="walk",direction='forward',repeat=3)
# res=YanAPI.get_sensors_infrared_value()
# run(flag)
# if res<500:
#     YanAPI.start_voice_tts("检测到障碍物距离"+str(res),False)
#     avold(6,'left',flag)
#     avold(6,'forward',flag-10)
#     avold(9,'right',flag)
# else:
#     YanAPI.start_voice_tts("未检测到障碍物",False)
#     avold(6,'forward',flag-10)

# run(flag)
# YanAPI.sync_play_motion()
# time.sleep(2)
# play(dong,1000)
# avold(3,'forward',flag-30)

# ***********第二部分**********#

flag=YanAPI.get_sensors_gyro()['data']['gyro'][0]['euler-z']
flag= flag if flag>0 else flag+360
# YanAPI.sync_play_motion('拍照')
# YanAPI.start_play_music('pai.mp3')
# res=color()
# YanAPI.sync_play_motion()
# YanAPI.start_voice_tts("检测到"+res+"物体请问放到几号区域", False)
# time.sleep(3)
# YanAPI.sync_play_motion()
# while 1:
#     hao=YanAPI.sync_do_voice_asr_value()['question']
#     if hao=='3号'or hao=='2号'or hao=='1号':
#         break
# YanAPI.start_voice_tts("收到指令，物品箱编号为"+hao,False)
# YanAPI.sync_play_motion('报箱子')

# avold(20,'right',247)
run(154)