simlego2.m

该程序用PID系统实现差动转向车辆控制

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


%% This script initialize the simulation and show the use of
%% the function deadReckonSpace
%%sr,sl expressed in meters
%%space traveled by the right wheel
sr=5;
%%space traveled by the left wheel
sl=8;

%%the initial orientation of the robot
%%the angle is counter clockwise from the x axis
theta0=0
%%b is the axis length of the robot that connect the 2 wheels expressed in
%%meters
b=0.5;

start_pos=[0,0,theta0];
end_pos=deadReckonSpace(start_pos,sr,sl,b);

%% Draw the traces for the left and right wheel
DrawRobotPosition(start_pos,b);
DrawRobotPosition(end_pos,b);

%%now define the space boundaries
%%becareful to plan  the trajectory well or the robot will
%%go outside
xmin=min(start_pos(1),end_pos(1))-b
xmax=max(start_pos(1),end_pos(1))+b
ymin=min(start_pos(2),end_pos(2))-b
ymax=max(start_pos(2),end_pos(2))+b
box_space=[xmin xmax ymin ymax];
axis(box_space);

title('Differential steering robot simulation');
grid on;
xlabel('X coordinate');
ylabel('Y coordinate');