car_sup.m

Matlab学习课件

function car_sup
% Supervisory controller for carriage position

procglbl	% Global definitions

global scar_next dmove dstop skip_cruise sign delv
	% These aren't really global,  but do have to be 'static'
	% Matlab doesn't have a 'static' declaration so this will have to do!

if Scar_sup == 0
	% Initialization section - occurs once only
	Scar_sup = 1;	% Make sure this section is not executed again!
	scar_next = 1;	% First state.
		%The default is -1 which indicates stay in same state
	return;
end

if tstep < Tcar_sup_next
	return;
end

Tcar_sup_next = Tcar_sup_next + Tcar_sup;	% Time for next run

if scar_next ~= -1
	% There has been a state change (including self-transition)
	 [i_audit,trans_trail]=audit(2,Scar_sup,scar_next,...
		tstep,trans_trail,i_audit);	% Record this transition
	Scar_sup = scar_next;
	run_entry = 1;
	scar_next = -1;	% Default - stay in same state
else
	run_entry = 0;
end

if Scar_sup == 1	% Wait for next move state 
		% compute move if a new move is requested
	% No Entry section
	%No Action section
	%Test/exit section
	if Xnext_car ~= Xset_car
		% A new move has been requested (Xset_car is the present setpoint)
		dmove = abs(Xnext_car - Xset_car);	% Total distance of the move
		dstop = 0.5 * Vc_car * Vc_car / Acc_car;
		if dstop > (0.5 * dmove)
			dstop = 0.5 * dmove;
			skip_cruise = 1;
		else
			skip_cruise = 0;
		end
		if Xnext_car > Xset_car, sign = 1;
		else sign = -1;
		end
		Vset_car = 0;
		scar_next = 2;
	end
elseif Scar_sup == 2		% Accelerate
	% Entry section
	if run_entry
		delv = Acc_car *  Tcar_sup * sign;	% Velocity increment
	end

	% Action section
	Vset_car = Vset_car + delv;
	Xset_car = Xset_car + Vset_car * Tcar_sup;

	% Test/exit section
	if skip_cruise & (abs(Xset_car - Xnext_car) >= dstop)
		scar_next = 4;	% Go directly to decel
	elseif abs(Vset_car) >= Vc_car
		% Go to cruise state
		Vset_car = Vc_car * sign;
		scar_next = 3;
	end

elseif Scar_sup == 3		% Cruise
	% No entry section
	% Action section
	Xset_car = Xset_car + Vset_car * Tcar_sup;

	% Test/exit section
	dend = abs(Xset_car - Xnext_car);
	if dend <= dstop
		% Go to decel
		scar_next = 4;
	end

elseif Scar_sup == 4		% Decel
	% No Entry section
	% Action section
	dend = abs(Xset_car - Xnext_car);	% Distance to stop from previous step
	tstop = sqrt(2 * dend / Acc_car) - Tcar_sup;	% Time to stop from this step
	if tstop < 0
		% End of profile has been reached
		tstop = 0;
		Xset_car = Xnext_car;
		Vset_car = 0;
	else
		dendnew = 0.5 * Acc_car * tstop * tstop;	% New distance to stop
		Vset_car = sqrt(2 * dendnew * Acc_car) * sign;
		Xset_car = Xnext_car - sign * dendnew;
	end

	% Test/exit section
	if tstop <= 0
		scar_next = 1;	% Go back to wait for next move
	end
else
	error('Task: Car_Sup -- unknown state encountered');
end