robot_brake.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # $Id: 
00003 """
00004 brake script for Marvin -- Function to send the controller a command,
00005 then read its response  with a 1-second timeout.
00006 
00007 This program can be run as a shell script or imported as a module.
00008 Help is provided in each case:
00009 
00010   as a script:
00011     --help describes the arguments and options
00012 
00013   as a module:
00014     import roslib
00015     roslib.load_manifest('art_servo')
00016     import brake.robot_brake
00017     help(brake.robot_brake.commands)
00018 
00019 """
00020 
00021 import serial
00022 import time
00023 import types
00024 import sys
00025 import getopt
00026 import os
00027 from os.path import basename, splitext
00028 
00029 # globals
00030 TTY="/dev/brake"
00031 
00032 class commands:
00033     """ Class to connect and send commands to brake.
00034     """
00035 
00036     def __init__(self, device=TTY):
00037         """ Constructor method.
00038             Takes serial port as input.
00039         """
00040         self.ser=serial.Serial(device, 38400, timeout=1)
00041         self.ser.open()
00042 
00043     def baud(self):   
00044         """ Set baud rate
00045         """
00046         print("setting brake baud rate")
00047         self.ser.setBaudrate(9600)
00048         self.ser.write("BAUD38400\r")
00049         time.sleep(3)
00050         self.ser.close()
00051         self.ser.setBaudrate(38400)
00052         self.ser.open()
00053         self.st()
00054 
00055     def servo_cmd(self,cmd):
00056         """ Send command and wait for response
00057         """
00058 
00059         print(cmd)
00060         cmd+=" RW\r"
00061     
00062         self.ser.write(cmd)
00063         status=self.ser.readline()
00064         if status == '':
00065             print("Device not responding")
00066         else:
00067             lstatus=status.split('\r')
00068             for x in lstatus:
00069                 if x:
00070                     print("Device returns (0x%x)\n" % (int(x)))
00071         return status
00072 
00073     def setup(self):
00074         """ Run brake setup routine
00075         """
00076 
00077         print("initializing brake on " + TTY)
00078         self.servo_cmd("ZS")            # reset all status bits
00079         self.servo_cmd("UCI")        # disable right (pos, on) limit
00080         self.servo_cmd("UDI")        # disable left (neg, off) limit
00081         self.servo_cmd("F=0")        # disable servo on stop
00082     
00083         self.servo_cmd("MP D=0 G")      # ensure position mode
00084         self.servo_cmd("UAI")        # set Port A as an input
00085         self.servo_cmd("O=0")        # set temporary origin
00086         self.servo_cmd("A=8*96")        # set acceleration
00087         self.servo_cmd("V=32212*48")    # set velocity
00088         
00089         self.servo_cmd("MT T=-125")     # push firmly in neg direction
00090         time.sleep(4)
00091         self.servo_cmd("RP")            # report encoder position
00092         self.servo_cmd("MT T=5")        # push gently in pos direction
00093         time.sleep(1)
00094         self.servo_cmd("MP D=0 G")      # back to position mode
00095         self.servo_cmd("RP")            # report encoder position
00096         self.servo_cmd("O=0")        # set the origin here
00097         
00098         self.servo_cmd("LIMD")  # enable directional limits
00099         self.servo_cmd("LIML")  # set limits active low (default)
00100     
00101         # UCP: Restore pin C to right (plus, brake-off) limit function
00102         self.servo_cmd("UCP")
00103 
00104         # UDM: Restore pin D to left (minus, brake-full) limit function
00105         self.servo_cmd("UDM")
00106 
00107         # F=1: stop and then servo in place after limit fault
00108         self.servo_cmd("F=1")
00109 
00110     def off(self):
00111         """ Release brake
00112         """
00113 
00114         print("disengaging brake")
00115         self.servo_cmd("Zr")
00116         self.servo_cmd("Zl")
00117         self.servo_cmd("MP P=0 G")
00118 
00119 
00120     def full(self):
00121         """ Engage brake fully
00122         """
00123         print("setting brake fully on")
00124         self.servo_cmd("MT T=200")
00125         self.servo_cmd("Zr")
00126         self.servo_cmd("Zl")
00127         time.sleep(1)
00128         self.servo_cmd("MP D=0 G")
00129         self.servo_cmd("RP")
00130         self.servo_cmd("c=UEA Rc")
00131 
00132     def pos(self):
00133         """ Return brake position info
00134         """
00135         print("motor position:")
00136         self.servo_cmd("RP")
00137         print("potentiometer value:")
00138         self.servo_cmd("c=UEA Rc")
00139         print("brake pressure:")
00140         self.servo_cmd("c=UAA Rc")
00141         print("position error limit:")
00142         self.servo_cmd("c=E Rc")
00143 
00144 
00145     def st(self):
00146         """ Print status of motor
00147         """
00148         print("current status")
00149         self.servo_cmd("")
00150 
00151 def usage(progname):
00152     """ print usage message
00153     """
00154     print("\n" + str(progname) + """[-h] command [ <serial-port> ]
00155 
00156  -h, --help\tprint this message
00157 
00158  command options:
00159     \tbaud\tset baud rate to 38400
00160     \tfull\tfully engage brake
00161     \toff\tfully release brake
00162     \tpos\treport current motor position and potentiometer reading
00163     \tsetup\tinitialize brake servo
00164     \tst\tprint current status
00165 
00166  default serial-port: /dev/brake
00167 """)
00168 
00169 # main program
00170 def main(argv=None):
00171     """ Main program. For either script or interactive use.
00172     """
00173     global TTY
00174 
00175     if argv is None:
00176         argv = sys.argv
00177 
00178     # extract base name of command ('', when imported)
00179     progname = basename(argv[0])
00180     if progname is "":
00181         progname = "robot-brake.py"
00182 
00183     # process parameters
00184     opts = {}
00185     try:
00186         o, params = getopt.gnu_getopt(argv[1:], 'h', ('help'))
00187 
00188     except getopt.error, msg:
00189         print(msg)
00190         print("for help use --help")
00191         return 8
00192 
00193     for k,v in o:
00194         opts[k] = v
00195     if '-h' in opts or '--help' in opts:
00196         usage(progname)
00197         return 9
00198     if len(params) < 1:
00199         usage(progname)
00200         print("no command provided")
00201         return 8
00202 
00203     if len(params) == 2:
00204         TTY = params[1]
00205 
00206     try:
00207         c = commands(TTY)
00208 
00209         CMD=params[0]
00210         if CMD=="baud":
00211             c.baud()
00212         elif CMD=="pos":
00213             c.pos()
00214         elif CMD=="full":
00215             c.full()
00216         elif CMD=="off":
00217             c.off()
00218         elif CMD=="setup":
00219             c.setup()
00220         elif CMD=="st":
00221             c.st()
00222         else:
00223             usage(progname)
00224             print("invalid command provided")
00225             return 7
00226 
00227     except serial.serialutil.SerialException, ioerror:
00228         print(ioerror)
00229         return 1
00230 
00231 
00232 if __name__ == "__main__":
00233     # run main function and exit
00234     sys.exit(main())


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12