commit a3134a7f0ac903b4a6b46b04c520d330b25c815f Author: biqu Date: Tue Sep 24 16:28:47 2024 -0400 JC MQTT app configuration diff --git a/MX64.json b/MX64.json new file mode 100644 index 0000000..54fed12 --- /dev/null +++ b/MX64.json @@ -0,0 +1,382 @@ +{ + "Protocol_1": { + "Control_Table": { + "Model_Number": [ + 0, + 2 + ], + "Firmware_Version": [ + 2, + 1 + ], + "ID": [ + 3, + 1 + ], + "Baud_Rate": [ + 4, + 1 + ], + "Return_Delay_Time": [ + 5, + 1 + ], + "CW_Angle_Limit": [ + 6, + 2 + ], + "CCW_Angle_Limit": [ + 8, + 2 + ], + "Temperature_Limit": [ + 11, + 1 + ], + "Min_Voltage_Limit": [ + 12, + 1 + ], + "Max_Voltage_Limit": [ + 13, + 1 + ], + "Max_Torque": [ + 14, + 2 + ], + "Status_Return_Level": [ + 16, + 1 + ], + "Alarm_LED": [ + 17, + 1 + ], + "Shutdown": [ + 18, + 1 + ], + "Multi_Turn_Offset": [ + 20, + 2 + ], + "Resolution_Divider": [ + 22, + 1 + ], + "Torque_Enable": [ + 24, + 1 + ], + "LED": [ + 25, + 1 + ], + "D_Gain": [ + 26, + 1 + ], + "I_Gain": [ + 27, + 1 + ], + "P_Gain": [ + 28, + 1 + ], + "Goal_Position": [ + 30, + 2 + ], + "Moving_Speed": [ + 32, + 2 + ], + "Torque_Limit": [ + 34, + 2 + ], + "Present_Position": [ + 36, + 2 + ], + "Present_Speed": [ + 38, + 2 + ], + "Present_Load": [ + 40, + 2 + ], + "Present_Voltage": [ + 42, + 1 + ], + "Present_Temperature": [ + 43, + 1 + ], + "Registered": [ + 44, + 1 + ], + "Moving": [ + 46, + 1 + ], + "Lock": [ + 47, + 1 + ], + "Punch": [ + 48, + 2 + ], + "Realtime_Tick": [ + 50, + 2 + ], + "Current": [ + 68, + 2 + ], + "Torque_Ctrl_Mode_Enable": [ + 70, + 1 + ], + "Goal_Torque": [ + 71, + 2 + ], + "Goal_Acceleration": [ + 73, + 1 + ] + }, + "Values": { + "Min_Position": 0, + "Max_Position": 4095, + "Max_Angle": 360 + } + }, + "Protocol_2": { + "Control_Table": { + "Control_Table": { + "Model_Number": [ + 0, + 2 + ], + "Model_Information": [ + 2, + 4 + ], + "Firmware_Version": [ + 6, + 1 + ], + "ID": [ + 7, + 1 + ], + "Baud_Rate": [ + 8, + 1 + ], + "Return_Delay_Time": [ + 9, + 1 + ], + "Drive_Mode": [ + 10, + 1 + ], + "Operating_Mode": [ + 11, + 1 + ], + "Secondary_ID": [ + 12, + 1 + ], + "Protocol_Type": [ + 13, + 1 + ], + "Homing_Offset": [ + 20, + 4 + ], + "Moving_Threshold": [ + 24, + 4 + ], + "Temperature_Limit": [ + 31, + 1 + ], + "Max_Voltage_Limit": [ + 32, + 2 + ], + "Min_Voltage_Limit": [ + 34, + 2 + ], + "PWM_Limit": [ + 36, + 2 + ], + "Current_Limit": [ + 38, + 2 + ], + "Acceleration_Limit": [ + 40, + 4 + ], + "Velocity_Limit": [ + 44, + 4 + ], + "Max_Position_Limit": [ + 48, + 4 + ], + "Min_Position_Limit": [ + 52, + 4 + ], + "Shutdown": [ + 63, + 1 + ], + "Torque_Enable": [ + 64, + 1 + ], + "LED": [ + 65, + 1 + ], + "Return_Status_Level": [ + 68, + 1 + ], + "Registered_Instruction": [ + 69, + 1 + ], + "Hardware_Error_Status": [ + 70, + 1 + ], + "Velocity_I_Gain": [ + 76, + 2 + ], + "Velocity_P_Gain": [ + 78, + 2 + ], + "Position_D_Gain": [ + 80, + 2 + ], + "Position_I_Gain": [ + 82, + 2 + ], + "Position_P_Gain": [ + 84, + 2 + ], + "Feedforward_2nd_Gain": [ + 88, + 2 + ], + "Feedforward_1st_Gain": [ + 90, + 2 + ], + "BUS_Watchdog": [ + 98, + 1 + ], + "Goal_PWM": [ + 100, + 2 + ], + "Goal_Current": [ + 102, + 2 + ], + "Goal_Velocity": [ + 104, + 4 + ], + "Profile_Acceleration": [ + 108, + 4 + ], + "Profile_Velocity": [ + 112, + 4 + ], + "Goal_Position": [ + 116, + 4 + ], + "Realtime_Tick": [ + 120, + 2 + ], + "Moving": [ + 122, + 1 + ], + "Moving_Status": [ + 123, + 1 + ], + "Present_PWM": [ + 124, + 2 + ], + "Present_Current": [ + 126, + 2 + ], + "Present_Velocity": [ + 128, + 4 + ], + "Present_Position": [ + 132, + 4 + ], + "Velocity_Trajectory": [ + 136, + 4 + ], + "Position_Trajectory": [ + 140, + 4 + ], + "Present_Input_Voltage": [ + 144, + 2 + ], + "Present_Temperature": [ + 146, + 1 + ] + } + }, + "Values:": { + "Min_Position": 0, + "Max_Position": 4095, + "Max_Angle": 360 + } + } +} \ No newline at end of file diff --git a/XM540W270.json b/XM540W270.json new file mode 100644 index 0000000..a76bedc --- /dev/null +++ b/XM540W270.json @@ -0,0 +1,68 @@ +{ + "Protocol_2": { + "Control_Table": { + "Model_Number": [0, 2], + "Model_Information": [2, 4], + "Firmware_Version": [6, 1], + "ID": [7, 1], + "Baud_Rate": [8, 1], + "Return_Delay_Time": [9, 1], + "Drive_Mode": [10, 1], + "Operating_Mode": [11, 1], + "Secondary_Shadow_ID": [12, 1], + "Protocol_Type": [13, 1], + "Homing_Offset": [20, 4], + "Moving_Threshold": [24, 4], + "Temperature_Limit": [31, 1], + "Max_Voltage_Limit": [32, 2], + "Min_Voltage_Limit": [34, 2], + "PWM_Limit": [36, 2], + "Current_Limit": [38, 2], + "Velocity_Limit": [44, 4], + "Max_Position": [48, 4], + "Min_Position": [52, 4], + "External_Port_Mode_1": [56, 1], + "External_Port_Mode_2": [57, 1], + "External_Port_Mode_3": [58, 1], + "Startup_Configuration": [60, 1], + "Shutdown": [63, 1], + "Torque_Enable": [64, 1], + "LED": [65, 1], + "Status_Return_Level": [68, 1], + "Registered_Instruction": [69, 1], + "Hardware_Error_Status": [70, 1], + "Velocity_I_Gain": [76, 2], + "Velocity_P_Gain": [78, 2], + "Position_D_Gain": [80, 2], + "Position_I_Gain": [82, 2], + "Position_P_Gain": [84, 2], + "Feedforward_2nd_Gain": [88, 2], + "Feedforward_1st_Gain": [90, 2], + "Bus_Watchdog": [98, 1], + "Goal_PWM": [100, 2], + "Goal_Current": [102, 2], + "Goal_Velocity": [104, 4], + "Profile_Acceleration": [108, 4], + "Profile_Velocity": [112, 4], + "Goal_Position": [116, 4], + "Realtime_Tick": [120, 2], + "Moving": [122, 1], + "Moving_Status": [123, 1], + "Present_PWM": [124, 2], + "Present_Current": [126, 2], + "Present_Velocity": [128, 4], + "Present_Position": [132, 4], + "Velocity_Trajectory": [136, 4], + "Position_Trajectory": [140, 4], + "Present_Input_Voltage": [144, 2], + "Present_Temperature": [146, 1], + "Backup_Ready": [147, 1], + "External_Port_Data_1": [152, 2], + "External_Port_Data_2": [154, 2], + "External_Port_Data_3": [156, 2], + "Indirect_Address_1": [168, 2], + "Indirect_Address_2": [170, 2], + "Indirect_Address_3": [172, 2] + } + } +} diff --git a/mqtt_listener.py b/mqtt_listener.py new file mode 100644 index 0000000..9ccc960 --- /dev/null +++ b/mqtt_listener.py @@ -0,0 +1,88 @@ +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() + diff --git a/mqtt_listener.service b/mqtt_listener.service new file mode 100644 index 0000000..20a9946 --- /dev/null +++ b/mqtt_listener.service @@ -0,0 +1,15 @@ +[Unit] +Description=MQTT Listener Service +After=network.target + +[Service] +ExecStart=/home/biqu/mqtt_start.sh +WorkingDirectory=/home/biqu +StandardOutput=inherit +StandardError=inherit +Restart=always +User=biqu + +[Install] +WantedBy=multi-user.target + diff --git a/mqtt_start.sh b/mqtt_start.sh new file mode 100755 index 0000000..dbfe86b --- /dev/null +++ b/mqtt_start.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +# navigate to the directory of the script +cd /home/biqu/screw_control/ + +# activate the python environment, if you're using one +# source /path_to_your_venv/bin/activate + +# run the script +/usr/bin/python3 mqtt_listener.py