102 lines
3.0 KiB
Python
102 lines
3.0 KiB
Python
import paho.mqtt.client as mqtt
|
|
import re
|
|
from dynio import *
|
|
import time
|
|
|
|
# State variable to check if HOME is received before POS
|
|
home_received = False
|
|
home = 0
|
|
dxl_io = []
|
|
motor = None
|
|
|
|
# Functions to run when the corresponding command is received
|
|
def home():
|
|
global motor, home
|
|
motor.write_control_table('Torque_Limit', 200)
|
|
motor.set_velocity_mode()
|
|
print('velocity mode set')
|
|
motor.torque_enable()
|
|
print('torque enabled')
|
|
motor.set_velocity(300)
|
|
print('velocity set')
|
|
time.sleep(1)
|
|
while motor.read_control_table('Present_Speed') > 0:
|
|
pass
|
|
motor.torque_disable()
|
|
home = motor.get_position()
|
|
motor.write_control_table('Multi_Turn_Offset', -1 * home)
|
|
motor.set_extended_position_mode()
|
|
motor.torque_enable()
|
|
|
|
def set_pos(num):
|
|
global motor
|
|
motor.set_position(num)
|
|
time.sleep(0.2)
|
|
while motor.read_control_table('Present_Speed') > 0:
|
|
pass
|
|
|
|
def disable_torque():
|
|
global motor, home_received
|
|
motor.torque_disable()
|
|
home_received = False
|
|
|
|
|
|
# The callback for when the client receives a CONNACK response from the server.
|
|
def on_connect(client, userdata, flags, rc):
|
|
print(f"Connected with result code {str(rc)}")
|
|
|
|
# Subscribing in on_connect() means that if we lose the connection and
|
|
# reconnect then subscriptions will be renewed.
|
|
client.subscribe("/meyer/#")
|
|
|
|
# Connect to Dynamixels
|
|
global dxl_io, motor
|
|
dxl_io = dxl.DynamixelIO('/dev/ttyACM0')
|
|
motor = dxl_io.new_mx106(1,1)
|
|
motor.get_position()
|
|
motor.write_control_table('Torque_Limit', 200)
|
|
|
|
|
|
# The callback for when a PUBLISH message is received from the server.
|
|
def on_message(client, userdata, msg):
|
|
global home_received, motor
|
|
|
|
message = msg.payload.decode()
|
|
if msg.topic != "/meyer/":
|
|
return
|
|
|
|
if message == "HOME":
|
|
home()
|
|
home_received = True
|
|
time.sleep(0.5)
|
|
set_pos(int(-250*53.248))
|
|
client.publish("/meyer/pos", -250.0)
|
|
elif message == "DISABLE":
|
|
disable_torque()
|
|
elif re.match(r"^POS -?\d+$", message): # This matches POS followed by a number
|
|
pos_num = int(message.split()[1]) # Extract the number from the message
|
|
pos_num *= 5.3248
|
|
|
|
if pos_num < -25000 or pos_num > 0: # Check if the number is out of range
|
|
client.publish("/meyer/error", "Error: Number out of range")
|
|
elif not home_received: # Check if HOME was received before POS
|
|
client.publish("/meyer/error", "Error: HOME command has to be run before POS")
|
|
else:
|
|
set_pos(int(pos_num))
|
|
client.publish("/meyer/pos", round(pos_num / 53.248,1))
|
|
else:
|
|
client.publish("/meyer/error", "Error: Unknown command")
|
|
|
|
|
|
client = mqtt.Client()
|
|
client.on_connect = on_connect
|
|
client.on_message = on_message
|
|
|
|
# Connect to the broker
|
|
client.connect("192.168.1.1", 1883, 60)
|
|
|
|
# Blocking call that processes network traffic, dispatches callbacks and
|
|
# handles reconnecting.
|
|
client.loop_forever()
|
|
|