def moveAngle(thetar, V, dir):
if thetar < -75: thetar = -75
if thetar > 75: thetar = 75
theta = thetar / 180 * math.pi
if V > 0: dir = 0
elif V < 0: dir = 1
V = abs(V)
if V > 200: V = 200
elif V < 0: V = 0
serial_connection = Connection(port='/dev/ttyUSB0', baudrate=1000000)
tp = computeAngles(theta, V)
pR = tp[0]
pL = tp[1]
vR = tp[2]
vL = tp[3]
serial_connection.goto(2, pR, speed=250, degrees=False)
serial_connection.goto(4, pL, speed=250, degrees=False)
#vR = 200 if vR>200 else vR
#vR = 0 if vR<0 else vR
#if vL>200: vL = 200;
#if vL<0: vL=0;
serial_connection.set_cw_angle_limit(1, 0)
serial_connection.set_ccw_angle_limit(1, 0)
#set to wheel mode - CW and CCW angles limits to zeros sets wheel mode
serial_connection.write_data(
1, 32, pyax12.utils.int_to_little_endian_bytes(vR + 1024 * dir))
serial_connection.set_cw_angle_limit(3, 0)
serial_connection.set_ccw_angle_limit(1, 0)
serial_connection.write_data(
3, 32, pyax12.utils.int_to_little_endian_bytes(vL + 1024 * (1 - dir)))
serial_connection.flush()
serial_connection.close()
class Emu():
"""Klasse til at kontrollere Dynamixel AX12 actuators"""
def start(self):
"""Metode til at finde USB-porten, fx COM12 på Win, ttyUSB0 på linux"""
#self.sc = Connection(port="/dev/ttyUSB0", baudrate=57600)
self.sc = Connection(port="/dev/ttyUSB0", baudrate=1000000)
#self.sc = Connection(port="COM12", baudrate=1000000)
self.sc.flush()
def scanUnits(self):
"""Scanner dynamixel motorer og returnere deres id'er i en liste"""
ids = self.sc.scan()
return ids
def readDxl(self, ID):
"""Printer oplysninger motoren med ID"""
self.sc.flush()
self.sc.pretty_print_control_table(ID)
def jointMode(self, ID):
"""Skifter motoren med ID til joint mode"""
self.sc.set_cw_angle_limit(ID, 0, False)
self.sc.set_ccw_angle_limit(ID, 1023, False)
def wheelMode(self, ID):
"""Skifter motoren med ID til wheelmode"""
self.sc.set_ccw_angle_limit(ID, 0, False)
self.sc.set_cw_angle_limit(ID, 0, False)
def moveJoint(self, ID, position):
"""Flytter motoren med ID til position"""
self.sc.goto(ID, position, speed=512, degrees=True)
time.sleep(1)
def moveWheel(self, ID, speed):
"""Starter en motor i wheelmode med hastigheden speed"""
if speed < 0:
if speed < -1024:
speed = 2047
else:
speed = 1023 + -speed
else:
if speed > 1023:
speed = 1023
self.sc.flush()
self.sc.goto(ID, 0, int(speed), degrees=False)
def stop(self):
"""Lukker forbindelsen gennem USB-porten til dynamixlerne"""
self.sc.close()
def getPos(self, ID):
"""Returnere motoren med ID's position"""
position = self.sc.get_present_position(ID, True)
return position
from pyax12.connection import Connection
servo_connection = Connection(port="/dev/ttyACM0", baudrate=1000000)
servo_connection.flush()
# Consult the robotics emanual for more infomation on how
# the dynamixel servos interpret communications.
# === JOINT FUNCTIONS === #
# Set up a dynamixel so that it behaves like joint.
def jointMode(ID):
servo_connection.set_cw_angle_limit(ID,0,False)
servo_connection.set_ccw_angle_limit(ID,1023,False)
# Move a dynamixel that has been set up as a joint.
def moveJoint(ID, position, speed):
servo_connection.goto(int(ID), int(position), int(speed), False)
# === WHEEL FUNCTIONS === #
# Set up a dynamixel so that it behaves like wheel.
def wheelMode(ID):
servo_connection.set_ccw_angle_limit(ID,0,False)
servo_connection.set_cw_angle_limit(ID,0,False)
# Move a dynamixel that has been set up as a wheel.
def moveWheel(ID, speed):
# Negative speed moves CW, positive speed moves CCW
# Convert negative values of speed to between 1024 and 2047
from pyax12.connection import Connection
import time
import threading
sc = Connection(port="/dev/ttyACM0", baudrate=1000000)
sc.flush()
# Set up a dynamixel so that it behaves like joint
def jointMode(ID):
sc.set_cw_angle_limit(ID,0,False)
sc.set_ccw_angle_limit(ID,1023,False)
# Set up a dynamixel so that it behaves like wheel
def wheelMode(ID):
sc.set_ccw_angle_limit(ID,0,False)
sc.set_cw_angle_limit(ID,0,False)
# Print useful information about an individual dynamixel servo
def readDxl(ID):
sc.flush()
sc.pretty_print_control_table(ID)
# Move a dynamixel that has been set up as a joint
def moveJoint(ID, position, speed):
#print(ID,position,speed)
sc.goto(int(ID), int(position), int(speed), False)
# Move a dynamixel that has been set up as a wheel
# Negative speed moves CW, positive speed moves CCW
def moveWheel(ID, speed):
# Convert negative values of speed to between 1024 and 2047
class Robot(object):
def __init__(self,
speed=150,
port='/dev/ttyACM0',
baudrate=1000000,
start_id=2,
debug=False):
self.speed = speed
self.connection = Connection(port=port, baudrate=baudrate)
self.legs = Legs(connection=self.connection,
speed=self.speed,
start_id=start_id)
self.debug = debug
self.connection.flush()
if (self.debug):
print("[debug] connected to port", port, "at", baudrate, baudrate)
def reset(self):
if (self.debug):
print("[debug] scanning")
self.connection.scan()
if (self.debug):
print("[debug] ready")
def close(self):
self.connection.close()
if (self.debug):
print("[debug] closed")
def stand(self):
if (self.debug):
print("[debug] stand")
self.legs.all.hip.goto(45)
self.legs.all.knee.goto(-60)
self.legs.all.ankle.goto(-30)
def kneel(self):
if (self.debug):
print("[debug] kneel")
self.legs.all.hip.goto(-45)
self.legs.all.knee.goto(-90)
self.legs.all.ankle.goto(-90)
def flat(self):
if (self.debug):
print("[debug] flat")
self.legs.all.hip.goto(90)
self.legs.all.knee.goto(0)
self.legs.all.ankle.goto(0)
def walk(self, steps=1, direction="forward", knee_up=False):
if (self.debug):
print("[debug] walk", direction, steps, "steps")
if knee_up == False:
walk = Walk(self)
else:
walk = Walk(self, knee_up=knee_up)
for i in range(0, steps):
walk.step_vertical(["fl", "rr"], "down")
walk.step_horizontal("fl", "rl", direction)
walk.step_horizontal("rr", "fl", direction)
walk.step_vertical(["fl", "rr"], "up")
walk.step_vertical(["fr", "rl"], "down")
walk.step_horizontal("fr", "rr", direction)
walk.step_horizontal("rl", "fr", direction)
walk.step_vertical(["fr", "rl"], "up")
def get_status(self):
servos = [2, 3, 5, 6, 8, 9, 11, 12]
min_voltage = 999
max_voltage = 0
sum_voltage = 0
good_count = 0
for servo in servos:
try:
if not self.connection.ping(servo):
print("ping failed servo", servo)
voltage = self.connection.get_present_voltage(servo)
if voltage > max_voltage:
max_voltage = voltage
elif voltage < min_voltage:
min_voltage = voltage
sum_voltage += voltage
good_count = good_count + 1
except:
print("[error] cannot connect to", servo)
if good_count > 0:
print(min_voltage, max_voltage, sum_voltage / good_count)
else:
print("no servos online")
that is based on a raspberry pi 3 model B, running 2 dynamixel ax12a servos. Your
mileage on different systems using different architectures will vary..
"""
############################################################ Connection handling ###########################################
#This is worth saying twice- connections are a BIG F*****G DEAL, they are difficult
#things to manage, and special handling is needed to make them reliable
from pyax12.connection import Connection
from serial import SerialException
#this is a connection string that may be a (the?) problem keeping this damn turret from working.... if this comment line is still on your screen, a fix is still commencing.
serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600)
print(serial_connection)
serial_connection.flush()
serial_connection.close()
#this connection function is in case of error of the connection above, which SHOULD be fine
#But "SHOULD" is a f*****g terrible garuntee.
def connect():
print("connect function is running!")
try:
# flush the buffer of the connection and see if that allows it to reassert itself
#serial_connection.flush()
serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600)
print(str(serial_connection.ping(1)))
# try: # try first with default port name (ttyACM0) before trying elsewhere
