control pioneer to move and stop using Matlab and API

Typically: "How do I... ", "How can I... " questions
Post Reply
aia.nagy
Posts: 2
Joined: 01 Mar 2017, 20:16

control pioneer to move and stop using Matlab and API

Post by aia.nagy » 02 Mar 2017, 20:29

i have a problem that may pioneer didnot controlled correctly using matlab ,as it
first :didnot move in a straight line
second :there is a delay between matlab and vrep

my code is as following:

Code: Select all

% --------------------------------------------------------------%
close all
clear variables 
clc
% --------------------------------------------------------------%   
    disp('Program started');
   
    vrep=remApi('remoteApi');                                              % using the prototype file (remoteApiProto.m)
    vrep.simxFinish(-1);                                                   % just in case, close all opened connections
    
    
    clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
    
   % vrep.simxSynchronous(clientID, True);                                  %for synchronization
   % vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait);
    

    if (clientID>-1)
        disp('Connected to remote API server');
        
        % Handle 
        [returnCode,target_vehicle1]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking);
        [returnCode,target_vehicle2]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx#1',vrep.simx_opmode_blocking);
        [returnCode,mine_vehicle]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx#0',vrep.simx_opmode_blocking);
        [returnCode,floor_visible_handle]=vrep.simxGetObjectHandle(clientID,'ResizableFloor_5_25_visibleElement',vrep.simx_opmode_blocking);
        [returnCode,left_Motor]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor#0',vrep.simx_opmode_blocking);        
        [returnCode,right_Motor]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor#0',vrep.simx_opmode_blocking);
        [returnCode,front_Sensor]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor5#0',vrep.simx_opmode_blocking);
        [returnCode,front_Sensor_r]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor5#0',vrep.simx_opmode_blocking);
        [returnCode,front_Sensor_l]= vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor4#0',vrep.simx_opmode_blocking);
        
 
        [returnCode,target_vehicle]=vrep.simxGetDistanceHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking);
        %[returnCode,Camers]= vrep.simxGetObjectHandle(clientID,'Vision_sensor',vrep.simx_opmode_blocking);

        
        %Other code
        [returnCode]= vrep.simxSetJointTargetVelocity(clientID,left_Motor,10,vrep.simx_opmode_blocking);
        [returnCode]=vrep.simxSetJointTargetVelocity(clientID,right_Motor,10,vrep.simx_opmode_blocking);
        [returnCode,detectionState_r,detectedPoint_r,~,~]= vrep.simxReadProximitySensor(clientID,front_Sensor_r,vrep.simx_opmode_streaming);
        [returnCode,detectionState_l,detectedPoint_l,~,~]= vrep.simxReadProximitySensor(clientID,front_Sensor_l,vrep.simx_opmode_streaming);
        
        [returnCode,target_vehicle1_position]=vrep.simxGetObjectPosition(clientID,target_vehicle1,floor_visible_handle,vrep.simx_opmode_streaming);
        [returnCode,target_vehicle2_position]=vrep.simxGetObjectPosition(clientID,target_vehicle2,floor_visible_handle,vrep.simx_opmode_streaming);
        [returnCode,mine_vehicle_position]=vrep.simxGetObjectPosition(clientID,mine_vehicle,floor_visible_handle,vrep.simx_opmode_streaming);
        
        %[result,info]=vrep.simxGetOutMessageInfo(clientID,vrep.simx_headeroffset_message_id);
        %[result,info]=vrep.simxGetInMessageInfo(clientID,vrep.simx_headeroffset_message_id);
        %[returnCode,resolution,image]=vrep.simxGetVisionSensorImage2(clientID,Camers,0,vrep.simx_opmode_streaming);
        

        
        for i=1:20
            
        [returnCode,detectionState_r,detectedPoint_r,~,~]= vrep.simxReadProximitySensor(clientID,front_Sensor_r,vrep. simx_opmode_buffer);
        [returnCode,detectionState_l,detectedPoint_l,~,~]= vrep.simxReadProximitySensor(clientID,front_Sensor_l,vrep.simx_opmode_buffer);
        
        V1position_start=target_vehicle1_position;
        V2position_start=target_vehicle2_position;
        Mposition_start=mine_vehicle_position;
        
        [returnCode,target_vehicle1_position]=vrep.simxGetObjectPosition(clientID,target_vehicle1,floor_visible_handle,vrep.simx_opmode_buffer);
        [returnCode,target_vehicle2_position]=vrep.simxGetObjectPosition(clientID,target_vehicle2,floor_visible_handle,vrep.simx_opmode_buffer);
        [returnCode,mine_vehicle_position]=vrep.simxGetObjectPosition(clientID,mine_vehicle,floor_visible_handle,vrep.simx_opmode_buffer);
       %[returnCode,resolution,image]=vrep.simxGetVisionSensorImage2(clientID,Camers,0,vrep.simx_opmode_buffer);
        %imshow(image);
        
        V1position_new=target_vehicle1_position;
        V2position_new=target_vehicle2_position;
        Mposition_new=mine_vehicle_position;
        
        V1velocity=norm(V1position_start-V1position_new);
        V2velocity=norm(V2position_start-V2position_new);
        Mvelocity=norm(Mposition_start-Mposition_new);
        
        RDv1=norm(mine_vehicle_position-target_vehicle1_position);
        RDv2=norm(mine_vehicle_position-target_vehicle2_position);
        RVv1=norm(Mvelocity-V1velocity);
        RVv2=norm(Mvelocity-V2velocity);
        
        
        x=norm(detectedPoint_r);
        
        display(RDv1);
        display(RDv2);
        display(RVv1);
        display(RVv2);
        display(x);                                                        % to get the distance 
        fprintf('distance = %f \n',x);
        
        plot(x);
       % xlabel ('time','Fontsize',14);
       % ylabel ('Relative distance','Fontsize',14);
       % title ('Relative distance','Fontsize',12)
       % legend('Relative distance');
        
       
       % for i=1:3
        if (x<.8)
            break;
        end
        %end
        %pause(.1);        
        end
  
        [returnCode]= vrep.simxSetJointTargetVelocity(clientID,left_Motor,0,vrep.simx_opmode_blocking);
        [returnCode]=vrep.simxSetJointTargetVelocity(clientID,right_Motor,0,vrep.simx_opmode_blocking);
       
        vrep.simxFinish(-1);
    end
    
        vrep.delete();                                                     % call the destructor!

coppelia
Site Admin
Posts: 7324
Joined: 14 Dec 2012, 00:25

Re: control pioneer to move and stop using Matlab and API

Post by coppelia » 05 Mar 2017, 18:51

Hello,

how does the robot move? In a curved line? I didn't read through all the code, but if you really want to make sure to move on a straight line, you need some kind of sensor feedback (e.g. orientation of the robot). A real robot will also never move on a straight line in open loop.

The delay betwen Matlab and V-REP is normal, if it is not much higher than 10-20 ms. But you should make sure to stream all the values you read, and when you send a command that doesn't require a return value, use the oneshot-operation mode. Blocking mode is only if you require a value once: your client will basically send out a command, then wait until V-REP sends a reply.

If you need to run your client and V-REP synchronously, then you will have to run V-REP in synchronous mode.

Cheers

Post Reply