PhidgetMotor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """
00003  * Copyright (c) 2009, CoroWare
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  """
00030 
00031 
00032 """Phidgets HC Motor Control ROS service for CoroBot
00033 
00034 A ROS service to request a specific acceleration and individual
00035 side drive wheels speed.
00036 
00037 """
00038 
00039 __author__ = 'Bill Mania <bmania@coroware.com>'
00040 __version__ = '1'
00041 
00042 import roslib; roslib.load_manifest('phidget_motor')
00043 from corobot_msgs.msg import *
00044 from std_msgs.msg import *
00045 import rospy
00046 from  threading import Timer
00047 from geometry_msgs.msg import Twist
00048 from ctypes import *
00049 from Phidgets.Devices.MotorControl import MotorControl
00050 from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
00051 from Phidgets.Events.Events import EncoderPositionUpdateEventArgs
00052 from Phidgets.Devices.Encoder import Encoder
00053 import numpy
00054 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00055 
00056 motors_inverted = False # parameter. True if the motors have been inverted, meaning that index 1 is left and 0 is right
00057 encoders_inverted = False # parameter. True if the encoders have been inverted, meaning that index 1 is left and 0 is right
00058 stop_when_obstacle = False # use the motors current and encoders to detect if the robot bumped a wall and stop the motors
00059 
00060 phidget1065 = False # True if we have a phidget1065, false if 1064
00061 motorControl = 0
00062 motorControlRight = 0 # we have two motor controllers in can we use 1065 devices
00063 encoders = 0 # encoder board, in case we have a phidget 1064 motor controller board
00064 leftEncoderPosition = 0; #in an encoder board, the index of the left motor encoder. Generally is 0 but in some robots this has been inverted by mistake
00065 rightEncoderPosition = 1; #in an encoder board, the index of the right motor encoder. Generally is 1 but in some robots this has been inverted by mistake
00066 leftWheels = 0 # index of the left wheel on the motor controller
00067 rightWheels = 1 # index of the right wheel on the motor controller
00068 minAcceleration = 0 # minimum acceleration that can be send to the phidget device
00069 maxAcceleration = 0 # maximum acceleration that can be send to the phidget device
00070 minSpeed = -100 # minimum speed that can be send to the phidget device
00071 maxSpeed = 100 # maximum speed that can be send to the phidget device
00072 timer = 0 # timer used to stop the motors after the number of seconds given in the message. This is interesting in case communication with a controlling computer is lost. 
00073 posdataPub = 0 # publish encoder data if we have phidget 1065 device
00074 leftEncoderPub = 0# publish left encoder data if we have phidget 1065 device
00075 rightEncoderPub = 0# publish right encoder data if we have phidget 1065 device
00076 leftPosition = 0 # value of the left encoder, if we have a 1065
00077 rightPosition = 0 # value of the right encoder, if we have a 1065
00078 diagnosticPub = 0 #publish diagnostic messages
00079 motorsError = 0 # help to find out which message we send on the diagnostics topic
00080 encodersError = 0 # help to find out which message we send on the diagnostics topic
00081 timer = 0 # timer for encoderCheck function. Used only if stop_when_obstacle is true.
00082 pastCurrentLeft = 0.0 # used in the stop_when_obstacle procedure
00083 pastCurrentRight = 0.0 # used in the stop_when_obstacle procedure
00084 leftSpeed = 0 # used in the stop_when_obstacle procedure
00085 rightSpeed = 0 # used in the stop_when_obstacle procedure
00086 
00087 # diagnostics messages
00088 motorControllerDisconnected = "Phidgets Motor controller disconnected - Please make sure it is connected, try to disconnect and reconnect it, or restart"
00089 motorSpeedError = "Cannot Set Motor Speed - Make sure the Phidget Motor controller board has not been disconnected and the speed is within the range"
00090 encoderBoardDisconnected = "Phidgets Encoder board disconnected - Please make sure it is connected, try to disconnect and reconnect it, or restart"
00091 encoderValueError = "Cannot get encoder value - Please make sure the encoder board and encoders are connected"
00092 
00093 
00094 # stop the motors
00095 def stop():
00096     try:
00097         motorControl.setVelocity(leftWheels,0);
00098 
00099         if phidget1065 == True:
00100             motorControlRight.setVelocity(rightWheels,0);
00101         else:
00102                 motorControl.setVelocity(rightWheels, 0);
00103     except PhidgetException as e:
00104         rospy.logerr("Failed in setVelocity() %i: %s", e.code, e.details)
00105         motorsError = 2
00106         return(False)
00107     motorsError = 0
00108     return(True)
00109 
00110 
00111 # move the motors as requested. Request is a corobot_msgs/MotorCommand.msg message 
00112 # It containing an acceleration, speed for both the left and right wheel and a time value
00113 def move(request):
00114     global timer
00115 
00116     if timer:
00117         timer.cancel();
00118 
00119     rospy.logdebug(
00120             "Left: %d:%d, Right: %d:%d, Acceleration: %d", 
00121             leftWheels,
00122             request.leftSpeed,
00123             rightWheels,
00124             request.rightSpeed,
00125             request.acceleration
00126             )
00127 
00128     # Make sure the acceleration and the speed is within the limits
00129     if request.acceleration > maxAcceleration:
00130         acceleration = float(maxAcceleration)
00131     elif request.acceleration < minAcceleration:
00132         acceleration = float(minAcceleration)
00133     else: 
00134         acceleration = float(request.acceleration)
00135 
00136     if request.leftSpeed < minSpeed:
00137         leftSpeed = float(minSpeed)
00138     elif request.leftSpeed > maxSpeed:
00139         leftSpeed = float(maxSpeed)
00140     else:
00141         leftSpeed = float(request.leftSpeed)
00142     if request.rightSpeed < minSpeed:
00143         rightSpeed = float(minSpeed)
00144     elif request.rightSpeed > maxSpeed:
00145         rightSpeed = float(maxSpeed)
00146     else:
00147         rightSpeed = float(request.rightSpeed)
00148 
00149     # on some motors the left and right motors have been inverted by mistake and the robot goes backward instead of forward. This is the solution for this problem
00150     if motors_inverted == True:
00151         temp = rightSpeed
00152         rightSpeed = -leftSpeed
00153         leftSpeed = -temp
00154 
00155     rospy.logdebug(
00156             "Left: %d:%d, Right: %d:%d, Acceleration: %d", 
00157             leftWheels,
00158             leftSpeed,
00159             rightWheels,
00160             rightSpeed,
00161             acceleration
00162             )
00163 
00164     # set the acceleration
00165     try:
00166         motorControl.setAcceleration(leftWheels, acceleration);
00167                 
00168         if phidget1065 == True:
00169             motorControlRight.setAcceleration(rightWheels, acceleration);
00170         else:
00171             motorControl.setAcceleration(rightWheels, acceleration);
00172     except PhidgetException as e:
00173         rospy.logerr("Failed in setAcceleration() %i: %s", e.code, e.details)
00174         motorsError = 2
00175         return(False)
00176 
00177     # set the velocity
00178     try:
00179         motorControl.setVelocity(leftWheels, leftSpeed);
00180 
00181         if phidget1065 == True:
00182             motorControlRight.setVelocity(rightWheels, rightSpeed);
00183         else:
00184             motorControl.setVelocity(rightWheels, rightSpeed);
00185     except PhidgetException as e:
00186         rospy.logerr("Failed in setVelocity() %i: %s", e.code, e.details)
00187         motorsError = 2
00188         return(False)
00189 
00190     # start the timer to stop the motors after the requested duration
00191     if request.secondsDuration != 0:
00192         timer = Timer(request.secondsDuration, stop)
00193         timer.start()
00194     motorsError = 0
00195     return(True)
00196 
00197 
00198 """move the left wheel as requested. This function doesn't use timer, and doesn't setup acceleration. 
00199    However it is used by the differential drive package to be able to command the robot with twist messages.
00200 """
00201 def left_move(request):
00202     leftSpeed = float(request.data)
00203 
00204     if leftSpeed < minSpeed:
00205         leftSpeed = float(minSpeed)
00206     elif leftSpeed > maxSpeed:
00207         leftSpeed = float(maxSpeed)
00208 
00209 
00210     # set the velocity
00211     try:
00212         motorControl.setAcceleration(leftWheels, 20);
00213         motorControl.setVelocity(leftWheels, leftSpeed);
00214 
00215     except PhidgetException as e:
00216         rospy.logerr("Failed in setVelocity() %i: %s", e.code, e.details)
00217         motorsError = 2
00218         return(False)
00219 
00220     motorsError = 0
00221     return(True)
00222         
00223 
00224 def right_move(request):
00225     """move the right wheel as requested. This function doesn't use timer, and doesn't setup acceleration. 
00226        However it is used by the differential drive package to be able to command the robot with twist messages.
00227     """
00228 
00229     rightSpeed = float(request.data)
00230 
00231     if rightSpeed < minSpeed:
00232         rightSpeed = float(minSpeed)
00233     elif rightSpeed > maxSpeed:
00234         rightSpeed = float(maxSpeed)
00235 
00236 
00237     # set the velocity
00238     try:
00239         if phidget1065 == True:
00240             motorControlRight.setAcceleration(rightWheels, 20);
00241             motorControlRight.setVelocity(rightWheels, rightSpeed);
00242         else:
00243             motorControl.setAcceleration(rightWheels, 20);
00244             motorControl.setVelocity(rightWheels, rightSpeed);
00245 
00246     except PhidgetException as e:
00247         rospy.logerr("Failed in setVelocity() %i: %s", e.code, e.details)
00248         motorsError = 2
00249         return(False)
00250 
00251     motorsError = 0
00252     return(True)
00253 
00254 
00255 def mcAttached(e):
00256     return
00257 
00258 def mcError(e):
00259     return
00260 
00261 
00262 def mcVelocityChanged(e):
00263     return
00264 
00265 
00266 # left encoder callback, used with phidget 1065 devices
00267 def leftEncoderUpdated(e):
00268     global leftPosition, rightPosition, posdataPub, leftEncoderPub
00269         
00270     leftPosition += e.positionChange
00271     try:
00272         if motorControlRight:
00273             # update the right encoder so that we have a correct value of both encoders at a given time.
00274             rightPosition = motorControlRight.getEncoderPosition(rightWheels) 
00275     except:
00276         encodersError = 2
00277         
00278     sendEncoderPosition()
00279 
00280     return
00281 
00282 
00283 # right encoder callback, used with phidget 1065 devices
00284 def rightEncoderUpdated(e):
00285     global leftPosition, rightPosition, posdataPub, rightEncoderPub
00286 
00287     rightPosition -= e.positionChange
00288     try:
00289         if motorControl:
00290             # update the left encoder so that we have a correct value of both encoders at a given time.
00291             leftPosition = motorControl.getEncoderPosition(leftWheels) 
00292     except:
00293         encodersError = 2
00294         
00295     sendEncoderPosition()
00296     return
00297 
00298 
00299 # encoder board callback, used only if a phidget 1064 is present
00300 def encoderBoardPositionChange(e):
00301     global leftEncoderPosition, rightEncoderPosition, leftPosition, rightPosition
00302 
00303     if e.index == leftEncoderPosition:
00304         leftPosition = leftPosition + e.positionChange
00305     elif e.index == rightEncoderPosition:
00306         rightPosition = rightPosition - e.positionChange
00307 
00308     sendEncoderPosition()
00309     return
00310 
00311 
00312 # send message on the position topic
00313 def sendEncoderPosition():
00314     global posdataPub, rightEncoderPub, leftEncoderPub, leftPosition, rightPosition
00315     msg = PosMsg()
00316     msg.px = leftPosition
00317     msg.py = rightPosition
00318     msg.header.stamp = rospy.Time.now()
00319     posdataPub.publish(msg)
00320 
00321     #publish the same data as an Int, for more conveniency
00322     msg = Int16()
00323     msg.data = numpy.int16(rightPosition)
00324     rightEncoderPub.publish(msg)
00325     msg.data = numpy.int16(leftPosition)
00326     leftEncoderPub.publish(msg)
00327 
00328     return
00329     
00330     
00331 """ Check the encoders position and see if the wheels have moved or not.
00332     If the encoders didn't move but the motors have a speed set, that means that the robot bumped
00333     in which case we run the backAndRotate() function. 
00334 """    
00335 def checkEncoders():
00336 
00337     global timer, phidget1065, pastCurrentLeft, pastCurrentRight, leftSpeed, rightSpeed, motorControl, motorControlRight, encoders, leftPosition, rightPosition
00338 
00339     timer.cancel()
00340 
00341     if (phidget1065 == True):
00342         newLeftPosition = motorControl.getEncoderPosition(leftWheels)
00343         newRightPosition = motorControlRight.getEncoderPosition(rightWheels)
00344     else:
00345         newLeftPosition = encoders.getPosition(leftWheels)
00346         newRightPosition = encoders.getPosition(rightWheels)        
00347 
00348 
00349     #Check if the encoder for left or right wheel didn't move a lot compared to the speed it should be at
00350     if (((newLeftPosition - leftPosition) < (leftSpeed*10) and (newLeftPosition - leftPosition) > -(leftSpeed*10) ) or ((newRightPosition - rightPosition) < (rightSpeed*10) and (newRightPosition - rightPosition) > -(rightSpeed*10))):
00351         # Bump detected, we stop the motors
00352         stop()
00353 
00354     # get the current going to each motors
00355     currentLeft = motorControl.getCurrent(leftWheels)
00356     if (phidget1065 == True):
00357         currentRight = motorControlRight.getCurrent(rightWheels)
00358     else:
00359         currentRight = motorControl.getCurrent(rightWheels)
00360 
00361     # We also check if there is a big increase (70% more) in the current in the motor, indicating an obstacle 
00362     if(pastCurrentRight != 0.0 and pastCurrentLeft != 0.0 and ( currentLeft >= pastCurrentLeft * 1.7 or currentRight >= pastCurrentRight * 1.7 ) and newLeftSpeed == leftSpeed and newRightSpeed == rightSpeed and rightSpeed != 0 and leftSpeed != 0):
00363         #bump detect, we stop the motors
00364         stop()
00365 
00366     pastCurrentLeft = currentLeft
00367     pastCurrentRight = currentRight
00368 
00369     leftPosition = newLeftPosition
00370     rightPosition = newRightPosition
00371 
00372     # get the velocity of the motors
00373     leftSpeed = motorControl.getVelocity(leftWheels)
00374     if (phidget1065 == True):
00375         rightSpeed = motorControlRight.getVelocity(rightWheels)
00376     else:
00377         rightSpeed = motorControl.getVelocity(rightWheels)
00378 
00379     # start the timer again
00380     timer = Timer(0.5, checkEncoders)
00381     timer.start()
00382     return      
00383 
00384 
00385 """ Open and Attach the phidget motor control board. Check if an 1064 or 1065 board is used and attach another 1065 board in case a 1065 is detected, or attach an encoder board in case a 1064 is detected
00386 """
00387 def initMotorAndEncoderBoards():
00388 
00389     global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted
00390 
00391     try:
00392         motorControl = MotorControl()
00393     except:
00394         rospy.logerr("Unable to connect to Phidget HC Motor Control")
00395         return
00396 
00397     try:
00398         motorControl.setOnAttachHandler(mcAttached)
00399         motorControl.setOnErrorhandler(mcError)
00400         motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
00401         motorControl.openPhidget()
00402 
00403         #attach the board
00404         motorControl.waitForAttach(10000)
00405 
00406         """use the function getMotorCount() to know how many motors the Phidget board can take
00407 
00408         if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is 
00409         separated from the phidget 1064.
00410         if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled 
00411         in this file as it is part of the 1065 board. 
00412 
00413         """
00414         if motorControl.getMotorCount() == 1:
00415             phidget1065 = True 
00416             rightWheels = 0
00417             motorControlRight = MotorControl()
00418             motorControlRight.setOnAttachHandler(mcAttached)
00419             motorControlRight.setOnErrorhandler(mcError)
00420             motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
00421                         
00422             if motorControl.getSerialNum() > motorControlRight.getSerialNum(): 
00423                 """ As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
00424                 """
00425                 motorControlTemp = motorControl
00426                 motorControl = motorControlRight
00427                 motorControlRight = motorControlTemp
00428 
00429             #Set up the encoders handler
00430             motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
00431             motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
00432 
00433             #attach the board
00434             motorControlRight.openPhidget()
00435             motorControlRight.waitForAttach(10000)
00436 
00437         # we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board.
00438         else: 
00439             encoders = Encoder()
00440             encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
00441             encoders.openPhidget()
00442             encoders.waitForAttach(10000)
00443             # some robots have the left and right encoders switched by mistake
00444             if(motors_inverted or encoders_inverted): 
00445                 leftEncoderPosition = 1;
00446                 rightEncoderPosition = 0;
00447             encoders.setEnabled(leftEncoderPosition, True)
00448             encoders.setEnabled(rightEncoderPosition, True)
00449            
00450 
00451     except PhidgetException as e:
00452         motorsError = 1
00453         encodersError = 1
00454         rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
00455         return
00456     except:
00457         motorsError = 1
00458         encodersError = 1
00459         rospy.logerr("Unable to register the motors and encoders board")
00460         return
00461 
00462     if motorControl.isAttached():
00463         rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
00464     if phidget1065 == True:
00465         if motorControlRight.isAttached():
00466             rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
00467     else:
00468         rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())
00469 
00470 
00471     if stop_when_obstacle:
00472         timer = Timer(1.0, checkEncoders)
00473         timer.start()
00474     return
00475 
00476 
00477 #Called every second and send diagnostic messages to tell the user if everything is ok or if something is wrong
00478 def diagnosticsCallback (event):
00479 
00480     array = DiagnosticArray()
00481 
00482     # Set the timestamp for diagnostics
00483     array.header.stamp = rospy.Time.now()
00484     
00485     motors_message = DiagnosticStatus(name = 'PhidgetMotors', level = 0,message = 'initialized', hardware_id='Phidget')
00486     
00487     if (motorsError == 1):
00488         motors_message.level = 2
00489         motors_message.message = "Phidget Motor controller can't be initialized"
00490         motors_message.values = [ KeyValue(key = 'Recommendation', value = motorControllerDisconnected)]
00491     if (motorsError == 2):
00492         motors_message.level = 2
00493         motors_message.message = "Can't set up motor speed"
00494         motors_message.values = [ KeyValue(key = 'Recommendation', value = motorSpeedError)]
00495         
00496     encoders_message = DiagnosticStatus(name = 'PhidgetEncoders', level = 0,message = 'initialized', hardware_id='Phidget')
00497     
00498     if (encodersError == 1):
00499         encoders_message.level = 2
00500         encoders_message.message = "Phidget Encoder board can't be initialized"
00501         encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderBoardDisconnected)]
00502     if (encodersError == 2):
00503         encoders_message.level = 2
00504         encoders_message.message = "Can't get encoder value"
00505         encoders_message.values = [ KeyValue(key = 'Recommendation', value = encoderValueError)]
00506     
00507     array.status = [ motors_message, encoders_message ]
00508   
00509     diagnosticPub.publish(array)
00510 
00511 
00512 """Initialize the PhidgetMotor service
00513 
00514    Establish a connection with the Phidget HC Motor Control and
00515    then with the ROS Master as the service PhidgetMotor
00516 
00517 """
00518 def setupMoveService():
00519 
00520     rospy.init_node(
00521             'PhidgetMotor',
00522             log_level = rospy.DEBUG
00523             )
00524 
00525     global motorControl, encoders, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, leftWheels, posdataPub, leftEncoderPub, rightEncoderPub, stop_when_obstacle, diagnosticPub
00526     timer = 0
00527     
00528     motors_inverted = rospy.get_param('~motors_inverted', False)
00529     encoders_inverted = rospy.get_param('~encoders_inverted', False)
00530     stop_when_obstacle = rospy.get_param('~stop_when_obstacle', False)
00531 
00532     # initialize the phidgets boards
00533     initMotorAndEncoderBoards() 
00534 
00535     if motorControl != 0 and motorControl.isAttached():
00536         minAcceleration = motorControl.getAccelerationMin(leftWheels)
00537         maxAcceleration = motorControl.getAccelerationMax(leftWheels)
00538 
00539 
00540         phidgetMotorTopic = rospy.Subscriber("PhidgetMotor", MotorCommand ,move)
00541 
00542         # topic used for differential_drive package to enable twist commands
00543         leftWheelTopic = rospy.Subscriber("lmotor_cmd", Float32 ,left_move) 
00544 
00545         # topic used for differential_drive package to enable twist commands
00546         rightWheelTopic = rospy.Subscriber("rmotor_cmd", Float32 ,right_move)
00547 
00548         posdataPub = rospy.Publisher("position_data", PosMsg)
00549         leftEncoderPub = rospy.Publisher("lwheel", Int16)
00550         rightEncoderPub = rospy.Publisher("rwheel", Int16)
00551         diagnosticPub = rospy.Publisher('/diagnostics', DiagnosticArray)
00552         
00553         rospy.Timer(rospy.Duration(1.0), diagnosticsCallback, oneshot=False)
00554 
00555         rospy.spin()
00556 
00557     return
00558 
00559 
00560 
00561 if __name__ == "__main__":
00562     setupMoveService()
00563 
00564     try:
00565         motorControl.setVelocity(leftWheels, 0)
00566         if phidget1065 != True:
00567             motorControl.setVelocity(rightWheels, 0)
00568         
00569         motorControl.closePhidget()
00570         if phidget1065 == True:
00571             motorControlRight.setVelocity(rightWheels, 0)
00572             motorControlRight.closePhidget()
00573         else:
00574             encoders.closePhidget()
00575     except:
00576         pass


phidget_motor
Author(s): Bill Mania/bmania@coroware.com, Morgan Cormier/mcormier@coroware.com
autogenerated on Sun Oct 5 2014 23:18:11