Touchbot gesture library utilities addition
The old way of writing programs to the robot's eeprom was very
cumbersome. It required a lot of duplicated code and was almost
impossible to read as each instruction involved ~5 lines of python code,
so even simple gestures were unweildy.
Here I add a some functions to the roibot module to wraps virtually all of
the functionality required for writing programs to the eeprom in a
couple easy functions. They auto-increment line numbers and point table
entries, and retry every instruction behind the scenes. Each
instruction also raises exceptions now if they fail, so the whole script
can be wrapped in a single try: statement to stop on any failure.
To additionally offload more of the overhead a run_program.py gesture
was added. This has a run_program() function that essentially loads a
new program that you pass it into eeprom and then runs it for you
without each gesture script having to do all the setup individually.
You can also run the run_program gesture all on its own to simply
execute whatever program is currently in memory. This saves eeprom
write cycles which are limited, and is convenient when doing many
repeated runs of the same script.
basic.py has been rewritten to use the new methodology of programming
and serve as an example for future gestures. The other gestures will be
converted at a later date
BUG=chromium-os:34930
TEST=From the gestures directory, run "make" to build the wrapper
scripts, then use "./basic" to make sure that it can program a new set
of instructions to the eeprom and run them. Once it has completed, run
"./run_program" to re-execute what it currently in memory without
re-writing it.
Change-Id: If5549d82ed52da2fc71cba6f89eaa8a822fedc2d
Signed-off-by: Charlie Mooney <charliemooney@chromium.org>
Reviewed-on: https://gerrit.chromium.org/gerrit/34386
Reviewed-by: Mark Koudritsky <kamrik@chromium.org>
diff --git a/gestures/basic.py b/gestures/basic.py
index afae1df..63e4d67 100644
--- a/gestures/basic.py
+++ b/gestures/basic.py
@@ -3,128 +3,60 @@
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.
#
-import sys
-import time
+# Traverse the boundary of a touchpad. Useful for determining if the values
+# are good/safe and tweaking them or simply as a basic example to start with.
+#
import roibot
-
-# Open the robot interface.
-# robot = roibot.ROIbot(sys.argv[1])
-robot = roibot.ROIbot(port='/dev/ttyUSB0',
- baudrate=9600,
- bytesize=8,
- parity='E',
- stopbits=1,
- rtscts=0)
-
-# If the robot doesn't respond in 2 seconds to a command, then something
-# is wrong.
-robot.timeout = 2
-
-#
-# Perform an operation on the robot
-#
-try:
- # Clear outstanding error conditions (if any).
- #
- # This will clean up any bad outstanding state that exists from:
- #
- # o line noise from a previous connection
- # o resulting from the robot responding to flow control events
- # occuring during initialization of the serial port:
- # o DTR off->on
- # o CTS/RTS
- # o etc.
- #
- # The operation is performed twice in case the output buffer is not
- # clear; technically, we should be able to just send a CR/Lf then the
- # command (TODO: Test this theory)
- #
- print "COMMAND: Clear error status"
- if robot.execCancelError():
- print "clean state"
- else:
- print "failed to clear state"
- if robot.execCancelError():
- print "clean state"
- else:
- sys.exit("failed to clear state")
-
- print "COMMAND: Current status..."
- for statusElement in roibot.error.statusReport(robot):
- print " ", statusElement
-
- # print roibot.error.statusReport(robot)
- # line = robot.sendCommand("SYSP")
- # print line
-
- print "COMMAND: Reset"
- if robot.execReset():
- print "Robot reset."
- else:
- print "Reset failed, retrying..."
- if robot.execReset():
- print "Robot reset."
- else:
- print "Fatal: Reset failed."
- exit(1)
-
- print "COMMAND: Enable host mode"
- if robot.modeHostON():
- print "Host mode enabled"
- else:
- print "Failed to set host mode"
-
- print "COMMAND: Servo ON"
- if not robot.execServoON():
- print "Servo failed"
- for statusElement in roibot.error.statusReport(robot):
- print " ", statusElement
+import run_program
- #
- # A simple motion program...
- #
- print "COMMAND: Return to origin"
- if not robot.execReturnToOrigin():
- sys.exit("origin reset failed")
+def program(robot):
+ """Upload a new program to the robot. This program moves the finger along
+ the border of the touchpad
+ """
+ bounds = roibot.BoundingVolume("lumpy")
+ SPEED_SLOW = 30
+ SPEED_TABLE_SLOW = 1
+ SPEED_FAST = 60
+ SPEED_TABLE_FAST = 2
+ STEPS = 10
+ TOUCH_Z = bounds.paperZ()
+ UP_Z = TOUCH_Z - 15
- # Wait for completion
- roibot.motion.waitForExecution(robot)
+ print "Begin Programming"
+ # Set up the speed tables
+ robot.setParam("T2 V%02d=%06.01f" % (SPEED_TABLE_SLOW, SPEED_SLOW))
+ robot.setParam("T2 V%02d=%06.01f" % (SPEED_TABLE_FAST, SPEED_FAST))
- # Show inaccuracy in settle at origin... :(
- print "This is not precise unless we wait for it to settle..."
- print robot.sendCommand("STAS", "SNO=0")
- print robot.monRequestPresentPosition()
+ #get into position
+ robot.addCmd("SPD V=%02d" % SPEED_TABLE_FAST)
- print robot.sendCommand("STAS", "SNO=0")
- print robot.monRequestPresentPosition()
+ pt = robot.addPoint(bounds.minX(), bounds.minY(), bounds.upZ())
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt)
- print robot.sendCommand("STAS", "SNO=0")
- print robot.monRequestPresentPosition()
+ pt1 = robot.addPoint(bounds.minX(), bounds.minY(), bounds.paperZ())
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt1)
- # move at high speed to X=+0200.00
- roibot.motion.moveOneAxis(robot, "+0200.00", "SP", "SP", "SP")
- # back up to X=+0150.00
- roibot.motion.moveOneAxis(robot, "+0150.00", "SP", "SP", "SP")
- # continue at low speed to X=+243.46
- # TODO: There is no in-band parameter support for selection of low
- # speed at this time, so this operation actually occurs at
- # normal (high) speed.
- roibot.motion.moveOneAxis(robot, "+0243.46", "SP", "SP", "SP")
- # continue at high speed to Y=+0050.00
- roibot.motion.moveOneAxis(robot, "SP", "+0050.00", "SP", "SP")
+ # go around the border
+ robot.addCmd("SPD V=%02d" % SPEED_TABLE_SLOW)
- # announce our arrival at our destination
- print robot.monRequestPresentPosition()
+ pt = robot.addPoint(bounds.maxX(), bounds.minY(), bounds.paperZ())
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt)
-except roibot.roibot.ROIbotTimeoutException:
- # timeouts - usually communications or stuck servo related
- print "timed out!"
-except:
- # all other errors
- for statusElement in roibot.error.statusReport(robot):
- print " ", statusElement
+ pt = robot.addPoint(bounds.maxX(), bounds.maxY(), bounds.paperZ())
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt)
-robot.timeout = None
-robot.close()
+ pt = robot.addPoint(bounds.minX(), bounds.maxY(), bounds.paperZ())
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt)
+
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt1)
+
+ # Lift up to get the laptop out
+ pt = robot.addPoint(bounds.minX(), bounds.minY(), bounds.upZ() - 50)
+ robot.addCmd("MOVP a PT=%03d CN=00 S V=00 POST" % pt)
+
+ # End the program nicely
+ robot.addCmd("END")
+
+run_program.run_program(program)
diff --git a/gestures/run_program.py b/gestures/run_program.py
new file mode 100644
index 0000000..7f1ad71
--- /dev/null
+++ b/gestures/run_program.py
@@ -0,0 +1,62 @@
+#
+# Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
+# Use of this source code is governed by a BSD-style license that can be
+# found in the LICENSE file.
+#
+# EEPROM programming wrapper. The run_prog() function can be used externally
+# to load a program into eeprom or this script can be run on its own to run
+# whatever program is currently stored there. This allows us to cut down on
+# the number of read/write cycles to the eeprom as well as makes the code for
+# gestures much nicer.
+#
+import roibot
+
+
+def run_program(program=None):
+ # Open the robot interface.
+ robot = roibot.ROIbot(port='/dev/ttyUSB0',
+ baudrate=9600,
+ bytesize=8,
+ parity='E',
+ stopbits=1,
+ rtscts=0)
+ robot.timeout = 2
+
+ try:
+ # Clear any pending errors and reset the robot
+ robot.tryOrFail(robot.execCancelError)
+ robot.tryOrFail(robot.execReset)
+
+ # Switch to sequential programming mode (load a program to eeprom)
+ robot.tryOrFail(robot.modeHostON)
+ robot.tryOrFail(robot.execServoON)
+ robot.tryOrFail(robot.modeSequential)
+ robot.tryOrFail(robot.modeProgram)
+
+ # If we have a new program to load onto the robot
+ if program != None:
+ program(robot)
+
+ # Switch the program mode to auto (run the program in eeprom)
+ robot.tryOrFail(robot.modeAutomatic)
+
+ # Go back to the origin so it starts from a known location
+ robot.tryOrFail(robot.execReturnToOrigin)
+ roibot.motion.waitForExecution(robot)
+
+ # Start eeprom program 1
+ robot.tryOrFail(robot.execStartSequential, 1)
+ print "Execution started."
+
+ except Exception as e:
+ print "Exception Caught"
+ print "Error(" + str(e) + ")"
+ for statusElement in roibot.error.statusReport(robot):
+ print " ", statusElement
+ raise
+ finally:
+ robot.timeout = None
+ robot.close()
+
+if __name__=="__main__":
+ run_program()
diff --git a/roibot/__init__.py b/roibot/__init__.py
index 95f5686..074cd86 100644
--- a/roibot/__init__.py
+++ b/roibot/__init__.py
@@ -18,3 +18,28 @@
import error
import motion
import program
+import pickle
+
+
+class BoundingVolume:
+ """This class represents the area in which a touchpad is. It loads the data
+ from a manually generated pickled file, and presents it to the script in
+ a simpler format instead of having to remember all the matrix indices.
+ """
+ def __init__(self, device, display=False):
+ path = "../devices/" + device + ".pp"
+ self.deviceRange = pickle.load(open(path, "r"))
+ def minX(self):
+ return self.deviceRange[0][0]
+ def maxX(self):
+ return self.deviceRange[0][1]
+ def minY(self):
+ return self.deviceRange[1][0]
+ def maxY(self):
+ return self.deviceRange[1][1]
+ def upZ(self):
+ return self.paperZ() - 15
+ def paperZ(self):
+ return self.deviceRange[2][0]
+ def clickZ(self):
+ return self.deviceRange[2][1]
diff --git a/roibot/roibot.py b/roibot/roibot.py
index b20f882..81b0c4b 100644
--- a/roibot/roibot.py
+++ b/roibot/roibot.py
@@ -59,6 +59,8 @@
_AdhocTestON = True
_writeEEPROM = False
_hostModeON = False
+ _line_number = 1
+ _point_number = 1
#
# Utility functions
@@ -656,4 +658,46 @@
"""Request error history."""
return self.sendCommand("EHTR", "NO=01")
+ # High level EEPROM programming instructions
+ #
+ # The follow functions wrap the commands for writing a program to the
+ # EEPROM of the robot. They all will make multiple attempts to issue
+ # their command, and raise an exception if they are unsuccessful.
+ def addCmd(self, cmd):
+ """Upload a new command into the next spot in the eeprom. This adds a
+ new instruction to the program in the robots memory.
+ """
+ print "ADDING: %04d %s" % (self._line_number, cmd)
+ self.tryOrFail(self.textWriteSequential,
+ "%04d" % self._line_number, cmd)
+ self._line_number += 1
+
+ def setParam(self, cmd):
+ """Set a parameter's value. This can be any "parameter" in the robot,
+ such as altering speed profiles or point tables, etc.
+ """
+ print "SETTING: %s" % cmd
+ self.tryOrFail(self.parameterWrite, cmd)
+
+ def addPoint(self, x, y, z):
+ """Add a new point to the point table (returns the point number)."""
+ self.setParam("T1 PT=%03d " % self._point_number +
+ "X=+%07.02f " % x +
+ "Y=+%07.02f " % y +
+ "Z=+%07.02f " % z +
+ "R=+0000.00")
+ self._point_number += 1
+ return self._point_number - 1
+
+ def tryOrFail(self, fn, *args, **kwargs):
+ """Make multiple attempts to call a function fn(). The function should
+ return True or False. If after NUM_RETRIES attempts, there still
+ hasn't been a success, raise an exception.
+ """
+ NUM_RETRIES = 2
+ for i in range(NUM_RETRIES):
+ result = fn(*args, **kwargs)
+ if result:
+ return result
+ raise unrecoverableError
# End
