curve.m

来自「该程序用PID系统实现差动转向车辆控制」· M 代码 · 共 33 行

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

function vect_vel=curve(initial_pos,final_pos,b,time)
%%This function compute the kinematics for a turn
%% giving the initial position (including the heading)
%% and the final position (including the heading) of the robot
%% using the Tom Brown approssimation
x0=initial_pos(1);
y0=initial_pos(2);
theta0=initial_pos(3);
x1=final_pos(1);
y1=final_pos(2);
theta1=final_pos(3);
%% ss=(sr+sl)/2
%% sm=(sr-sl)/2
if(theta1==0)
    ss=(x1-x0);
else
    ss=(y1-y0)/sin(theta1);
end

sm=(theta1-theta0)*b;
sr=ss+sm;
sl=ss-sm;

vr=sr/time;
vl=sl/time;

vect_vel=[vl,vr];

end%%end function