VREP仿真matlab控制四轮小车到达目标位置

matlab与VREP通信控制四轮小车到达指定位置，matlab控制小车实现转向、调速、调偏差，里面含有VERP建立四轮小车的模型

%此代码的功能是控制小车自动转向并行驶到目标点

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‘19999truetrue50005）;
if （clientID<0）
    disp（‘Failed connecting to remote API server‘）;    
else 
    vrep.simxAddStatusbarMessage（clientID‘Connection succeeded‘vrep.simx_opmode_oneshot）;
    vrep.simxStartSimulation（clientIDvrep.simx_opmode_oneshot）;
    [resMotorHandle_Left] = vrep.simxGetobjectHandle（clientID‘joint2‘vrep.simx_opmode_blocking）;
    [resMotorHandle_Right] = vrep.simxGetobjectHandle（clientID‘joint1‘vrep.simx_opmode_blocking）;
    
    [resTarget] = vrep.simxGetobjectHandle（clientID‘target‘vrep.simx_opmode_blocking）;%获取目标位置
    [resTip_M] = vrep.simxGetobjectHandle（clientID‘tip_M‘vrep.simx_opmode_blocking）;%获取M点
    [resTip_R] = vrep.simxGetobjectHandle（clientID‘tip_R‘vrep.simx_opmode_blocking）;%获取R点
    
    [~Position]=vrep.simxGetJointPosition（clientIDTargetvrep.simx_opmode_streaming）;
    disp（‘Position:‘）;
    disp（Position）;
    
    tic; %计时开始
    Left_vel = 0.2;  %正号向后
    Right_vel = 0;%正号向后
    [~num]=vrep.simxGetobjectParent（clientID5vrep.simx_opmode_oneshot）;%获取父关节句柄编号
    disp（‘num:‘）;
    disp（num）;
    Adjustnum=1;
    while toc<100 %计时结束
        vrep.simxSetJointTargetVelocity（clientIDMotorHandle_LeftLeft_velvrep.simx_opmode_oneshot  ）;%设置非球关节固有目标速度
        vrep.simxSetJointTargetVelocity（clientIDMotorHandle_RightRight_velvrep.simx_opmode_oneshot  ）;
        
        [~Position_M]=vrep.simxGetobjectPosition（clientIDTip_M-1vrep.simx_opmode_blocking）;%注意读取模式否则会出问题获取tip_M相对于坐标原点的x/y/z值
        [~Position_R]=vrep.simxGetobjectPosition（clientIDTip_R-1vrep.simx_opmode_blocking）;%获取tip_R相对于坐标原点的x/y/z值
        [~Position_T]=vrep.simxGetobjectPosition（clientIDTarget-1vrep.simx_opmode_blocking）;%获取Target相对于坐标原点的x/y/z值
        
        
        MT=[Position_T（1）-Position_M（1）Position_T（2）-Position_M（2）];%向量MT
        MR=[Position_R（1）-Position_M（1）Position_R（2）-Position_M（2）];%向量MR
        
        angle（Adjustnum）=acos（（MT（1）*MR（1）+MT（2）*MR（2））/（norm（MT）*norm（MR）））*180/pi;
       
        if angle（Adjustnum）>90
            Left_vel=0.2;
            Right_vel=0;
           if angle（Adjustnum）>110
             Left_vel=1;
             Right_vel=0;
           end
        else
            Left_vel=-0.2;
            Right_vel=0;
           if angle（Adjustnum）< 70
             Left_vel=-1;
             Right_vel=0;
           end
        end
            
        
        if angle（Adjustnum）>85 &&  angle（Adjustnum）<95
            if angle（Adjustnum）>88 &&  angle（Adjustnum）<92
                 Left_vel=-2;
                Right_vel=-2;
            else
                if angle（Adjustnum）>90
                    Left_vel=0.5;
                    Right_vel=0;
                else
                    Left_vel=-0.5;
         

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-10-28 12:31  控制小车到达目标点\
     文件      310664  2018-10-28 12:35  控制小车到达目标点\car.ttt
     目录           0  2018-10-28 12:35  控制小车到达目标点\Matlab\
     文件        3959  2018-10-28 12:35  控制小车到达目标点\Matlab\body.m
     文件      100875  2018-01-25 17:03  控制小车到达目标点\Matlab\remApi.m
     文件       70656  2018-01-25 17:03  控制小车到达目标点\Matlab\remoteApi.dll
     文件       35378  2018-01-25 17:03  控制小车到达目标点\Matlab\remoteApiProto.m
     文件        2718  2018-10-23 15:13  控制小车到达目标点\Matlab\simpleTest.m
     文件          97  2018-10-24 08:49  控制小车到达目标点\Matlab\test.m
     文件        1025  2018-10-26 22:47  控制小车到达目标点\Matlab\Untitled.m