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

Fixed gear ratios

Browse source
This commit is contained in:
Thomas Muller 2022-03-22 16:42:04 -04:00
parent 0e0629054c
commit 293f603a8b
1 changed files with 19 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -10,6 +10,13 @@ from math import pi
from serial import Serial from serial import Serial
class CobotMove(Node): class CobotMove(Node):
J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
def __init__(self): def __init__(self):
super().__init__('cobot_move') super().__init__('cobot_move')
# Set up a logger for the node # Set up a logger for the node
@ -51,22 +58,22 @@ class CobotMove(Node):
# Callbacks for move/jX subscriptions # Callbacks for move/jX subscriptions
def _move_j1_cb(self, msg): def _move_j1_cb(self, msg):
self._cur[0] = int(msg.data / pi / 2. * 8000 * 4) self._cur[0] = int(msg.data * J1_COUNTS_PER_RAD)
def _move_j2_cb(self, msg): def _move_j2_cb(self, msg):
self._cur[1] = int(msg.data / pi / 2. * 10000 * 4) self._cur[1] = int(msg.data * J2_COUNTS_PER_RAD)
def _move_j3_cb(self, msg): def _move_j3_cb(self, msg):
self._cur[2] = int(msg.data / pi / 2. * 10000) self._cur[2] = int(msg.data * J3_COUNTS_PER_RAD)
def _move_j4_cb(self, msg): def _move_j4_cb(self, msg):
self._cur[3] = int(msg.data / pi / 2. * 2000) self._cur[3] = int(msg.data * J4_COUNTS_PER_RAD)
def _move_j5_cb(self, msg): def _move_j5_cb(self, msg):
self._cur[4] = int(msg.data / pi / 2. * 2000) self._cur[4] = int(msg.data * J5_COUNTS_PER_RAD)
def _move_j6_cb(self, msg): def _move_j6_cb(self, msg):
self._cur[5] = int(msg.data / pi / 2. * 2000) self._cur[5] = int(msg.data * J6_COUNTS_PER_RAD)
# Send all current positions to the teensy # Send all current positions to the teensy
def _dosomething(self): def _dosomething(self):
@ -77,12 +84,12 @@ class CobotMove(Node):
# Parse the line into each angle # Parse the line into each angle
blocks = re.findall(r'[ABCDEF]-*\d+', line) blocks = re.findall(r'[ABCDEF]-*\d+', line)
a = float(blocks[0][1:]) / 8000. / 4. * pi * 2. a = float(blocks[0][1:]) / J1_COUNTS_PER_RAD
b = float(blocks[1][1:]) / 10000 / 4. * pi * 2. b = float(blocks[1][1:]) / J2_COUNTS_PER_RAD
c = float(blocks[2][1:]) / 10000. * pi * 2. c = float(blocks[2][1:]) / J3_COUNTS_PER_RAD
d = float(blocks[3][1:]) / 2000. * pi * 2. d = float(blocks[3][1:]) / J4_COUNTS_PER_RAD
e = float(blocks[4][1:]) / 2000. * pi * 2. e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
f = float(blocks[5][1:]) / 2000. * pi * 2. f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD
# Publish those to the update/jX subscribers # Publish those to the update/jX subscribers
self.s_j1.publish(Float32(data=a)) self.s_j1.publish(Float32(data=a))