def openPort(self, port, baud=115200, interpolate=True):
try:
# TODO: add ability to select type of driver
self.port = Driver(port, baud, interpolate)
self.panel.port = self.port
self.panel.portUpdated()
self.sb.SetStatusText(port + '@' + str(baud), 1)
except BaseException:
self.port = None
self.sb.SetBackgroundColour('RED')
self.sb.SetStatusText('Could Not Open Port ' + port, 0)
self.sb.SetStatusText('not connected', 1)
self.timer.Start(20)
return self.port
class Arm(object):
def __init__(self, port="/dev/ttyUSB0"):
self.driver = Driver(port=port)
self.movement_engine = ArmMovementEngine()
#Closes the window the driver has control of
def close(self):
self.driver.close()
#Recenters the arm to its starting position
def recenter(self):
self.move((512, 512))
#Moves arm to its rest position
def return_to_rest(self):
self.move_to_point([20, 13.5])
#Moves arm to correct position
def move(self, goal_position):
start_position = self.current_position()
self.set_speed([MIN_SPEED, MIN_SPEED])
for i in SERVOS:
self.driver.setReg(
i, P_GOAL_POSITION_L,
[goal_position[i % 2] % 256, goal_position[i % 2] >> 8])
while self._is_moving():
position = self.current_position()
speed = [
_adjusted_speed(start_position[i % 2], goal_position[i % 2],
position[i % 2]) for i in SERVOS
]
self.set_speed(speed)
#Determines where robot needs to move
def move_to_point(self, pt):
goal_position = self.movement_engine.convert_point(pt)
self.move(goal_position)
#Sets the speed of the servos
def set_speed(self, speed):
for i in SERVOS:
self.driver.setReg(i, P_GOAL_SPEED_L,
[speed[i % 2] % 256, speed[i % 2] >> 8])
#Returns the current position of the servo
def current_position(self):
return self._values_for_register(P_PRESENT_POSITION_L)
#Checks if the servos are moving
def _is_moving(self):
return any(
[self.driver.getReg(index, P_MOVING, 1) == 1 for index in SERVOS])
#Returns the value in specified register
def _values_for_register(self, register):
return [
_register_bytes_to_value(self.driver.getReg(index, register, 2))
for index in SERVOS
]
class Arm(object):
def __init__(self, port="/dev/ttyUSB0"):
self.driver = Driver(port=port)
self.movement_engine = ArmMovementEngine()
def close(self):
self.driver.close()
def recenter(self):
self.move((512, 512))
def return_to_rest(self):
self.move_to_point([20, 13.5])
def move(self, goal_position):
start_position = self.current_position()
self.set_speed([MIN_SPEED, MIN_SPEED])
for i in SERVOS:
self.driver.setReg(
i, P_GOAL_POSITION_L,
[goal_position[i % 2] % 256, goal_position[i % 2] >> 8])
while self._is_moving():
position = self.current_position()
speed = [
_adjusted_speed(start_position[i % 2], goal_position[i % 2],
position[i % 2]) for i in SERVOS
]
self.set_speed(speed)
def move_to_point(self, pt):
goal_position = self.movement_engine.convert_point(pt)
self.move(goal_position)
def set_speed(self, speed):
for i in SERVOS:
self.driver.setReg(i, P_GOAL_SPEED_L,
[speed[i % 2] % 256, speed[i % 2] >> 8])
def current_position(self):
return self._values_for_register(P_PRESENT_POSITION_L)
def _is_moving(self):
return any(
[self.driver.getReg(index, P_MOVING, 1) == 1 for index in SERVOS])
def _values_for_register(self, register):
return [
_register_bytes_to_value(self.driver.getReg(index, register, 2))
for index in SERVOS
]
from pypose.driver import Driver from pypose.ax12 import P_MOVING, P_GOAL_SPEED_L import arm import gripper import time driver = Driver(port='/dev/tty.usbserial-AR0JW21B') # Import AX-12 register constants # Get "moving" register for servo with ID 1 #is_moving = driver.getReg(1, P_MOVING, 1) # Set the "moving speed" register for servo with ID 1 #speed = 50 # A number between 0 and 1023 #driver.setReg(1, P_GOAL_SPEED_L, [speed%256, speed>>8]) #driver.setReg(2,P_GOAL_SPEED_L, [speed%256, speed>>8]) g = gripper.Gripper() g.move(512) g.close()
def __init__(self, port="/dev/tty.usbserial-AR0JW21B"):
self.driver = Driver(port=port)
class Gripper(object):
def __init__(self, port="/dev/tty.usbserial-AR0JW21B"):
self.driver = Driver(port=port)
#self.movement_engine = ArmMovementEngine()
#Closes the window the driver has control of
def close(self):
self.driver.close()
#Recenters the arm to its starting position
def recenter(self):
self.move(512)
def grip(self):
self.move(900)
def release(self):
self.move(512)
#Moves arm to its rest position
#def return_to_rest(self):
# self.move_to_point([20, 13.5])
#Moves arm to correct position
def move(self, goal_position):
#print(goal_position)
time.sleep(0.2)
start_position = self.current_position()
#print(start_position)
self.set_speed(MIN_SPEED)
self.driver.setReg(SERVO_1, P_GOAL_POSITION_L, [goal_position%256, goal_position>>8])
while self._is_moving():
position = self.current_position()
speed = _adjusted_speed(start_position, goal_position, position)
self.set_speed(speed)
#Determines where robot needs to move
#def move_to_point(self, pt):
# goal_position = self.movement_engine.convert_point(pt)
# self.move(goal_position)
#Sets the speed of the servos
def set_speed(self, speed):
self.driver.setReg(SERVO_1, P_GOAL_SPEED_L, [speed%256, speed>>8])
#Returns the current position of the servo
def current_position(self):
#print(self._values_for_register(P_PRESENT_POSITION_L))
return self._values_for_register(P_PRESENT_POSITION_L)
#Checks if the servos are moving
def _is_moving(self):
return self.driver.getReg(SERVO_1, P_MOVING, 1) == 1
#Returns the value in specified register
def _values_for_register(self, register):
#for i in range(250,256):
# ret = self.driver.getReg(i, register, 2)
# if ret == -1:
# print(str(i) + "wrong id")
# else:
# print(ret)
# print(str(i) + " IS THE SERVO ID")
# break
return _register_bytes_to_value(self.driver.getReg(SERVO_1, register, 2))
def __init__(self, port="/dev/ttyUSB0"):
self.driver = Driver(port=port)
self.movement_engine = ArmMovementEngine()
class Editor(wx.Frame):
"""Implements the main editor window. """
ID_NEW = wx.NewIdRef()
ID_OPEN = wx.NewIdRef()
ID_SAVE = wx.NewIdRef()
ID_SAVE_AS = wx.NewIdRef()
ID_EXIT = wx.NewIdRef()
ID_EXPORT = wx.NewIdRef()
ID_RELAX = wx.NewIdRef()
ID_PORT = wx.NewIdRef()
ID_ABOUT = wx.NewIdRef()
ID_TEST = wx.NewIdRef()
ID_TIMER = wx.NewIdRef()
ID_COL_MENU = wx.NewIdRef()
ID_LIVE_UPDATE = wx.NewIdRef()
ID_2COL = wx.NewIdRef()
ID_3COL = wx.NewIdRef()
ID_4COL = wx.NewIdRef()
def __init__(self):
""" Creates pose editor window. """
wx.Frame.__init__(self,
None,
-1,
VERSION,
style=wx.DEFAULT_FRAME_STYLE)
# key data for our program
self.project = Project() # holds data for our project
self.panelIndex = dict() # existant tools
self.saveReq = False
self.panel = None
self.port = None
self.filename = ""
self.dirname = ""
self.columns = 2 # column count for pose editor
# for clearing red color on status bar
self.timer = wx.Timer(self, self.ID_TIMER)
self.timeout = 0
# build our menu bar
menubar = wx.MenuBar()
prjmenu = wx.Menu()
# dialog with name, # of servos
prjmenu.Append(self.ID_NEW, "&New\tCtrl+N", "", wx.ITEM_NORMAL)
prjmenu.Append(self.ID_OPEN, "&Open\tCtrl+O", "",
wx.ITEM_NORMAL) # open file dialog
prjmenu.AppendSeparator()
# if name unknown, ask, otherwise save
prjmenu.Append(self.ID_SAVE, "&Save\tCtrl+S", "", wx.ITEM_NORMAL)
prjmenu.Append(self.ID_SAVE_AS, "Save As") # ask for name, save
prjmenu.AppendSeparator()
prjmenu.Append(self.ID_EXIT, "&Quit\tCtrl+Q", "", wx.ITEM_NORMAL)
menubar.Append(prjmenu, "Project")
# Create tool panel
toolsmenu = wx.Menu()
for panel in panels:
name = panel.NAME
id = wx.NewIdRef()
self.panelIndex[id] = panel
toolsmenu.Append(id, name)
toolsmenu.Append(self.ID_EXPORT, "Export to AVR") # save as dialog
menubar.Append(toolsmenu, "Tools")
configmenu = wx.Menu()
# dialog box: arbotix/thru, speed, port
configmenu.Append(self.ID_PORT, "Port")
columnmenu = wx.Menu()
columnmenu.Append(self.ID_2COL, "2 columns")
columnmenu.Append(self.ID_3COL, "3 columns")
columnmenu.Append(self.ID_4COL, "4 columns")
configmenu.AppendSubMenu(columnmenu, "Pose editor")
# live update
self.live = configmenu.Append(self.ID_LIVE_UPDATE,
"Live pose update",
kind=wx.ITEM_CHECK)
# configmenu.Append(self.ID_TEST,"test") # for in-house testing of boards
menubar.Append(configmenu, "Configuration")
helpmenu = wx.Menu()
helpmenu.Append(self.ID_ABOUT, "About")
menubar.Append(helpmenu, "Help")
self.SetMenuBar(menubar)
# configure events
self.Bind(wx.EVT_MENU, self.newFile, self.ID_NEW)
self.Bind(wx.EVT_MENU, self.openFile, self.ID_OPEN)
self.Bind(wx.EVT_MENU, self.saveFile, self.ID_SAVE)
self.Bind(wx.EVT_MENU, self.saveFileAs, self.ID_SAVE_AS)
self.Bind(wx.EVT_MENU, sys.exit, self.ID_EXIT)
for t in self.panelIndex.keys():
self.Bind(wx.EVT_MENU, self.loadTool, t)
self.Bind(wx.EVT_MENU, self.export, self.ID_EXPORT)
self.Bind(wx.EVT_MENU, self.doRelax, self.ID_RELAX)
self.Bind(wx.EVT_MENU, self.doPort, self.ID_PORT)
self.Bind(wx.EVT_MENU, self.doTest, self.ID_TEST)
self.Bind(wx.EVT_MENU, self.doAbout, self.ID_ABOUT)
self.Bind(wx.EVT_CLOSE, self.doClose)
self.Bind(wx.EVT_TIMER, self.OnTimer, id=self.ID_TIMER)
self.Bind(wx.EVT_MENU, self.setLiveUpdate, self.ID_LIVE_UPDATE)
self.Bind(wx.EVT_MENU, self.do2Col, self.ID_2COL)
self.Bind(wx.EVT_MENU, self.do3Col, self.ID_3COL)
self.Bind(wx.EVT_MENU, self.do4Col, self.ID_4COL)
# editor area
self.sb = self.CreateStatusBar(2)
self.sb.SetStatusWidths([-1, 250])
self.sb.SetStatusText('not connected', 1)
self.loadTool()
self.sb.SetStatusText('please create or open a project...', 0)
self.Centre()
self.Show(True)
def loadTool(self, e=None):
"""Load toolpane."""
if self.panel is not None:
self.panel.save()
# self.sizer.Remove(self.panel)
self.panel.Destroy()
self.ClearBackground()
# instantiate
if e is None:
# PoseEditor
self.panel = panels[0](self, self.port)
else:
self.panel = self.panelIndex[e.GetId()](self, self.port)
self.sizer = wx.BoxSizer(wx.VERTICAL)
self.sizer.Add(self.panel, 1, wx.EXPAND | wx.ALL, 10)
self.SetSizer(self.sizer)
self.SetAutoLayout(1)
self.sizer.Fit(self)
self.sb.SetStatusText(self.panel.STATUS)
self.panel.SetFocus()
def newFile(self, e):
"""Open a dialog that asks for robot name and servo count."""
dlg = NewProjectDialog(self, -1, "Create New Project")
if dlg.ShowModal() == wx.ID_OK:
self.project.new(dlg.name.GetValue(), dlg.count.GetValue(),
int(dlg.resolution.GetValue()))
self.loadTool()
self.sb.SetStatusText('created new project ' + self.project.name +
', please create a pose...')
self.SetTitle(VERSION + " - " + self.project.name)
self.panel.saveReq = True
self.filename = ""
dlg.Destroy()
def openFile(self, e):
"""Loads a robot file into the GUI."""
dlg = wx.FileDialog(self, "Choose a file", self.dirname, "", "*.ppr",
wx.FD_OPEN)
if dlg.ShowModal() == wx.ID_OK:
self.filename = dlg.GetPath()
self.dirname = dlg.GetDirectory()
print("Opening: " + self.filename)
self.project.load(self.filename)
self.SetTitle(VERSION + " - " + self.project.name)
dlg.Destroy()
self.loadTool()
self.sb.SetStatusText('opened ' + self.filename)
def saveFile(self, e=None):
"""Save a robot file from the GUI."""
if self.filename == "":
dlg = wx.FileDialog(self, "Choose a file", self.dirname, "",
"*.ppr", wx.FD_SAVE)
if dlg.ShowModal() == wx.ID_OK:
self.filename = dlg.GetPath()
self.dirname = dlg.GetDirectory()
dlg.Destroy()
else:
return
if self.filename[-4:] != ".ppr":
self.filename = self.filename + ".ppr"
self.project.saveFile(self.filename)
self.sb.SetStatusText('saved ' + self.filename)
def saveFileAs(self, e):
self.filename = ""
self.saveFile()
def export(self, e):
"""Export a pose file for use with Sanguino Library."""
if self.project.name == "":
self.sb.SetBackgroundColour('RED')
self.sb.SetStatusText('please create a project')
self.timer.Start(20)
return
dlg = wx.FileDialog(self, "Choose a file", self.dirname, "", "*.h",
wx.FD_SAVE)
if dlg.ShowModal() == wx.ID_OK:
self.project.export(dlg.GetPath())
self.sb.SetStatusText("exported " + dlg.GetPath(), 0)
dlg.Destroy()
def findPorts(self):
"""Return a list of serial ports """
self.ports = []
for p in list_ports.comports():
try:
s = serial.Serial(p.device)
s.close()
self.ports.append(p.device)
except OSError:
pass
def doPort(self, e=None):
""" open a serial port """
if self.port is None:
self.findPorts()
dlg = wx.SingleChoiceDialog(self, 'Port (Ex. COM4 or /dev/ttyUSB0)',
'Select Communications Port', self.ports)
# dlg = PortDialog(self,'Select Communications Port',self.ports)
if dlg.ShowModal() == wx.ID_OK:
if self.port is not None:
self.port.ser.close()
print("Opening port: " + self.ports[dlg.GetSelection()])
self.openPort(self.ports[dlg.GetSelection()])
dlg.Destroy()
def openPort(self, port, baud=115200, interpolate=True):
try:
# TODO: add ability to select type of driver
self.port = Driver(port, baud, interpolate)
self.panel.port = self.port
self.panel.portUpdated()
self.sb.SetStatusText(port + '@' + str(baud), 1)
except BaseException:
self.port = None
self.sb.SetBackgroundColour('RED')
self.sb.SetStatusText('Could Not Open Port ' + port, 0)
self.sb.SetStatusText('not connected', 1)
self.timer.Start(20)
return self.port
def doTest(self, e=None):
if self.port is not None:
self.port.execute(253, 25, list())
def doRelax(self, e=None):
""" Relax servos so you can pose them. """
if self.port is not None:
print("PyPose: relaxing servos...")
for servo in range(self.project.count):
self.port.setReg(servo + 1, ax12.P_TORQUE_ENABLE, [
0,
])
else:
self.sb.SetBackgroundColour('RED')
self.sb.SetStatusText("No Port Open", 0)
self.timer.Start(20)
def doAbout(self, e=None):
license = """
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA)
"""
info = wx.adv.AboutDialogInfo()
info.SetName(VERSION)
info.SetDescription(
"A lightweight pose and capture software for the ArbotiX robocontroller"
)
info.SetCopyright("""
Copyright (c) 2020 Funtech-Makers. All right reserved.
Copyright (c) 2008-2010 Michael E. Ferguson. All right reserved.
""")
info.SetLicense(license)
info.SetWebSite("http://www.vanadiumlabs.com")
wx.adv.AboutBox(info)
def doClose(self, e=None):
# TODO: edit this to check if we NEED to save...
if self.project.save:
dlg = wx.MessageDialog(None, 'Save changes before closing?', '',
wx.YES_NO | wx.CANCEL | wx.ICON_QUESTION)
r = dlg.ShowModal()
if r == wx.ID_CANCEL:
e.Veto()
return
elif r == wx.ID_YES:
self.saveFile()
pass
self.Destroy()
def OnTimer(self, e=None):
self.timeout = self.timeout + 1
if self.timeout > 50:
self.sb.SetBackgroundColour(wx.NullColour)
self.sb.SetStatusText("", 0)
self.sb.Refresh()
self.timeout = 0
self.timer.Stop()
def do2Col(self, e=None):
self.columns = 2
if self.panelIndex == 0:
self.loadTool()
def do3Col(self, e=None):
self.columns = 3
if isinstance(self.panel, panels[0]):
self.loadTool()
def do4Col(self, e=None):
self.columns = 4
if isinstance(self.panel, panels[0]):
self.loadTool()
def setLiveUpdate(self, e=None):
if isinstance(self.panel, panels[0]):
self.panel.live = self.live.IsChecked()
from pypose.driver import Driver
from pypose.ax12 import P_MOVING, P_GOAL_SPEED_L
import arm
import gripper
import time
driver = Driver(port='/dev/tty.usbserial-AR0JW21B',baud=2000000)
# Import AX-12 register constants
# Get "moving" register for servo with ID 1
#is_moving = driver.getReg(1, P_MOVING, 1)
# Set the "moving speed" register for servo with ID 1
#speed = 50 # A number between 0 and 1023
#driver.setReg(1, P_GOAL_SPEED_L, [speed%256, speed>>8])
#driver.setReg(2,P_GOAL_SPEED_L, [speed%256, speed>>8])
for i in range(254,255):
time.sleep(0.2)
ret = driver.getReg(i, 36, 2)
if ret == -1:
print(str(i) + "wrong id")
else:
print(ret)
print(str(i) + " IS THE SERVO ID")
break
from pypose.driver import Driver from pypose.ax12 import P_MOVING, P_GOAL_SPEED_L import arm import gripper import time driver = Driver(port='/dev/tty.usbserial-AR0JW21B') # Import AX-12 register constants # Get "moving" register for servo with ID 1 #is_moving = driver.getReg(1, P_MOVING, 1) # Set the "moving speed" register for servo with ID 1 #speed = 50 # A number between 0 and 1023 #driver.setReg(1, P_GOAL_SPEED_L, [speed%256, speed>>8]) #driver.setReg(2,P_GOAL_SPEED_L, [speed%256, speed>>8]) 761 496 a = arm.Arm() time.sleep(0.2) print(a.current_position()) a.recenter() a.move((383, 467)) time.sleep(1) driver.setReg(2, 24, [0, 0]) driver.setReg(1, 24, [0, 0]) time.sleep(1) a.recenter()