thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Real teensy protocol

Browse source
This commit is contained in:
Thomas Muller 2021-09-08 22:35:29 +00:00
parent 754d5d5d4c
commit 5c294be8df
1 changed files with 18 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

33
src/cobot_platform/cobot_platform/move.py
View file

@ -8,9 +8,9 @@ from serial import Serial
from enum import Enum
class Servos(Enum):
BODY = 0
ARM = 1
HAND = 2
J1 = 'A'
J2 = 'B'
J3 = 'C'
class CobotMove(Node):
def __init__(self):
@ -25,24 +25,27 @@ class CobotMove(Node):
base_topic = 'move/'
self.create_subscription(Float32, base_topic+'body', self._move_body_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'arm', self._move_arm_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'hand', self._move_hand_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data)
def _move_body_cb(self, msg):
self._move_servo(Servos.BODY, msg.data)
self._serial.write(b'STA0.0.1\n')
print(self._serial.readline())
def _move_arm_cb(self, msg):
self._move_servo(Servos.ARM, msg.data)
def _move_j1_cb(self, msg):
self._move_servo(Servos.J1, msg.data)
def _move_hand_cb(self, msg):
self._move_servo(Servos.HAND, msg.data)
def _move_j2_cb(self, msg):
self._move_servo(Servos.J2, msg.data)
def _move_j3_cb(self, msg):
self._move_servo(Servos.J3, msg.data)
def _move_servo(self, serv_num, pos):
cmd = 'SV'
cmd = 'MT'
# self._buf[serv_num] = pos
self._logger.info('COBOT: Servo {serv_num} to {pos}')
self._serial.write(f'{cmd}V{serv_num}P{pos}\n'.encode('UTF-8'))
self._serial.write(f'{cmd}A{int(pos * 10000)}B0C0D0E0F0\n'.encode('UTF-8'))
print(self._serial.readline())
# def _push ?