simaccelerationsimpson.m

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

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

%%This simulation implement the acceleration in the model
%% a Simpson method to solve the equations is used
close all
%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.02;
%%the right wheel velocity
vr=0.02;
%% the acceleration time
ar=0.55;
al=0.5;

%%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;
%%the simulation time
tstart=0;
tend=4;
timestep=0.005;


%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0,theta0];
position_pts=[start_pos];
old_pos=[start_pos];
lwheel_trace=LeftWheelLoc(start_pos,b);
rwheel_trace=RightWheelLoc(start_pos,b);
maxNumSteps=500;
figure(1);
title(['Odometry simulation using acceleration...','ar =',num2str(ar),'al =',num2str(al)]);
xlabel('X coordinate');
ylabel('Y coordinate');
DrawRobotPosition(old_pos,b);
numSteps= getSimpsonIntervalsStart(ar,al,vr,vl,b,tend-tstart);
fprintf('The number of steps are: %s',num2str(numSteps));
if (numSteps<30)
    numSteps=30;
else
 if(numSteps>maxNumSteps)
        numSteps=maxNumSteps;
    end
end%%end if
stepSize = (tend-tstart) / numSteps;

for i=1:1:nsteps
    new_pos=deadReckonSimpson(ar,al,vr,vl,b,old_pos,i*stepSize);
    position_pts=[position_pts; new_pos];
    lwheel_trace=[lwheel_trace; LeftWheelLoc(new_pos,b)];
    rwheel_trace=[rwheel_trace; RightWheelLoc(new_pos,b)];
    old_pos=new_pos;
    DrawRobotPosition(old_pos,b);
    vl=vl*stepSize+vl;
    vr=vr*stepSize+vr;
    pause(0.5);
end;

figure(2);
hold on;
DrawWheelTrace(lwheel_trace,'g');
DrawWheelTrace(rwheel_trace,'b');