def statusInterpreter(status):

    # Variable Initialization
    gripperStatus = 0
    gripperMovement = 0

    # Gripper Reset
    if (status.gACT == 0) and (status.gIMC == 0):
        gripperStatus = 0

    # Gripper Activating
    if (status.gACT == 1) and (status.gIMC == 1) and (status.gSTA == 0):
        gripperStatus = 1

    # Gripper Activated
    if (status.gACT == 1) and (status.gIMC == 3):
        gripperStatus = 2

    # Gripper Open
    if (status.gPOA < 110) and (status.gPOB < 110) and (status.gPOC < 110):
        handClosed = False

    # Gripper Closed
    if (status.gPOA >= 110) and (status.gPOB >= 110) and (status.gPOC >= 110):
        handClosed = True

    # Gripper Moving
    if (status.gDTA == 0) and (status.gDTA == 0) and (status.gDTA == 0):
        handMoving = True

    # Gripper Stopped
    if (status.gDTA == 3) and (status.gDTA == 3) and (status.gDTA == 3):
        handMoving = False


    # Hand Moving and Opening
    if handMoving and not handClosed:
        gripperMovement = 0

    # Hand Moving and Closing
    if handMoving and handClosed:
        gripperMovement = 1

    # Hand Stoppend and Open
    if not handMoving and not handClosed:
        gripperMovement = 2

    # Hand Stopped and Closed
    if not handMoving and handClosed:
        gripperMovement = 3


    output = HoldingRegister()
    output.data = [gripperStatus, gripperMovement]
    
    # rospy.loginfo("Updating Robot Registers")

    pub_robot.publish(output)
Пример #2
0
    def setValues(self, address, value):
        """
            Sets the requested values to the holding registers
            and publishes they new values on a rostopic

            :param address: The starting address
            :type address: int
            :param values: The new values to be set
            :type values: list[int]
        """

        ModbusSequentialDataBlock.setValues(self, address, value)
        if address >= self.reading_start:
            msg = HoldingRegister()
            msg.data = value
            self.pub.publish(msg)
Пример #3
0
    def setValues(self, address, value):
        """ 
            Sets the requested values to the holding registers
            and publishes they new values on a rostopic 

            :param address: The starting address
            :type address: int
            :param values: The new values to be set
            :type values: list[int]
        """
#         print "address",address,"value",value
        
        
        ModbusSequentialDataBlock.setValues(self,address, value)
        if address >= self.reading_start:
            msg = HoldingRegister()
            msg.data = value
            self.pub.publish(msg)
Пример #4
0
        rospy.loginfo("Modbus server registers have been updated: %s",
                      str(msg.data))

    sub = rospy.Subscriber("modbus_wrapper/input",
                           HoldingRegister,
                           showUpdatedRegisters,
                           queue_size=500)
    #################

    #################
    # Example 3
    # writing to modbus registers using a standard ros publisher
    pub = rospy.Publisher("modbus_wrapper/output",
                          HoldingRegister,
                          queue_size=500)
    output = HoldingRegister()
    output.data = xrange(20, 40)
    output2 = HoldingRegister()
    output2.data = xrange(40, 20, -1)

    rospy.loginfo("Sending arrays to the modbus server")
    while not rospy.is_shutdown():
        rospy.sleep(1)
        pub.publish(output)
        rospy.sleep(1)
        pub.publish(output2)
    #################

    # Stops the listener on the modbus
    modclient.stopListening()
Пример #5
0
        rospy.loginfo("Modbus register have been written: %s", str(msg.data))
        rospy.sleep(2)

    sub = rospy.Subscriber("modbus_server/read_from_registers",
                           HoldingRegister,
                           callback,
                           queue_size=500)
    ###############

    ###############
    # Example 4
    # Publisher to write first 20 modbus registers from any node
    pub = rospy.Publisher("modbus_server/write_to_registers",
                          HoldingRegister,
                          queue_size=500)
    rospy.sleep(1)
    msg = HoldingRegister()
    msg.data = range(20)
    msg2 = HoldingRegister()
    msg2.data = range(20, 0, -1)

    while not rospy.is_shutdown():
        pub.publish(msg)
        rospy.sleep(1)
        pub.publish(msg2)
        rospy.sleep(1)
    ################
    rospy.spin()

    mws.stopServer()
Пример #6
0
    
    ###############
    # Example 3
    # Listen for the writeable modbus registers in any node
    def callback(msg):
        rospy.loginfo("Modbus register have been written: %s",str(msg.data))
        rospy.sleep(2)
    sub = rospy.Subscriber("modbus_server/read_from_registers",HoldingRegister,callback,queue_size=500) 
    ###############
    
    
    ###############
    # Example 4
    # Publisher to write first 20 modbus registers from any node
    pub = rospy.Publisher("modbus_server/write_to_registers",HoldingRegister,queue_size=500)
    rospy.sleep(1)
    msg = HoldingRegister()
    msg.data = range(20)
    msg2 = HoldingRegister()
    msg2.data = range(20,0,-1)
 
    while not rospy.is_shutdown():
        pub.publish(msg)
        rospy.sleep(1)
        pub.publish(msg2)
        rospy.sleep(1)
    ################
    rospy.spin()
        
    mws.stopServer()
Пример #7
0
 #################
 # Example 2
 # Create a listener that show us a message if anything on the readable modbus registers change
 rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
 def showUpdatedRegisters(msg):
     rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
 sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
 #################
 
  #################
 # Example 3
 # writing to modbus registers using a standard ros publisher
 
 pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
 output = HoldingRegister()
 output.data = [1 for x in xrange(0,6)]
 output2 = HoldingRegister()
 output2.data = [0 for x in xrange(0,6)]
  
 rospy.loginfo("Sending arrays to the modbus server")
 for i in xrange(5):
     rospy.sleep(1)
     pub.publish(output)
     rospy.sleep(1)
     pub.publish(output2)
 #################
 
 rospy.loginfo("Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c")
 rospy.spin()
 # Stops the listener on the modbus
 modclient.stopListening()
Пример #8
0
 
 #################
 # Example 2
 # Create a listener that show us a message if anything on the readable modbus registers change
 rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
 def showUpdatedRegisters(msg):
     rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
 sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
 #################
 
 #################
 # Example 3
 # writing to modbus registers using a standard ros publisher
 pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
 output = HoldingRegister()
 output.data = xrange(20,40)
 output2 = HoldingRegister()
 output2.data = xrange(40,20,-1)
 
 rospy.loginfo("Sending arrays to the modbus server")
 while not rospy.is_shutdown():
     rospy.sleep(1)
     pub.publish(output)
     rospy.sleep(1)
     pub.publish(output2)
 #################
 
 # Stops the listener on the modbus
 modclient.stopListening()
 
 
Пример #9
0
    sub = rospy.Subscriber("modbus_wrapper/input",
                           HoldingRegister,
                           showUpdatedRegisters,
                           queue_size=500)
    rospy.spin()
    #################

    #################
    # Example 3
    # writing to modbus registers using a standard ros publisher

    pub = rospy.Publisher("modbus_wrapper/output",
                          HoldingRegister,
                          queue_size=500)
    output = HoldingRegister()
    output.data = [1 for x in range(0, 6)]
    output2 = HoldingRegister()
    output2.data = [0 for x in range(0, 6)]

    rospy.loginfo("Sending arrays to the modbus server")
    for i in xrange(5):
        rospy.sleep(1)
        pub.publish(output)
        rospy.sleep(1)
        pub.publish(output2)
    #################

    rospy.loginfo(
        "Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c"
    )
    # Stops the listener on the modbus