def work(self, input_items, output_items):
out = output_items[0]
# Get single IP packet from TUN Device
buf = self.tun_handler.read()
if self.use_ot_packets:
# Create smaller OT packet from IP packet
#packet = PacketHandler.to_ot(buf[4:])
packet = PacketHandler.to_ot(buf)
else:
#packet = dpkt.ip.IP(buf[4:])
packet = dpkt.ip.IP(buf)
# TODO: DEBUG message, remove in production
print "TUN Source: ", PacketHandler.show(packet)
# Write packet to output buffer
result = np.fromstring(str(packet), dtype=np.uint8)
print "Packetlänge source "+ str(len(result))
out[:len(result)] = result
# Write length tag to output buffer
self.add_item_tag(0, self.nitems_written(0),
pmt.string_to_symbol(self.len_tag_key),
pmt.from_long(len(result)),
pmt.PMT_NIL)
return len(result)
def __init__(self, device_path=None, baud_rate=57600):
self.device_path = device_path
self.baud_rate = baud_rate
self.portHandler = PortHandler(self.device_path,
baudrate=self.baud_rate)
self.portHandler.openPort()
self.packetHandler = PacketHandler(XM430W350.PROTOCOL_VERSION)
def __init__(self, pkt, in_port, data, datapath, controller, federazione,
public_to_private_a, public_to_private_b):
PacketHandler.__init__(self, pkt)
self._pkt = pkt
self._datapath = datapath
self._in_port = in_port
self._data = data
self._controller = controller
self._federation = federazione
self._public_to_private_a = public_to_private_a
self._public_to_private_b = public_to_private_b
def __init__(self, pkt, in_port, data, datapath, controller, federazione,
public_to_private_a, public_to_private_b, pkt_dns):
PacketHandler.__init__(self, pkt)
self._pkt = pkt
self._pkt_dns = pkt_dns
self._datapath = datapath
self._in_port = in_port
self._data = data
self._controller = controller
self._federation = federazione
self._public_to_private_a = public_to_private_a
self._public_to_private_b = public_to_private_b
print '--------DNSResponseHandler-Alg1-----'
def __init__(self, pkt, dp_to_customer, in_port, data, datapath,
controller, federazione, public_to_private_a,
public_to_private_b):
PacketHandler.__init__(self, pkt)
self._pkt = pkt
self._dp_to_customer = dp_to_customer
self._federazione = federazione
self._controller = controller
self._in_port = in_port
self._datapath = datapath
self._data = data
self._public_to_private_a = public_to_private_a
self._public_to_private_b = public_to_private_b
self._public_address_src = None
self._public_address_dst = None
def handle_msg(self, msg):
buf = pmt.to_python(msg)[1]
# Cast numpy buf to string to prevent doomsday
buf = ''.join(map(chr, buf))
if self.use_ot_packets:
addr = self.tun_handler.tun.addr
dstaddr = self.tun_handler.tun.dstaddr
packet = PacketHandler.to_ip(buf, dstaddr, addr)
else:
packet = dpkt.ip.IP(buf)
# TODO: DEBUG message, remove in production
print "TUN Sink: ", PacketHandler.show(packet)
# Write packet to TUN Device
#print len(str(packet))
# TODO: IFF_NO_PI (remove 4 byte)
#self.tun_handler.write(' ' * 4 + str(packet))
self.tun_handler.write(str(packet))
def __init__(self, pkt, datapath, in_port, controller):
PacketHandler.__init__(self, pkt)
self._pkt = pkt
self._datapath = datapath
self._in_port = in_port
self._controller = controller
from properties import properties
print(properties)
#Command line arguments
timestamp = False
verbose = False
for argument in sys.argv:
if argument == '-v':
verbose = True
elif argument == '-t':
timestamp = True
#Remember: launch pigpio daemon (sudo pigpiod)
#declare packetHandler
packetHandler = PacketHandler(verbose)
#declare pigpio
pi = pigpio.pi()
#create servos
pan = Servo(pi, properties['pan_pin'], properties['starting_position'],
properties['min_position'], properties['max_position'],
properties['invert_x'])
#allow servos a second to connect
sleep(1)
def moveServos(values):
pan.moveTo(pi, values[0])
class XM430W350:
'''
XM430-W350 reference - http://emanual.robotis.com/docs/kr/dxl/x/xm430-w350/
'''
# define motor control address
ADDR_PRO_TORQUE_ENABLE = 64
ADDR_PROFILE_ACCELERATION = 108
ADDR_PROFILE_VELOCITY = 112
ADDR_PRO_GOAL_POSITION = 116
ADDR_PRO_PRESENT_POSITION = 132
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# const setting value
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 20
MINMUM_MOTOR_POSITION = 0
MAX_MOTOR_POSITION = 4095
MIN_ANGLE = 0
MAX_ANGLE = 359
def __init__(self, device_path=None, baud_rate=57600):
self.device_path = device_path
self.baud_rate = baud_rate
self.portHandler = PortHandler(self.device_path,
baudrate=self.baud_rate)
self.portHandler.openPort()
self.packetHandler = PacketHandler(XM430W350.PROTOCOL_VERSION)
def connectionTest(self, motor_id=1):
'''
@deprecated:
to check motor device path is valid and motor connection is vaild
'''
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PRO_TORQUE_ENABLE,
XM430W350.TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "connectionTest Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "connectionTest Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
def connection_test(self, motor_id=1):
'''
to check motor device path is valid and motor connection is vaild
'''
self.connectionTest(motor_id)
def setToqueEnable(self, motor_id=1, torqueEnable=True):
'''
@deprecated:
motor의 토크를 활성화 한다
'''
torqueState = int(torqueEnable)
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PRO_TORQUE_ENABLE,
torqueState)
error_msg = None
if dxl_comm_result != COMM_SUCCESS:
error_msg = "setToqueEnable Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "setToqueEnable Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
def set_toque_enable(self, motor_id=1, torqueEnable=True):
'''
motor의 토크를 활성화 한다
'''
self.setToqueEnable(motor_id, torqueEnable)
def setProfileVelocity(self, motor_id=1, profileVelocity=50):
'''
@deprecated:
0 is unlimited
value range 0 ~ 32767
max value 32767
'''
if profileVelocity < 0:
profileVelocity = 0
if profileVelocity > 32767:
profileVelocity = 32767
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PROFILE_VELOCITY,
profileVelocity)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "setProfileVelocity Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "setProfileVelocity Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
def set_profile_velocity(self, motor_id=1, profile_velocity=50):
'''
0 is unlimited
value range 0 ~ 32767
max value 32767
'''
self.setProfileVelocity(motor_id, profile_velocity)
def setProfileAcceleration(self, motor_id=1, profileAcceleration=50):
'''
@deprecated:
0 is unlimited
value range 0 ~ 32767
max value 32767
'''
if profileAcceleration < -1:
profileAcceleration = 0
if profileAcceleration > 32767:
profileAcceleration = 32767
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PROFILE_ACCELERATION,
profileAcceleration)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "setProfileVelocity Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "setProfileVelocity Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
def set_profile_acceleration(self, motor_id=1, profile_acceleration=50):
'''
0 is unlimited
value range 0 ~ 32767
max value 32767
'''
self.setProfileAcceleration(motor_id, profile_acceleration)
def setMotorAngle(self, motor_id=1, angle=0, angleReverse=False):
'''
@deprecated:
set motor angle range -180 ~ 179
angleReverse is rotate direction
'''
if angleReverse:
angle = angle * -1
if angle < -180:
angle = -180
if angle > 179:
angle = 179
transformAngle = angle + 180
robotisAngle = int(
interp(transformAngle, [
XM430W350.MIN_ANGLE, XM430W350.MAX_ANGLE
], [XM430W350.MINMUM_MOTOR_POSITION, XM430W350.MAX_MOTOR_POSITION
]))
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PRO_GOAL_POSITION,
robotisAngle)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "setMotorAngle Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "setMotorAngle Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
while True:
# Read present position
dxl_present_position, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx(
self.portHandler, motor_id,
XM430W350.ADDR_PRO_PRESENT_POSITION)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "while moving motor, error occured - {0}, device_path : {1}, and motor id : {2}, input angle : {3}, robotis angle : {4}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id, angle, robotisAngle)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "while moving motor, error occured - {0}, device_path : {1}, and motor id : {2}, input angle : {3}, robotis angle : {4}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id, angle, robotisAngle)
raise Exception(error_msg)
# waiting for reach robotisAngle
if not abs(robotisAngle - dxl_present_position
) > XM430W350.DXL_MOVING_STATUS_THRESHOLD:
break
def set_motor_angle(self, motor_id=1, angle=0, angle_reverse=False):
'''
set motor angle range -180 ~ 179
angle_reverse is rotate direction
'''
self.setMotorAngle(motor_id, angle, angle_reverse)
def getPresentMotorAngle(self, motor_id=1, angleReverse=False):
'''
@deprecated:
get motor angle
return value rangle -180 ~ 179
angleReverse is rotate direction
'''
dxl_present_position, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx(
self.portHandler, motor_id, XM430W350.ADDR_PRO_PRESENT_POSITION)
if dxl_comm_result != COMM_SUCCESS:
error_msg = "getPresentMotorAngle Error - {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getTxRxResult(dxl_comm_result),
self.device_path, motor_id)
raise Exception(error_msg)
elif dxl_error != 0:
error_msg = "getPresentMotorAngle Error- {0}, device_path : {1}, and motor id : {2}".format(
self.packetHandler.getRxPacketError(dxl_error),
self.device_path, motor_id)
raise Exception(error_msg)
angle = int(
interp(dxl_present_position, [
XM430W350.MINMUM_MOTOR_POSITION, XM430W350.MAX_MOTOR_POSITION
], [XM430W350.MIN_ANGLE, XM430W350.MAX_ANGLE]))
angle = angle - 180
if angleReverse:
angle = angle * -1
return angle
def get_present_motor_angle(self, motor_id=1, angle_reverse=False):
'''
get motor angle
return value rangle -180 ~ 179
angle_reverse is rotate direction
'''
return self.getPresentMotorAngle(motor_id, angle_reverse)
## Initiate global variables
## Perhaps create a dictionary for all the attributes of a specific motor
# import modules
import os # for key presses, reading and writing
from packet_handler import PacketHandler
from port_handler import PortHandler
import AX_constants as AX
# Initialize PortHandler instance
# Set the port path
portHandler = PortHandler(AX.DEVICE_PORT)
# Initialize PacketHandler instance
# Set the protocol version
packetHandler = PacketHandler(AX.DEVICE_PROTOCOL)
def getch():
"""Arguments:
Return: ch
Description: commands to get key presses
"""
if os.name == 'nt':
import msvcrt
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
