Arm1.txt

function checkColor(detectedObjectHandle)
    r,rgbData=sim.getShapeColor(detectedObjectHandle,nil,sim.colorcomponent_ambient_diffuse)
    if rgbData[1]==1 and rgbData[2]~=0 and rgbData[3]>0.015 then sim.setIntegerSignal('boxColor',1)--yellow
    elseif rgbData[1]==0 and rgbData[2]==1 and rgbData[3]<0.1 then sim.setIntegerSignal('boxColor',2)--green
    elseif rgbData[1]==1 and rgbData[2]>0.3 and rgbData[3]<0.1 then sim.setIntegerSignal('boxColor',3)--orange
    elseif rgbData[1]==1 and rgbData[2]==0 and rgbData[3]==0 then sim.setIntegerSignal('boxColor',4)--red
    else print('no such color')
    end
end

function colorSort(Proximity_sensor,aboveboxpos)
    result,PSensor_distance,detectedPoint,detectedObjectHandle=sim.readProximitySensor(Proximity_sensor)
    
    sim.setIntegerSignal('detectedBox',1)
    sim.setIntegerSignal('detectedObjectHandle',detectedObjectHandle)
             
    checkColor(detectedObjectHandle)
    sim.setObjectInt32Parameter(detectedObjectHandle,3003,1)
    sim.resetDynamicObject(detectedObjectHandle)
    sim.setObjectParent(detectedObjectHandle,target,true)
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,aboveboxpos,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
    
    color=sim.getIntegerSignal('boxColor')
    while true do
        result1=sim.readProximitySensor(Y1)
        result2=sim.readProximitySensor(Y2)
        result3=sim.readProximitySensor(Y3)
        if result1>0 and result2>0 and result3>0 then
            sim.wait(0.25)
        else break
        end
    end
    while true do
        result01=sim.readProximitySensor(R1)
        result02=sim.readProximitySensor(R2)
        result03=sim.readProximitySensor(R3)
        if result01>0 and result02>0 and result03>0 then
            sim.wait(0.25)
        else break
        end
    end
    
    position4={1.8-(color-1)*0.5,-0.5,0.85} --inside basket
    position5={1.8-(color-1)*0.5,-0.5,0.9} --above basket    
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,position5,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,position4,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
        
    sim.setObjectInt32Parameter(detectedObjectHandle,3003,0)
    sim.setObjectParent(detectedObjectHandle,-1,true)
    counter[color]=counter[color]+1
    
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,position5,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
    return position4,position5
end

function pickFromTable(i,j,x0,y0,z0,cubeSize,offset)
    
    aboveboxpos={x0+j*(cubeSize+offset),y0+i*(cubeSize+offset),z0+(cubeSize+offset)+0.02}
    onboxpos={x0+j*(cubeSize+offset),y0+i*(cubeSize+offset),z0}
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,aboveboxpos,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
    sim.rmlMoveToPosition(target,base,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,onboxpos,orient,targetVel)
    sim.setObjectOrientation(target, -1, {0,0,0})
    return aboveboxpos    
end

function sensor()
    sens={0,0,0,0}
    if sim.readProximitySensor(proxYellow)==1 then sens[1]=1 end
    if sim.readProximitySensor(proxGreen)==1 then sens[2]=1  end
    if sim.readProximitySensor(proxOrange)==1 then sens[3]=1  end
    if sim.readProximitySensor(proxRed)==1 then sens[4]=1  end
    return sens
end

function sysCall_threadmain()
    -- Put some initialization code here
    Proximity_sensor=sim.getObjectHandle('Proximity_sensor')
    target=sim.getObjectHandle('Target')
    proxRed=sim.getObjectHandle('Proximity_sensorR')
    proxGreen=sim.getObjectHandle('Proximity_sensorG')
    proxOrange=sim.getObjectHandle('Proximity_sensorO')
    proxYellow=sim.getObjectHandle('Proximity_sensorY')
    Y1=sim.getObjectHandle('Proximity_sensorY')
    Y2=sim.getObjectHandle('Proximity_sensorY2')
    Y3=sim.getObjectHandle('Proximity_sensorY3')
    R1=sim.getObjectHandle('Proximity_sensorR')
    R2=sim.getObjectHandle('Proximity_sensorR2')
    R3=sim.getObjectHandle('Proximity_sensorR3')
    
    sim.setIntegerSignal('boxColor',5)
    sim.setIntegerSignal('detectedBox',1)
    
    base=-1    
    vel=1
    accel=70
    jerk=70
    currentVel={0,0,0,0}
    currentAccel={0,0,0,0}
    maxVel={vel,vel,vel,vel}
    maxAccel={accel,accel,accel,accel}
    maxJerk={jerk,jerk,jerk,jerk}
    targetVel={0,0,0,0}
    orient=nil
    lengthStack=18
    widthStack=26
    x0=0.2
    y0=-2.1
    z0=0.73
    x1=1.14
    cubeSize=0.06
    offset=0.01
    counter={0,0,0,0}

    for i = 1,lengthStack,1
    do
        for j = 1,widthStack/2,1
        do
            aboveboxpos=pickFromTable(i,j,x0,y0,z0,cubeSize,offset)
            insideboxpos,aboveboxpos=colorSort(Proximity_sensor,aboveboxpos)
        end
    end
    for i = 1,lengthStack,1
    do
        for j = 1,widthStack/2,1
        do
            aboveboxpos=pickFromTable(i,j,x1,y0,z0,cubeSize,offset)
            insideboxpos,aboveboxpos=colorSort(Proximity_sensor,aboveboxpos)
        end
    end
end



function sysCall_cleanup()
    -- Put some clean-up code here
end

-- See the user manual or the available code snippets for additional callback functions and details