FIFO Queue in Polyscope

Hi!

For my project I’d like to programm the robot to tend six workspaces spread around a constant radius around the robot base. The workspaces sequentialy trigger the robot tending through inputs inside a thread. I would like to achieve the FIFO approach. If for example, the workspace 1 triggers the robot to approach and do some work, and in the mean time, while the robot is doing it’s magic on the #1, the #5 and #3 triggers as well, the robot would first go to the third and after that to the fifth eventhough, the workspace #5 was asking for the robot first. I figured this is due to the sequential, step-by-step code execution inside Robot Program (Polyscope). I want to make sure that first-come first-serve basis is established which is critical for my applicaiton.

*The triggering digital inputs are pulses.

Is there a way to program a FIFO queue on a URScript level? I’ve already looked at this tutorial but it’s old and hard to work it out.

My code, toned down for three workspaces, right now looks like this:

Before Start
Go_WS1 = False
Go_WS2 = False
Go_WS3  = False

Robot Program

if Go_WS1 = True
Do magic here and then,
Go_WS1 = False
if Go_WS2 = True
Do magic here and then,
Go_WS2 = False
if Go_WS3 = True
Do magic here and then,
Go_WS3 = False


Thread

If Digital_IN[0] == True
Set Go_WS1 = True
If Digital_IN[1] == True
Set Go_WS2 = True
If Digital_IN[2] == True
Set Go_WS3 = True

first go to the third and after that to the fifth eventhough, the workspace #5 was asking for the robot first

If you want actual first come first serve, the robot should service number 5 first, then 3. By this logic, you could re-queue #1 repeatedly in such a way that #5 never gets serviced, if you scan from top to bottom. I had a similar use case (using 5 stations) and wrote my own queue datastructure in URScript, which I’ve modified for your 6 stations and included below.

global queue = [0,0,0,0,0,0]
queueSize = 6
queueIndex = 0
queueBack = queueSize - 1

def pushQueue(switchNumber):
  count = 0
  while(count < 4000):
    count = count + 1
  end
  queue[queueIndex] = switchNumber
  queueIndex = queueIndex + 1
  if(queueIndex >= queueSize):
    queueIndex = queueIndex - 1
  end
  sync()
end

def popQueue():
  count = 0
  while(count < 4000):
    count = count + 1
  end
  returnNum = queue[0]
  shiftLeft()
  queueIndex = queueIndex - 1
  return returnNum
end

def shiftLeft():
  queue[0] = queue[1]
  queue[1] = queue[2]
  queue[2] = queue[3]
  queue[3] = queue[4]
  queue[4] = queue[5]
  queue[5] = 0
end

def getFirstInQueue():
  return queue[0]
end

def isInQueue(number):
  i = 0
  while(i < queueSize):
    if(number == queue[i]):
      return True
    end
    i = i + 1
  end
  return False
end

isInQueue(stationNumber) returns true if the provided station number is present in the queue, false otherwise
pushQueue(stationNumber) loads the provided station number into the queue at the first empty spot
popQueue() returns the stationNumber in the first spot of the queue and shifts everything else left
getFirstInQueue() returns the station number in the first spot on the queue. Does not alter the queue contents

Here’s how to use the Thread and the “isInQueue()” function to prevent loading the queue with duplicate calls to the same station:
image

And here is how to drive the main program:

image

Let me know if you have any questions about any of it

1 Like
If Go_NEXT == 0 
   If Digital_IN[0] == True
   Set Go_NEXT = 1
   If Digital_IN[1] == True
   Set Go_NEXT = 2
   If Digital_IN[2] == True
   Set Go_NEXT = 3

Instead of setting 3(5) different variables for your goto next, set a single variable to 1-5 and add a top level if to only check inputs if not waiting on a gotonext

Then use a normal switch case to call each separate station job. At the start of that station, clear the Go_NEXT to zero

1 Like

Hi @eric.feldmann,

I’ve tried your script today. Just copied and ran it exactly like you wrote. It worked flawlessly, thank you! I haven’t noticed any slow-down regarding computation resources either.

If I were to add another exception into this six-station FIFO, for example, something like a tool change that occurs after some counter triggers it and that tool change must happen with a top priority before continuing on with going to the next station in the queue. How could I expand this script to accomodate something like an interrupt. Maybe go with an Event node?

Kind regards

1 Like

Depends on what you’re after regarding priority. Should it finish it’s current station, and then tool change, or should it completely abort its current task and go tool change? I’ve seen people desire both operations.

The easiest thing is to just let the robot finish what it’s currently doing, then go toolchange. For this, I would probably write a “pushToFront()” method for that script file. You’d have to increase the array (the top lines of the file) to be of size 7 and contain 7 elements. Then just mimic the popQueue() command in reverse. Do a shiftRight() method where you assign all elements one index to the right, then insert some number into index 0 that indicates toolchange. (Maybe use 0, or 1000, or whatever). Then you just add another if() into your main program that says if(getFirstInQueue() == 0 (or 1000, etc)) then call tool change.

Otherwise, if you’d rather not mess with the queue script, you can just put the whole block of ifs() that you currently have inside ANOTHER if() that just says something like (if not(tool_change_requested)). Then have your thread set a boolean tool_change_requested to True whenever you deem it necessary. Basically, just block the entire queuing operation with that boolean.

Hi,

That’s exactly what I did. I was going for a “finish what you are currently doing and after that prioritize tool change. When tool is changed, resume”. Much appreciated!

The locking flag is also a convinient idea. Thanks.

I would make a Var called “Next_Run” and increment it after each workspace run is complete.
Make another var called “Run_Number”, When a Workspace requests work, make it’s G0_WS1 = Run_Number and then increment Run_Number. At then end of the shift, you would see how many Cycles the robot had made.
The each job would start with
If Go_WS1=Next_Run , this would cause them to be done in the order requested.