89 lines
2.4 KiB
Python
89 lines
2.4 KiB
Python
import paho.mqtt.client as mqtt
|
|
import re
|
|
from dynio import *
|
|
import time
|
|
import threading
|
|
from statistics import mean
|
|
from collections import deque, Counter
|
|
|
|
# State variable to check if HOME is received before POS
|
|
home_received = False
|
|
dxl_io = []
|
|
feed_motor = None
|
|
|
|
def set_feed(num):
|
|
global feed_motor
|
|
feed_motor.set_velocity(75)
|
|
feed_motor.torque_enable()
|
|
feed_motor.set_position(int(convert_to_ticks(num)))
|
|
|
|
def disable_torque():
|
|
global feed_motor
|
|
feed_motor.torque_disable()
|
|
|
|
def convert_to_ticks(angle):
|
|
# Define the input range
|
|
min_input = 0
|
|
max_input = 90
|
|
|
|
# Define the output range
|
|
max_output = 1212
|
|
min_output = max_output - 1024
|
|
|
|
# Ensure the angle is within the input range
|
|
angle = max(min(angle, max_input), min_input)
|
|
|
|
# Perform the inverse linear transformation
|
|
value = ((angle - min_input) / (max_input - min_input)) * (max_output - min_output) + min_output
|
|
|
|
return value
|
|
|
|
# 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("/jc/feed")
|
|
|
|
# Connect to Dynamixels
|
|
global dxl_io, feed_motor
|
|
dxl_io = dxl.DynamixelIO('/dev/ttyS0')
|
|
feed_motor = dxl_io.new_mx64(2, 2)
|
|
feed_motor.get_position()
|
|
|
|
# The callback for when a PUBLISH message is received from the server.
|
|
def on_message(client, userdata, msg):
|
|
global home_received, feed_motor
|
|
|
|
message = msg.payload.decode()
|
|
|
|
pos_num = float(message) # Extract the number from the message and convert to float
|
|
set_feed(pos_num)
|
|
|
|
# Define a function to continuously report the log
|
|
def continuous_log_reporting():
|
|
global feed_motor
|
|
while True:
|
|
if feed_motor:
|
|
client.publish("/jc/feed/log", (feed_motor.get_position() - 188) * 90 / 1024)
|
|
time.sleep(1) # Adjust the sleep time as needed
|
|
|
|
# Start the continuous log reporting in a separate thread
|
|
log_thread = threading.Thread(target=continuous_log_reporting)
|
|
log_thread.daemon = True
|
|
log_thread.start()
|
|
|
|
|
|
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()
|
|
|