def setupMoveService():
    """Initialize the PhidgetMotor service

    Establish a connection with the Phidget HC Motor Control and
    then with the ROS Master as the service PhidgetMotor

    """
    node_name = 'PhidgetMotor'

    rospy.init_node(node_name, log_level=rospy.DEBUG)

    serial_no = rospy.get_param("~serial_no", 0)
    if not serial_no == 0:
        rospy.loginfo("Using motor controller with serial number %d",
                      serial_no)
    else:
        rospy.loginfo(
            "No serial number specified.  This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers"
        )

    global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
    timer = 0
    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
        return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        if serial_no != 0:
            motorControl.openPhidget(serial_no)
        else:
            motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)
        """use the function getMotorCount() to know how many motors the Phidget board can take

        if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is 
        separated of the phidget 1064.
        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 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True
            rightWheels = 0
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnDetachHandler(mcDetached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
            motorControlRight.setOnInputChangeHandler(mcInputChanged)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)

            if motorControl.getSerialNum() > motorControlRight.getSerialNum():
                """ 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
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

#Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Unable to register the handlers: %i: %s", e.code,
                     e.details)
        return
    except AttributeError as e:
        rospy.logerr("Unable to register the handlers: %s", e)
        return
    except:
        rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0])
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                      motorControl.getDeviceName(),
                      motorControl.getSerialNum(),
                      motorControl.getDeviceVersion())
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                          motorControlRight.getDeviceName(),
                          motorControlRight.getSerialNum(),
                          motorControlRight.getDeviceVersion())

    minAcceleration = motorControl.getAccelerationMin(leftWheels)
    maxAcceleration = motorControl.getAccelerationMax(leftWheels)

    motors_inverted = rospy.get_param('~motors_inverted', False)

    phidgetMotorTopic = rospy.Subscriber(node_name, MotorCommand, move)
    phidgetMotorService = rospy.Service(node_name, Move, move)
    if phidget1065 == True:
        posdataPub = rospy.Publisher("position_data", PosMsg)
    rospy.spin()
Пример #2
0
def initMotorAndEncoderBoards():

    global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted

    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
     	return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)

        """use the function getMotorCount() to know how many motors the Phidget board can take

        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 
        separated from the phidget 1064.
        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 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True 
            rightWheels = 0
            motorControlRight = MotorControl()
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
			
            if motorControl.getSerialNum() > motorControlRight.getSerialNum(): 
                """ 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
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

            #Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.openPhidget()
            motorControlRight.waitForAttach(10000)

        # 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.
        else: 
            encoders = Encoder()
            encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
            encoders.openPhidget()
            encoders.waitForAttach(10000)
            # some robots have the left and right encoders switched by mistake
            if(motors_inverted or encoders_inverted): 
                leftEncoderPosition = 1;
                rightEncoderPosition = 0;
            encoders.setEnabled(leftEncoderPosition, True)
            encoders.setEnabled(rightEncoderPosition, True)
           

    except PhidgetException as e:
        motorsError = 1
        encodersError = 1
        rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
        return
    except:
        motorsError = 1
        encodersError = 1
        rospy.logerr("Unable to register the motors and encoders board")
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
    else:
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())


    if stop_when_obstacle:
        timer = Timer(1.0, checkEncoders)
        timer.start()
    return
Пример #3
0
def initMotorAndEncoderBoards():

    global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted

    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
        return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)

        """use the function getMotorCount() to know how many motors the Phidget board can take

        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 
        separated from the phidget 1064.
        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 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True 
            rightWheels = 0
            motorControlRight = MotorControl()
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
                        
            if motorControl.getSerialNum() > motorControlRight.getSerialNum(): 
                """ 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
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

            #Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.openPhidget()
            motorControlRight.waitForAttach(10000)

        # 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.
        else: 
            encoders = Encoder()
            encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
            encoders.openPhidget()
            encoders.waitForAttach(10000)
            # some robots have the left and right encoders switched by mistake
            if(motors_inverted or encoders_inverted): 
                leftEncoderPosition = 1;
                rightEncoderPosition = 0;
            encoders.setEnabled(leftEncoderPosition, True)
            encoders.setEnabled(rightEncoderPosition, True)
           

    except PhidgetException as e:
        motorsError = 1
        encodersError = 1
        rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
        return
    except:
        motorsError = 1
        encodersError = 1
        rospy.logerr("Unable to register the motors and encoders board")
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
    else:
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())


    if stop_when_obstacle:
        timer = Timer(1.0, checkEncoders)
        timer.start()
    return
Пример #4
0
def setupMoveService():
    """Initialize the PhidgetMotor service

    Establish a connection with the Phidget HC Motor Control and
    then with the ROS Master as the service PhidgetMotor

    """

    rospy.init_node(
            'PhidgetMotor',
            log_level = rospy.DEBUG
            )

    global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
    timer = 0
    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
     	return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)
        """use the function getMotorCount() to know how many motors the Phidget board can take

        if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is 
        separated of the phidget 1064.
        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 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True 
            rightWheels = 0
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnDetachHandler(mcDetached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
            motorControlRight.setOnInputChangeHandler(mcInputChanged)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
			
            if motorControl.getSerialNum() > motorControlRight.getSerialNum(): 
                """ 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
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

			#Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Unable to register the handlers: %i: %s", e.code, e.details)
        return
    except:
        rospy.logerr("Unable to register the handlers")
        return
     

    if motorControl.isAttached():
        rospy.loginfo(
                "Device: %s, Serial: %d, Version: %d",
                motorControl.getDeviceName(),
                motorControl.getSerialNum(),
                motorControl.getDeviceVersion()
                )
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo(
		            "Device: %s, Serial: %d, Version: %d",
		            motorControlRight.getDeviceName(),
		            motorControlRight.getSerialNum(),
		            motorControlRight.getDeviceVersion()
		            )

    minAcceleration = motorControl.getAccelerationMin(leftWheels)
    maxAcceleration = motorControl.getAccelerationMax(leftWheels)

    motors_inverted = rospy.get_param('~motors_inverted', False)

    phidgetMotorTopic = rospy.Subscriber("PhidgetMotor", MotorCommand ,move)
    phidgetMotorService = rospy.Service('PhidgetMotor',Move, move)
    if phidget1065 == True:
        posdataPub = rospy.Publisher("position_data", PosMsg)
    rospy.spin()