blob: bb979dfecf5ccab40209e0c9f0188a3d884e66bd [file] [log] [blame]
#
# 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.
#
# Reset the robot and return the finger to the origin
#
import sys
import time
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.
#
print "COMMAND: Clear error status"
robot.assertCancelError()
print "COMMAND: Current status..."
for statusElement in roibot.error.statusReport(robot):
print " ", statusElement
print "COMMAND: Reset"
robot.assertReset()
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
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 "EXPECTED"
print "['Return to origin complete', 'Positioning complete']"
print "['Sequential mode', 'Automatic mode', 'Host computer ON']"
print "['Servo ON', 'Reserved SNO=3 bit 0x02', 'Slave not ready']"
print "[]"
#
# A simple motion program...
#
print "COMMAND: Return to origin"
if not robot.execReturnToOrigin():
print "origin reset failed"
# Wait for completion
roibot.motion.waitForExecution(robot)
print "Return to origin complete."
print "COMMAND: Current status..."
for statusElement in roibot.error.statusReport(robot):
print " ", statusElement
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
robot.timeout = None
robot.close()