Code: Select all
# FreeFormV05.py ChatGPT on 2026/04/07
# Python 2 utility for SSC-32U
# Features:
# a <cmd> - free-form send
# f <file> - load & send first line
# s <file> - save current pose
# <ch> <pw> - simple move
# NO automatic Q polling
import serial
import time
import re
ROBOT_PORT = "/dev/ttyUSB1"
BAUD = 9600
TIMEOUT = 0.2
# -------------------------------
# Global pose tracking
# -------------------------------
current_pose = [1500,1500,1500,1500,1500,1500]
# -------------------------------
# Utility functions
# -------------------------------
def to_hex(data):
return " ".join(["%02X" % ord(ch) for ch in data])
def to_printable(data):
out = []
for ch in data:
o = ord(ch)
if 32 <= o <= 126:
out.append(ch)
elif ch == "\r":
out.append("\\r")
elif ch == "\n":
out.append("\\n")
else:
out.append(".")
return "".join(out)
def show_response(label, data):
print label
if data:
print "ASCII:", to_printable(data)
print "HEX :", to_hex(data)
print "LEN :", len(data)
else:
print "ASCII: <none>"
print "HEX : <none>"
print "LEN : 0"
def read_available(ser, settle_time=0.1, max_bytes=256):
time.sleep(settle_time)
chunks = []
total = 0
while total < max_bytes:
n = ser.inWaiting()
if n <= 0:
break
chunk = ser.read(n)
if not chunk:
break
chunks.append(chunk)
total += len(chunk)
time.sleep(0.02)
return "".join(chunks)
def ensure_cr(cmd):
if cmd.endswith("\r"):
return cmd
if cmd.endswith("\n"):
cmd = cmd[:-1]
return cmd + "\r"
# -------------------------------
# Pose tracking
# -------------------------------
def update_pose_from_command(cmd):
global current_pose
matches = re.findall(r'#(\d+)P(\d+)', cmd)
for ch, pw in matches:
ch = int(ch)
pw = int(pw)
if 0 <= ch < len(current_pose):
current_pose[ch] = pw
def build_pose_command(T=2000):
cmd = ""
for i, val in enumerate(current_pose):
cmd += "#%dP%d " % (i, val)
cmd += "T%d" % T
return cmd
# -------------------------------
# Serial send
# -------------------------------
def send_and_report(ser, cmd, label):
print
print "Sending", label
print "ASCII:", to_printable(cmd)
print "HEX :", to_hex(cmd)
print "LEN :", len(cmd)
ser.write(cmd)
response = read_available(ser)
show_response(label + " response:", response)
# -------------------------------
# Main
# -------------------------------
def main():
global current_pose
try:
bot = serial.Serial(ROBOT_PORT, BAUD, timeout=TIMEOUT)
except Exception as e:
print "ERROR opening port:", str(e)
return
print "Connected to:", ROBOT_PORT
# Initial VER check
send_and_report(bot, "VER\r", "VER")
print
print "FreeFormV05"
print "Commands:"
print " a <cmd> free form"
print " f <file> run file"
print " s <file> save pose"
print " <ch> <pw> move servo"
print " 9 exit"
try:
while True:
print
user_input = raw_input("Next Command: ").strip()
if not user_input:
continue
if user_input == "9":
break
parts = user_input.split()
# ---------------------------
# FILE COMMAND
# ---------------------------
if parts[0].lower() == "f":
if len(parts) < 2:
print "Use: f <filename>"
continue
filename = " ".join(parts[1:])
try:
fh = open(filename, "r")
line = fh.readline()
fh.close()
if not line:
print "File empty"
continue
cmd = ensure_cr(line.strip())
update_pose_from_command(cmd)
send_and_report(bot, cmd, "file command")
except Exception as e:
print "File error:", str(e)
continue
# ---------------------------
# SAVE COMMAND
# ---------------------------
if parts[0].lower() == "s":
if len(parts) < 2:
print "Use: s <filename>"
continue
filename = " ".join(parts[1:])
try:
cmd = build_pose_command()
fh = open(filename, "w")
fh.write(cmd + "\n")
fh.close()
print "Saved pose to", filename
print cmd
except Exception as e:
print "Save error:", str(e)
continue
# ---------------------------
# FREE FORM
# ---------------------------
if parts[0].lower() == "a":
if len(parts) < 2:
print "Use: a <command>"
continue
cmd = ensure_cr(" ".join(parts[1:]))
update_pose_from_command(cmd)
send_and_report(bot, cmd, "free-form")
continue
# ---------------------------
# SIMPLE MOVE
# ---------------------------
if len(parts) == 2:
try:
ch = int(parts[0])
pw = int(parts[1])
if pw < 500: pw = 500
if pw > 2500: pw = 2500
current_pose[ch] = pw
cmd = "#%dP%d T2000\r" % (ch, pw)
send_and_report(bot, cmd, "move")
except:
print "Invalid input"
continue
print "Invalid command"
except KeyboardInterrupt:
print "\nExiting"
finally:
bot.close()
print "Port closed"
if __name__ == "__main__":
main() # 249