From d8c53dab889eede7857e8c1ee13ed5a25b437132 Mon Sep 17 00:00:00 2001 From: wzx <17839623189@163.com> Date: Fri, 22 Dec 2023 14:59:28 +0800 Subject: [PATCH] update zkmetaapi --- ZKDogApi/zkmeta.py | 439 +++++++++++++++++------------------------ ZKDogApi/zkmetatest.py | 373 ++++++++++++++++++++++++++++++++++ 2 files changed, 558 insertions(+), 254 deletions(-) create mode 100644 ZKDogApi/zkmetatest.py diff --git a/ZKDogApi/zkmeta.py b/ZKDogApi/zkmeta.py index 261f016..4eeb857 100644 --- a/ZKDogApi/zkmeta.py +++ b/ZKDogApi/zkmeta.py @@ -1,43 +1,24 @@ -import sys +''' +Author: liu xiaoyan +Date: 2023-12-01 14:30:12 +LastEditors: WZX 17839623189@168.com +LastEditTime: 2023-12-05 17:01:44 +FilePath: /wzx/zkmetaapi/wukong/ZKMetaUnit/ZKMeta.py +''' +import os, sys import time +import math from typing import List -sys.path.append("../ZKMetaUnit") -import src.robot_interface as sdk -from utils import MQTT, Gyro, parse_robot_state -import math +from .utils import MQTT, parse_robot_state, calcu_vecloty, calcu_distance +import ZKMetaUnit.src.robot_interface as sdk + HIGHLEVEL = 0xee LOWLEVEL = 0xff -def calculate_movement(gyro_data, accel_data, time_step, initial_angle=0): - """ - Calculate the movement of an object based on gyroscope and accelerometer data. - - Parameters: - gyro_data (list): List of gyroscope data (in radians). - accel_data (list): List of accelerometer data (in m/s^2). - time_step (float): Time step in seconds. - initial_angle (float): Initial angle in radians. - - Returns: - (movement_x, movement_y, movement_z): The movement of the object in each axis. - """ - movement = [0, 0, 0] # x, y, z movement - angle = initial_angle # Current angle - - for i in range(len(gyro_data)): - if i == 0: - last_angle = angle - else: - angle += gyro_data[i] * time_step # Update angle - movement[2] += (angle - last_angle) * accel_data[i] * time_step # Calculate z movement using angle and acceleration - last_angle = angle - movement[0] += math.sin(angle) * accel_data[i] * time_step # Calculate x movement using angle and acceleration - movement[1] -= math.cos(angle) * accel_data[i] * time_step # Calculate y movement using angle and acceleration - return movement - class ZKDOG: def __init__(self) -> None: self.udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082) + self.MQTT = MQTT() self.cmd = sdk.HighCmd() self.state = sdk.HighState() self.udp.InitCmdData(self.cmd) @@ -52,8 +33,13 @@ class ZKDOG: self.cmd.yawSpeed = 0.0 self.cmd.reserve = 0 self.flag = 1 - self.mq = MQTT() - self.mq.run() + self.MQTT.run() + + def check_temp(self): + while True: + time.sleep(1) # Check every 1 second + with self.MQTT.lock: + return parse_robot_state(self.MQTT.bytes) def moving_unlocked(self, times: int = 1000): while times > 0: @@ -78,191 +64,141 @@ class ZKDOG: self.cmd.reserve = 0 - def forward(self, times: int = 1000, gaitType: int = 0, - velocaity: List=[0.1, 0], yawSpeed: int =0, footRaiseHeight: float=0.1): + def forward(self, distance: float = 1.0, gaitType: int = 0, + velocaity: List=[1, 0], yawSpeed: int =0, footRaiseHeight: float=0.1): + ''' + @ description: this is a function for moving forward and backward. + Parameters: + @distance: moving distance. + @gaitType: sport mode. Notice: Except not modification. + @velocaity: except max moving speed. + @yawSpeed: Except set the value=0 when moving. + @footRaiseHeight: Except not modification. + EXAMPLE 1: moving forward 0.1 m + zkdog = ZKDOG() + zkdog.forward(distance=0.1) + EXAMPLE 2: moving forward 10.1 m at 1m/s + zkdog.forward(distance=10.1, velocaity=[1, 0]) + EXAMPLE 3: moving backward 5m at 1m/s + zkdog.forward(distance=5, velocaity=[-1, 0]) + ''' - description: moving - @param times - @gaitType DO NOT MODIFICATION - @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. - Notice: do not modify the second value - @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 - 0 means do not rotate. - @footRaiseHeight float Foot elevation. Must be positive numbers. - return {*} - ''' if self.flag == 0: return False if self.flag == 1: self.moving_unlocked() - while times > 0: - time.sleep(0.05) - self.udp.Recv() - self.udp.GetRecv(self.state) - - self.recovery() - self.cmd.mode = 2 - self.cmd.gaitType = gaitType - self.cmd.velocity = velocaity # -1 ~ +1 - self.cmd.yawSpeed = yawSpeed - self.cmd.footRaiseHeight = footRaiseHeight - self.udp.SetSend(self.cmd) - self.udp.Send() - times -= 1 + with self.MQTT.lock: + past_dog_info = parse_robot_state(self.MQTT.bytes) + past_point = past_dog_info["position"] + current_point = past_point + if distance > 2: + while calcu_distance(current_point, past_point) < distance - 8*(math.pow(velocaity[0], 2)/12 + math.pow(velocaity[0], 2)/8): + time.sleep(0.1) + self.udp.Recv() + self.udp.GetRecv(self.state) + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + current_point = parse_robot_state(self.MQTT.bytes)["position"] + self.cmd.velocity = velocaity + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + current_point = parse_robot_state(self.MQTT.bytes)["position"] + time.sleep(2) + current_point = parse_robot_state(self.MQTT.bytes)["position"] + else: + while distance - calcu_distance(current_point, past_point) > 0.1: + time.sleep(0.1) + self.udp.Recv() + self.udp.GetRecv(self.state) + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + current_point = parse_robot_state(self.MQTT.bytes)["position"] + self.cmd.velocity = [0.15 if velocaity[0]>0 else -0.15, 0] + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + current_point = parse_robot_state(self.MQTT.bytes)["position"] + self.recovery() + self.udp.SetSend(self.cmd) + self.udp.Send() self.is_sitdown = False self.is_standup = True return True - - def move(self, tag: int = 50, gaitType: int = 0, - velocaity: List=[0.1, 0], yawSpeed: int = 0 , footRaiseHeight: float=0.1): - ''' - description: moving - @param times - @gaitType DO NOT MODIFICATION - @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. - Notice: do not modify the second value - @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 - 0 means do not rotate. - @footRaiseHeight float Foot elevation. Must be positive numbers. - return {*} - ''' - if self.flag == 0: - return False - if self.flag == 1: - self.moving_unlocked() - while tag > 0: - time.sleep(0.05) - - - with self.mq.lock: - robot_state = parse_robot_state(self.mq.bytes) - temp = robot_state["rpy"][2] - - print(robot_state) - gyro_data, accel_data = [], [] - gyro_data.append(self.state.imu.gyroscope[0]) - accel_data.append(self.state.imu.accelerometer[0]) - if tag ==1: - print(calculate_movement(gyro_data=gyro_data, accel_data=accel_data, time_step=0.05*tag)) - - self.udp.Recv() - self.udp.GetRecv(self.state) - - self.recovery() - self.cmd.mode = 2 - self.cmd.gaitType = gaitType - if tag <= 20: - self.cmd.velocity = 0.05 - else: - self.cmd.velocity = velocaity # -1 ~ +1 - self.cmd.yawSpeed = yawSpeed - self.cmd.footRaiseHeight = footRaiseHeight - self.udp.SetSend(self.cmd) - self.udp.Send() - tag -= 1 - self.is_sitdown = False - self.is_standup = True - return True - def turn_left(self, tag: int = 90, gaitType: int = 0, + def rolling(self, tag: int = 90, gaitType: int = 0, velocaity: List=[0, 0], yawSpeed: int = 1, footRaiseHeight: float=0.1): ''' - description: moving - @param times - @gaitType DO NOT MODIFICATION - @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. - Notice: do not modify the second value - @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 - 0 means do not rotate. - @footRaiseHeight float Foot elevation. Must be positive numbers. - return {*} - ''' - + @ description: rolling + Parameters: + @tag: Rotation Angle + @gaitType: sport mode, Notice Expect not modification + @velocaity: moving velocaity, Notice Expect not modification. + @yawSpeed: Arc velocity. Expect range from [-3, 3]. + @footRaiseHeight: Expect range from [0, 0.8] + + EXAMPLE: 180 degrees left at 1 rad/s + zkdog = ZKDOG() + zkdog.rolling(tag=180, yawSpeed=1) + ''' + if self.flag == 0: return False if self.flag == 1: self.moving_unlocked() - - with self.mq.lock: - robot_state = parse_robot_state(self.mq.bytes) - temp = robot_state["rpy"][2] - gyro = Gyro(yaw_start=temp) - - while gyro.get_corr_left_turn_angle() < tag: - time.sleep(0.05) - with self.mq.lock: - robot_state = parse_robot_state(self.mq.bytes) - gyro.update(robot_state["rpy"][2]) - - # with self.mq.lock: - # robot_state = parse_robot_state(self.mq.bytes) - # print("temp=>",temp,robot_state["rpy"], "step=>",gyro.get_corr_left_turn_angle(), "tag=>",tag, "self.cmd.yawSpeed", self.cmd.yawSpeed) - # print(f"左转角度: {gyro.get_corr_left_turn_angle()}，右转角度: {gyro.get_right_turn_angle()}") - + with self.MQTT.lock: + past_dog_info = parse_robot_state(self.MQTT.bytes) + past_yaw = past_dog_info["rpy"][-1] + current_yaw = past_yaw + param_rads = 180/math.pi + send_time = 0.1 + yaw_different = current_yaw - past_yaw + tmp_yawSpeed = yawSpeed + count = 0 + start_time = time.time() + while yaw_different < tag: self.udp.Recv() self.udp.GetRecv(self.state) self.recovery() self.cmd.mode = 2 self.cmd.gaitType = gaitType self.cmd.velocity = velocaity # -1 ~ +1 - - if tag-gyro.get_corr_left_turn_angle() <= 20: - self.cmd.yawSpeed = max((tag-gyro.get_corr_left_turn_angle())/20, 0.1) - else: + # 速度控制 + if tag-yaw_different > abs(yawSpeed)*param_rads * send_time*5: self.cmd.yawSpeed = yawSpeed + else: + tmp_yawSpeed = tmp_yawSpeed * 0.8 + self.cmd.yawSpeed = tmp_yawSpeed if abs(tmp_yawSpeed) > 0.1 else 0.1 if yawSpeed > 0 else -0.1 self.cmd.footRaiseHeight = footRaiseHeight self.udp.SetSend(self.cmd) self.udp.Send() - return True - - def turn_right(self, tag: int = 90, gaitType: int = 0, - velocaity: List=[0, 0], yawSpeed: int = -1, footRaiseHeight: float=0.1): - ''' - description: moving - @param times - @gaitType DO NOT MODIFICATION - @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. - Notice: do not modify the second value - @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 - 0 means do not rotate. - @footRaiseHeight float Foot elevation. Must be positive numbers. - return {*} - ''' - - if self.flag == 0: - return False - if self.flag == 1: - self.moving_unlocked() + time.sleep(send_time) + # 数值获取 + current_yaw = parse_robot_state(self.MQTT.bytes)["rpy"][-1] - with self.mq.lock: - robot_state = parse_robot_state(self.mq.bytes) - gyro = Gyro(yaw_start=robot_state["rpy"][2]) + if yawSpeed < 0: + yaw_different = (count+1)*360+past_yaw-current_yaw if past_yaw-current_yaw < 0 else past_yaw-current_yaw+count*360 - while gyro.get_corr_right_turn_angle() < tag: - time.sleep(0.05) - with self.mq.lock: - robot_state = parse_robot_state(self.mq.bytes) - gyro.update(robot_state["rpy"][2]) + else: + yaw_different = (count+1)*360+current_yaw-past_yaw if current_yaw-past_yaw < 0 else current_yaw - past_yaw+count*360 + end_time = time.time() + if end_time-start_time > 10 and past_yaw-2 <= current_yaw <= past_yaw+2: + count += 1 + start_time = time.time() - self.udp.Recv() - self.udp.GetRecv(self.state) - self.recovery() - self.cmd.mode = 2 - self.cmd.gaitType = gaitType - self.cmd.velocity = velocaity # -1 ~ +1 - - if tag-gyro.get_corr_right_turn_angle() <= 20: - self.cmd.yawSpeed = min(-(tag-gyro.get_corr_right_turn_angle())/20, -0.1) - else: - self.cmd.yawSpeed = yawSpeed - self.cmd.footRaiseHeight = footRaiseHeight - self.udp.SetSend(self.cmd) - self.udp.Send() - + self.recovery() + self.udp.SetSend(self.cmd) + self.udp.Send() + self.is_sitdown = False + self.is_standup = True return True - def sitdown(self): + def __sitdown(self): ''' description: sitdown return {*} @@ -278,14 +214,13 @@ class ZKDOG: time.sleep(0.002) # self.recovery() self.cmd.mode = sitdown_cmd[times//1000] - print(f"cmd mode is {self.cmd.mode}") times += 1 self.udp.SetSend(self.cmd) self.udp.Send() self.flag = 0 return True - def standup(self): + def __standup(self): ''' description: stand up return {*} @@ -300,74 +235,70 @@ class ZKDOG: self.cmd.mode = standup_cmd[times//1000] times += 1 self.udp.SetSend(self.cmd) - print(self.cmd.mode) self.udp.Send() self.flag = 1 return True def getcurrenrstate(self): return self.state.mode - + + def turn_left(self, tag=90): + try: + res = self.rolling(tag=tag) + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + + def turn_right(self, tag=90): + try: + res = self.rolling(tag=tag, yawSpeed = -1) + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + + def up(self, tag=1): + try: + res = self.forward(distance=tag) + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + + def down(self, tag=1): + try: + res = self.forward(distance=tag, velocaity=[-1, 0]) + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + + def bark(self, tag="bark"): + try: + tag="bark" + if not os.system(f"ssh unitree@192.168.123.13 -t 'aplay -D plughw:2,0 /home/unitree/tmp/{tag}'.wav"): + return {"type": True} + except Exception as e: + return {"type": False, "msg": e} + + def sitdown(self, tag=1): + try: + res = self.__sitdown() + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + + def standup(self, tag=1): + try: + res = self.__standup() + return {"type": res} + except Exception as e: + return {"type": False, "msg": e} + if __name__=="__main__": - go1 = ZKDOG() - - def get_args(k, v): - round = { - "turn-left": { - "times": v, - "velocaity": 0, - "yawSpeed": 1, - "footRaiseHeight": 0 - }, - "turn-right": { - "times": 90, - "velocaity": 0, - "yawSpeed": -0.5, - "footRaiseHeight": 0 - }, - "up": { - "times": int(450+750*float(v)), - "velocaity": 0.5, - "yawSpeed": 0, - "footRaiseHeight": 0.1 - }, - "down": { - "times": int(450+750*float(v)), - "velocaity": -0.5, - "yawSpeed": 0, - "footRaiseHeight": 0.1 - } - } - return round[k] - - def main(k, v): - # go1.sitdown() - # go1.standup() - # go1.forward(velocaity=[-0.2, 0], yawSpeed=-3) - new_args = get_args(k=k, v=v) - print(new_args) - times = new_args.get("times") - velocaity = new_args.get("velocaity") - yawSpeed = new_args.get("yawSpeed") - footRaiseHeight = new_args.get("footRaiseHeight") - res = go1.forward_copy(tag=times, velocaity=[velocaity, 0], yawSpeed=yawSpeed, - footRaiseHeight = footRaiseHeight) - - print(res) - # ************测试******************* - # go1.sitdown() - # go1.standup() - # # 前进 - # main(k="up", v=0.5) - # 左转 - # main(k="turn-left", v=180) - # go1.turn_right() - # go1.turn_left() - go1.move() - # 右转 - # main(k="turn-right", v=90) - # # 左转 - # main(k="turn-left", v=180) - # # 右转 - # main(k="turn-right", v=180) - + zd = ZKDOG() + + print(zd.down(tag=1)) + print(zd.turn_right(tag=180)) + print(zd.up(tag=1)) + print(zd.down(tag=1)) + # zd.sitdown() + # zd.standup() + # zd.bark() \ No newline at end of file diff --git a/ZKDogApi/zkmetatest.py b/ZKDogApi/zkmetatest.py new file mode 100644 index 0000000..261f016 --- /dev/null +++ b/ZKDogApi/zkmetatest.py @@ -0,0 +1,373 @@ +import sys +import time +from typing import List +sys.path.append("../ZKMetaUnit") +import src.robot_interface as sdk +from utils import MQTT, Gyro, parse_robot_state +import math +HIGHLEVEL = 0xee +LOWLEVEL = 0xff + +def calculate_movement(gyro_data, accel_data, time_step, initial_angle=0): + """ + Calculate the movement of an object based on gyroscope and accelerometer data. + + Parameters: + gyro_data (list): List of gyroscope data (in radians). + accel_data (list): List of accelerometer data (in m/s^2). + time_step (float): Time step in seconds. + initial_angle (float): Initial angle in radians. + + Returns: + (movement_x, movement_y, movement_z): The movement of the object in each axis. + """ + movement = [0, 0, 0] # x, y, z movement + angle = initial_angle # Current angle + + for i in range(len(gyro_data)): + if i == 0: + last_angle = angle + else: + angle += gyro_data[i] * time_step # Update angle + movement[2] += (angle - last_angle) * accel_data[i] * time_step # Calculate z movement using angle and acceleration + last_angle = angle + movement[0] += math.sin(angle) * accel_data[i] * time_step # Calculate x movement using angle and acceleration + movement[1] -= math.cos(angle) * accel_data[i] * time_step # Calculate y movement using angle and acceleration + return movement + +class ZKDOG: + def __init__(self) -> None: + self.udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082) + self.cmd = sdk.HighCmd() + self.state = sdk.HighState() + self.udp.InitCmdData(self.cmd) + self.udp.Recv() + self.udp.GetRecv(self.state) + self.cmd.gaitType = 0 + self.cmd.speedLevel = 0 + self.cmd.footRaiseHeight = 0 + self.cmd.bodyHeight = 0 + self.cmd.euler = [0, 0, 0] + self.cmd.velocity = [0, 0] + self.cmd.yawSpeed = 0.0 + self.cmd.reserve = 0 + self.flag = 1 + self.mq = MQTT() + self.mq.run() + + def moving_unlocked(self, times: int = 1000): + while times > 0: + time.sleep(0.002) + self.udp.Recv() + self.udp.GetRecv(self.state) + self.recovery() + self.cmd.mode = 6 + times -= 1 + self.udp.SetSend(self.cmd) + self.udp.Send() + + def recovery(self): + self.cmd.mode = 0 + self.cmd.gaitType = 0 + self.cmd.speedLevel = 0 + self.cmd.footRaiseHeight = 0 + self.cmd.bodyHeight = 0 + self.cmd.euler = [0, 0, 0] + self.cmd.velocity = [0, 0] + self.cmd.yawSpeed = 0.0 + self.cmd.reserve = 0 + + + def forward(self, times: int = 1000, gaitType: int = 0, + velocaity: List=[0.1, 0], yawSpeed: int =0, footRaiseHeight: float=0.1): + ''' + description: moving + @param times + @gaitType DO NOT MODIFICATION + @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. + Notice: do not modify the second value + @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 + 0 means do not rotate. + @footRaiseHeight float Foot elevation. Must be positive numbers. + return {*} + ''' + if self.flag == 0: + return False + if self.flag == 1: + self.moving_unlocked() + while times > 0: + time.sleep(0.05) + self.udp.Recv() + self.udp.GetRecv(self.state) + + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + self.cmd.velocity = velocaity # -1 ~ +1 + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + times -= 1 + self.is_sitdown = False + self.is_standup = True + return True + + + def move(self, tag: int = 50, gaitType: int = 0, + velocaity: List=[0.1, 0], yawSpeed: int = 0 , footRaiseHeight: float=0.1): + ''' + description: moving + @param times + @gaitType DO NOT MODIFICATION + @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. + Notice: do not modify the second value + @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 + 0 means do not rotate. + @footRaiseHeight float Foot elevation. Must be positive numbers. + return {*} + ''' + if self.flag == 0: + return False + if self.flag == 1: + self.moving_unlocked() + while tag > 0: + time.sleep(0.05) + + + with self.mq.lock: + robot_state = parse_robot_state(self.mq.bytes) + temp = robot_state["rpy"][2] + + print(robot_state) + gyro_data, accel_data = [], [] + gyro_data.append(self.state.imu.gyroscope[0]) + accel_data.append(self.state.imu.accelerometer[0]) + if tag ==1: + print(calculate_movement(gyro_data=gyro_data, accel_data=accel_data, time_step=0.05*tag)) + + self.udp.Recv() + self.udp.GetRecv(self.state) + + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + if tag <= 20: + self.cmd.velocity = 0.05 + else: + self.cmd.velocity = velocaity # -1 ~ +1 + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + tag -= 1 + self.is_sitdown = False + self.is_standup = True + return True + + def turn_left(self, tag: int = 90, gaitType: int = 0, + velocaity: List=[0, 0], yawSpeed: int = 1, footRaiseHeight: float=0.1): + ''' + description: moving + @param times + @gaitType DO NOT MODIFICATION + @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. + Notice: do not modify the second value + @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 + 0 means do not rotate. + @footRaiseHeight float Foot elevation. Must be positive numbers. + return {*} + ''' + + if self.flag == 0: + return False + if self.flag == 1: + self.moving_unlocked() + + with self.mq.lock: + robot_state = parse_robot_state(self.mq.bytes) + temp = robot_state["rpy"][2] + gyro = Gyro(yaw_start=temp) + + while gyro.get_corr_left_turn_angle() < tag: + time.sleep(0.05) + with self.mq.lock: + robot_state = parse_robot_state(self.mq.bytes) + gyro.update(robot_state["rpy"][2]) + + # with self.mq.lock: + # robot_state = parse_robot_state(self.mq.bytes) + # print("temp=>",temp,robot_state["rpy"], "step=>",gyro.get_corr_left_turn_angle(), "tag=>",tag, "self.cmd.yawSpeed", self.cmd.yawSpeed) + # print(f"左转角度: {gyro.get_corr_left_turn_angle()}，右转角度: {gyro.get_right_turn_angle()}") + + self.udp.Recv() + self.udp.GetRecv(self.state) + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + self.cmd.velocity = velocaity # -1 ~ +1 + + if tag-gyro.get_corr_left_turn_angle() <= 20: + self.cmd.yawSpeed = max((tag-gyro.get_corr_left_turn_angle())/20, 0.1) + else: + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + return True + + def turn_right(self, tag: int = 90, gaitType: int = 0, + velocaity: List=[0, 0], yawSpeed: int = -1, footRaiseHeight: float=0.1): + ''' + description: moving + @param times + @gaitType DO NOT MODIFICATION + @velocaity float The value interval is [-1.0, 1.0]. Positive numbers are forward, negative numbers are backward. + Notice: do not modify the second value + @yawSpeed int Rotational speed. Negative numbers are left turns, positive numbers are right turns。 + 0 means do not rotate. + @footRaiseHeight float Foot elevation. Must be positive numbers. + return {*} + ''' + + if self.flag == 0: + return False + if self.flag == 1: + self.moving_unlocked() + + with self.mq.lock: + robot_state = parse_robot_state(self.mq.bytes) + gyro = Gyro(yaw_start=robot_state["rpy"][2]) + + while gyro.get_corr_right_turn_angle() < tag: + time.sleep(0.05) + with self.mq.lock: + robot_state = parse_robot_state(self.mq.bytes) + gyro.update(robot_state["rpy"][2]) + + self.udp.Recv() + self.udp.GetRecv(self.state) + self.recovery() + self.cmd.mode = 2 + self.cmd.gaitType = gaitType + self.cmd.velocity = velocaity # -1 ~ +1 + + if tag-gyro.get_corr_right_turn_angle() <= 20: + self.cmd.yawSpeed = min(-(tag-gyro.get_corr_right_turn_angle())/20, -0.1) + else: + self.cmd.yawSpeed = yawSpeed + self.cmd.footRaiseHeight = footRaiseHeight + self.udp.SetSend(self.cmd) + self.udp.Send() + + return True + + def sitdown(self): + ''' + description: sitdown + return {*} + ''' + if self.flag == 0: + return False + + if self.flag == 1: + times = 0 + sitdown_cmd = [1, 6, 5, 7] + # 1 -> 6 -> 5 -> 7 + while times < 4000: + time.sleep(0.002) + # self.recovery() + self.cmd.mode = sitdown_cmd[times//1000] + print(f"cmd mode is {self.cmd.mode}") + times += 1 + self.udp.SetSend(self.cmd) + self.udp.Send() + self.flag = 0 + return True + + def standup(self): + ''' + description: stand up + return {*} + ''' + times = 0 + standup_cmd = [7, 5, 6] + if self.flag == 1: + return False + if self.flag == 0: + while times < 3000: + time.sleep(0.002) + self.cmd.mode = standup_cmd[times//1000] + times += 1 + self.udp.SetSend(self.cmd) + print(self.cmd.mode) + self.udp.Send() + self.flag = 1 + return True + + def getcurrenrstate(self): + return self.state.mode + +if __name__=="__main__": + go1 = ZKDOG() + + def get_args(k, v): + round = { + "turn-left": { + "times": v, + "velocaity": 0, + "yawSpeed": 1, + "footRaiseHeight": 0 + }, + "turn-right": { + "times": 90, + "velocaity": 0, + "yawSpeed": -0.5, + "footRaiseHeight": 0 + }, + "up": { + "times": int(450+750*float(v)), + "velocaity": 0.5, + "yawSpeed": 0, + "footRaiseHeight": 0.1 + }, + "down": { + "times": int(450+750*float(v)), + "velocaity": -0.5, + "yawSpeed": 0, + "footRaiseHeight": 0.1 + } + } + return round[k] + + def main(k, v): + # go1.sitdown() + # go1.standup() + # go1.forward(velocaity=[-0.2, 0], yawSpeed=-3) + new_args = get_args(k=k, v=v) + print(new_args) + times = new_args.get("times") + velocaity = new_args.get("velocaity") + yawSpeed = new_args.get("yawSpeed") + footRaiseHeight = new_args.get("footRaiseHeight") + res = go1.forward_copy(tag=times, velocaity=[velocaity, 0], yawSpeed=yawSpeed, + footRaiseHeight = footRaiseHeight) + + print(res) + # ************测试******************* + # go1.sitdown() + # go1.standup() + # # 前进 + # main(k="up", v=0.5) + # 左转 + # main(k="turn-left", v=180) + # go1.turn_right() + # go1.turn_left() + go1.move() + # 右转 + # main(k="turn-right", v=90) + # # 左转 + # main(k="turn-left", v=180) + # # 右转 + # main(k="turn-right", v=180) +