Three Sorter

#import the joy of programming python module pyjop
from pyjop import *
#connect to the current SimEnv
SimEnv.connect()

#create references to entities in the SimEnv
env = SimEnvManager.first()
env.reset()

#function: change belt speed
def setSpd(conv,s):
    conv.set_target_speed(s)

#function: check is belt has a package on it
def isTran(conv):
    return conv.get_is_transporting()

#function: return rfid tag
def tag(rid):
    return rid.get_rfid_tag()

#define list: conveyor belts
belt = []
for b in range(5):
    belt.append(ConveyorBelt.find("belt" + str(b)))

#define list: range finders
rf = []
for b in range(2):
    rf.append(RangeFinder.find("scan" + str(b)))

#### main loop to retrieve data from the SimEnv, calculate stuff and send commands back into the SimEnv
while SimEnv.run_main():

    #initialize the belts that always go forward
    setSpd(belt[0],5)
    setSpd(belt[2],5)
    setSpd(belt[3],5)

    #barrel sorter
    if tag(rf[0]) == "Barrel":
        setSpd(belt[1],-10)
        while isTran(belt[1]):
            setSpd(belt[1],-10)
            sleep(1)
    elif tag(rf[0]) == "Cone" or "Box":
        setSpd(belt[1],5)
    else:
        if not isTran(belt[1]):
            setSpd(belt[1],0)

    #second sorter
    if tag(rf[1]) == "Cone":
        setSpd(belt[4],7)
    elif tag(rf[1]) == "Box":
        setSpd(belt[4],-7)
    else:
        if not isTran(belt[4]):
            setSpd(belt[4],0)



    #if the first 2 belts are empty then spawn the next package
    if not isTran(belt[0]) and not isTran(belt[1]):
        ObjectSpawner.find("spawner").spawn()

#cleanup close code
SimEnv.disconnect()