Advertisement
Guest User

Untitled

a guest
Sep 5th, 2021
67
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/python3 -B
  2. import numpy as np
  3. import asyncio
  4. import time, math
  5. import moteus
  6. import rospy
  7. from std_msgs.msg import String
  8. rospy.init_node('Pcan2moteus_for_ros', anonymous=True)
  9.  
  10. class CHAMPCONTROLLER: # a Class who let interact champ quadruped package with Ylo2, based on 12 moteus controllers
  11.     def __init__(self):
  12.         self.pcanBoard_initialized = False # bool variable will act as a condition for can ports initialization, or not ( if already initialized !)
  13.         self.position=0.1  # for now, only position is usefull on Champ package
  14.         self.velocity=0.01
  15.         self.maximum_torque=1.5
  16.         self.stop_position=math.nan
  17.         self.watchdog_timeout=math.nan
  18.         self.feedforward_torque=0.0
  19.         self.query=True
  20.         self.send_command_var_old = [math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan]
  21.         self.send_command_var_new = [0.0, 0.0, 0.0,   # FRONT LEFT
  22.                                       0.0, 0.0, 0.0, # FRONT RIGHT
  23.                                       0.0, 0.0, 0.0, # BACK LEFT
  24.                                       0.0, 0.0, 0.0] # BACK RIGHT
  25.  
  26.     async def InitializePcanPorts(self): # initialize the 4 ports of the Pcan M2 Board, only once, until shutdown.
  27.         while not self.pcanBoard_initialized:
  28.           # We create one 'moteus.Controller' instance for each servo
  29.           self.pcanBus1 = moteus.PythonCan(interface='pcan', channel='PCAN_PCIBUS1')
  30.           self.C1 = moteus.Controller(id=1, transport=self.pcanBus1)
  31.           self.C2 = moteus.Controller(id=2, transport=self.pcanBus1)
  32.           self.C3 = moteus.Controller(id=3, transport=self.pcanBus1)
  33.           print("  --- The 4 ports have been correctly initialized")
  34.           self.pcanBoard_initialized = True # pass thru the initialization process, if active
  35.  
  36.     async def stop_commands(self):
  37.         await self.pcanBus1.cycle([
  38.             self.C3.make_stop(),
  39.             self.C1.make_stop(),
  40.             self.C2.make_stop(),
  41.             ])
  42.         await asyncio.sleep(0.02)
  43.         print("        --- Motors are stopped, now\n")
  44.  
  45.     async def send_commands(self, jointTrajectory):
  46.         await self.InitializePcanPorts()
  47.         FL = await self.pcanBus1.cycle([
  48.           self.C1.make_position(position=jointTrajectory[0], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
  49.           self.C2.make_position(position=jointTrajectory[1], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
  50.           self.C3.make_position(position=jointTrajectory[2], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
  51.         ])
  52.         print("    --- Position orders are sent to motors")
  53.         for result in FL:
  54.           motor_id = {result.id}.pop() # extract the int id
  55.           try :
  56.             self.send_command_var_new[motor_id-1] = (f"{result.values[moteus.Register.POSITION]}")
  57.           except :
  58.             self.send_command_var_new[motor_id-1] = self.send_command_var_old[motor_id-1]
  59.         self.send_command_var_old = self.send_command_var_new # actualize list, to help an empty id request, pick the previous good value
  60.         print("    --- Position commands are queried :")
  61.         print(self.send_command_var_new)
  62.  
  63. app = CHAMPCONTROLLER()
  64.  
  65. async def main(commands):
  66.     print("Running Main() :")
  67.     await asyncio.sleep(0.02)
  68.     await app.send_commands(commands)
  69.     await app.InitializePcanPorts()
  70.     await app.stop_commands()
  71.    
  72. def callback(data):
  73.     commands = (data.data)
  74.     c = commands.split(',')
  75.     x = np.array(c)
  76.     y = x.astype(np.float)
  77.     asyncio.run(main(y), debug=True)
  78.  
  79. def pcan2moteus_node():
  80.   rospy.Subscriber("pcan2moteus", String, callback)
  81.   rospy.spin()
  82.  
  83. if __name__ == '__main__':
  84.     pcan2moteus_node()
Advertisement
RAW Paste Data Copied
Advertisement