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