CoppeliaSim forums

Code: Select all

function sysCall_init()
    -- Initialize motors and the vision sensor
    motGauche = sim.getObjectHandle("Pioneer_p3dx_leftMotor")
    motDroit = sim.getObjectHandle("Pioneer_p3dx_rightMotor")
    ir = sim.getObjectHandle("Vision_sensor")
    
    -- Initialize drawing object for tracking the robot's path
    robotTrace = sim.addDrawingObject(sim.drawing_linestrip + sim.drawing_cyclic, 2, 0, -1, 200, {1, 1, 0}, nil, nil, {1, 1, 0})

    -- Initialize parameters
    v = 0.2 -- Linear velocity
    k = 0.6 -- Proportional gain for the controller
    b = 0.33 / 2 -- Coefficient for the speed adjustment
    R = 0.1 -- Wheel radius
    
    -- Initialize graph for visualizing the robot's position
    graph = sim.getObjectHandle("/Graph")
    objectPosX = sim.addGraphStream(graph, 'object pos x', 'm', 1)
    objectPosY = sim.addGraphStream(graph, 'object pos y', 'm', 1)
    sim.addGraphCurve(graph, 'pioneer position x/y', 2, {objectPosX, objectPosY}, {0, 0}, 'm by m')
end

function sysCall_sensing()
    -- Mark the robot's path and track the actual path it follows
    local p = sim.getObjectPosition(sim.getObjectHandle('Pioneer_p3dx'), -1)
    sim.addDrawingObjectItem(robotTrace, p)
end

function sysCall_actuation()
    -- Capture the image from the vision sensor
    local result, data = sim.getVisionSensorImage(ir + sim.handleflag_greyscale)
    if result >= 0 then
        -- Check the structure of 'data'
        if type(data) == "table" and #data >= 3 then
            local width = data[1] -- Image width
            local height = data[2] -- Image height
            local imageData = data[3] -- Image data starts from index 3

            -- Verify that 'imageData' is a table
            if type(imageData) == "table" then
                -- Calculate the average gray level of the image
                local averageGray = 0
                local numPixels = width * height
                
                for i = 1, numPixels do
                    averageGray = averageGray + imageData[i]
                end
                averageGray = averageGray / numPixels

                -- Calculate error and motor speeds
                local error = (averageGray - 0.5) * 2 -- Normalize around 0
                local w = -k * error
                local vg = v - (b * w)
                local vd = v + (b * w)
                local wg = vg / R
                local wd = vd / R

                -- Update motor velocities
                sim.setJointTargetVelocity(motGauche, wg)
                sim.setJointTargetVelocity(motDroit, wd)

                -- Record the robot's coordinates
                local pos = sim.getObjectPosition(sim.getObjectHandle('Pioneer_p3dx'), -1)
                
                -- Update the graph
                sim.setGraphStreamValue(graph, objectPosX, pos[1])
                sim.setGraphStreamValue(graph, objectPosY, pos[2])
            else
                print("Error: imageData is not a table.")
            end
        else
            print("Error: The data returned by getVisionSensorImage is incorrect.")
        end
    else
        -- If reading fails, stop the robot or adjust safety behaviors
        sim.setJointTargetVelocity(motGauche, 0)
        sim.setJointTargetVelocity(motDroit, 0)
    end
end

function sysCall_cleanup()
    -- Cleanup if necessary
end

Code: Select all

if result >= 0 then