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