def connect_stm():
for port in list_ports.comports():
if (port.serial_number == SNR) and (port.pid == PID) and (port.vid
== VID):
port = '/dev/' + port.name
else:
print 'No STM32 found. Aborting'
computerPort = serialWrapper.SerialWrapper(port)
commands = packetBuilder.CommandsList()
return computerPort, commands
def create_connection_to_stm():
port_number = get_com_port_number()
return serialWrapper.SerialWrapper(port_number)
from serial.tools import list_ports
commands_to_stm = packetBuilder.CommandsList()
def get_com_port_number():
vid = 1155
pid = 22336
snr = '3677346C3034'
for port in list_ports.comports():
if (port.serial_number == snr) and (port.pid == pid) and (port.vid == vid):
return '/dev/' + port.name
port_number = get_com_port_number()
port = serialWrapper.SerialWrapper(port_number)
packet = packetBuilder.BuildPacket(commands_to_stm.openConeCrasher)
reply = port.sendRequest(packet.bytearray).reply
print reply
raw_input('PressEnter')
packet = packetBuilder.BuildPacket(commands_to_stm.closeConeCrasher)
reply = port.sendRequest(packet.bytearray).reply
print reply
raw_input('PressEnter')
while True:
angle = float(raw_input('Angle: '))
packet = packetBuilder.BuildPacket(commands_to_stm.setManipulatorAngle, angle)
def __init__(self, initialCoordinates):
portNumber = self.GetPortNumber()
#self.computerPort = serialWrapper.SerialWrapper("/dev/ttyACM2")
self.computerPort = serialWrapper.SerialWrapper(portNumber)
self.commands = packetBuilder.CommandsList()
self.InitializeRobot(initialCoordinates)
coordinates = [x, y, fi]
packet = packetBuilder.BuildPacket(commands.setCoordinates, coordinates)
recievedPacket = computerPort.sendRequest(packet.bytearray)
#if recievedPacket.reply != 'Ok':
# raise Exception('setCoordinates failed')
port = port_number()
if port:
print 'STM32 found on port %s' %port
else:
print 'No STM32 found. Aborting'
sys.exit()
# COM port initialization
computerPort = serialWrapper.SerialWrapper(port)
# we will choose commands which we want to send from this list
commands = packetBuilder.CommandsList()
# Initialize PID, Trajectory and Kinematics
initPTC()
while True:
print '\nList of available commands: \n1 Movement\n2 Get Coordinates',\
'\n3 Set Coordinates'
command = raw_input('Command number: ')
if command == '1':
movement()
elif command == '2':
pass
robot_local = (robot_global[0] + ruler[0], robot_global[1] + ruler[1])
return robot_local
def rotation(robot_local, angle):
"""Rotates robot points from left point sys to landmark coord sys"""
rot_mat = numpy.array([[math.cos(angle), -math.sin(angle)],
[math.sin(angle), math.cos(angle)]])
print 'Rot_mat: ', rot_mat
robot = numpy.asarray(robot_local)
product = numpy.dot(rot_mat, robot)
return product
# COM port initialization
computerPort = serialWrapper.SerialWrapper("/dev/ttyACM2")
# we will choose commands which we want to send from this list
commands = packetBuilder.CommandsList()
# Initialize PID, Trajectory and Kinematics
initPTC()
objectPosition = get_position.GetObjectPosition()
seashelWasNotDetected = True
#cap = cv2.VideoCapture(1)
while (seashelWasNotDetected):
time.sleep(5)
#currentCoordinates = getCurrentCoordinates()