diff --git a/example/wireless_controller/wireless_controller.py b/example/wireless_controller/wireless_controller.py index 0596d1a..995487f 100644 --- a/example/wireless_controller/wireless_controller.py +++ b/example/wireless_controller/wireless_controller.py @@ -1,55 +1,131 @@ import time import sys +import struct + from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize -from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_ -from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_ +# Uncomment the following two lines when using Go2、Go2-W、B2、B2-W、H1 robot +# from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowState_ +# from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ + +# Uncomment the following two lines when using G1、H1-2 robot +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ + +class unitreeRemoteController: + def __init__(self): + # key + self.Lx = 0 + self.Rx = 0 + self.Ry = 0 + self.Ly = 0 + + # button + self.L1 = 0 + self.L2 = 0 + self.R1 = 0 + self.R2 = 0 + self.A = 0 + self.B = 0 + self.X = 0 + self.Y = 0 + self.Up = 0 + self.Down = 0 + self.Left = 0 + self.Right = 0 + self.Select = 0 + self.F1 = 0 + self.F3 = 0 + self.Start = 0 + + def parse_botton(self,data1,data2): + self.R1 = (data1 >> 0) & 1 + self.L1 = (data1 >> 1) & 1 + self.Start = (data1 >> 2) & 1 + self.Select = (data1 >> 3) & 1 + self.R2 = (data1 >> 4) & 1 + self.L2 = (data1 >> 5) & 1 + self.F1 = (data1 >> 6) & 1 + self.F3 = (data1 >> 7) & 1 + self.A = (data2 >> 0) & 1 + self.B = (data2 >> 1) & 1 + self.X = (data2 >> 2) & 1 + self.Y = (data2 >> 3) & 1 + self.Up = (data2 >> 4) & 1 + self.Right = (data2 >> 5) & 1 + self.Down = (data2 >> 6) & 1 + self.Left = (data2 >> 7) & 1 + + def parse_key(self,data): + lx_offset = 4 + self.Lx = struct.unpack('> i + print("WARNING: Please ensure there are no obstacles around the robot while running this example.") + input("Press Enter to continue...") - print(key_state) - - - -if __name__ == "__main__": if len(sys.argv)>1: ChannelFactoryInitialize(0, sys.argv[1]) else: ChannelFactoryInitialize(0) - - sub = ChannelSubscriber("rt/wirelesscontroller", WirelessController_) - sub.Init(WirelessControllerHandler, 10) - - while True: - time.sleep(10.0) + + custom = Custom() + custom.Init() + + while True: + time.sleep(1) \ No newline at end of file