// SQ file for Segway simulator.
// http://www.segway.com/
// Francois Roulet, mailto:francois.roulet@epfl.ch
// EPFL - Swiss Federal Institute of Technology.
// Version 2.0, 13.2.2005
// Thanks to Yves Piguet of Calerga for his invaluable help.


title "Segway simulator"
// Interactive Ackermann poles placement for Segway

version
{@
<pre>Version 2.0, 13.2.2005
francois.roulet@epfl.ch

EPFL
Swiss Federal Institute of Technology
CH-1015 Lausanne
Switzerland</pre>
@}

help
{@<pre>
There's a Segway simulator (with the mechanical data of Joe-le-pendule):

In the first pane,
you can move the poles of the closed-loop system.
In the last pane,
you can adjust a disturbing torque.</pre>
@}


variable q			// Characteristic polynomial

variable A23, A43	// matrix A coefficients
variable B2, B4		// matrix B coefficients

variable Pc			// Controllability Matrix
variable qA			// Characteristic polynomial of matrix A
variable G			// System State
variable K			// Feedback matrix = the solution !
variables t, u		// FreeWave vector

variable G_closedloop		// Closed Loop Transfert function

variable Place		// Poles placement


menu "Poles placement A"
		_checkmark(Place==1) (q,Place) = SetPlaceSettings(1)

menu "Poles placement B"
		_checkmark(Place==2) (q,Place) = SetPlaceSettings(2)

menu "Poles placement C"
		_checkmark(Place==3) (q,Place) = SetPlaceSettings(3)


init (A23,A43,B2,B4,G,q,Place,t,u) = init


make (K, G_closedloop) = calc_Closedloop (G, q)



function
{@

use stdlib, lti


function (A23,A43,B2,B4,G,q,Place,t,u) = init

A23 = -5.75; A43 = 41.8; B2 = 1.58; B4 = -5.73;
G = ss([0,A23,0;0,0,1;0,A43,0], [B2;0;B4], eye(3));
Place = 2;												// Poles placement B
q = poly([-3.5, -3.5-j, -3.5+j]);
t = [0, 1.5, 4.5, 5.2, 8];   u = [0, 1.5, 1.5, 0, 0];


subplots('Closed-Loop Poles\tReaction to Impulse disturbance\nBode Magnitude\tReaction to arbitrary Torque input\nFeedback matrix K')


function (q,Place) = SetPlaceSettings(Place)

switch Place
	case 1
		q = poly([-3.5, -3.5-3.5j, -3.5+3.5j]);
	case 2
		q = poly([-3.5, -3.5-j, -3.5+j]);
    case 3
    	q = poly([-8.5, -1-j, -1+j]);
end		
		

function drawPoles(q)
	
	z = [roots(q)];	
	z = [z(:); 0];
	sc = [min(real(z)), max(real(z)), min(imag(z)), max(imag(z))];
	sc = 1.5 * (sc + max(abs(sc)) * 0.1 * [-1, 1, -1, 1]);
	scale('equal', sc);
	sgrid;
	
	plotroots(q,'x',1);
	label('sigma', 'jw');


function (q,msg) = dragPoles(q,nb,z0,z1)
	
	if isempty(nb)
		cancel
	end
	(q, zz) = movezero(q, z0, z1);
	msg = dispComplex('Closed-loop pole: ', zz);


function (msg, cursor) = overCLPoles(z)
	
	if isempty(z)
		cancel;
	end
	cursor = true;
	msg = dispComplex('Closed-loop pole: ', z);


function msg = dispComplex(s, z)
	
	if imag(z) == 0
		msg = sprintf('%s%.2g', s, z);
	elseif imag(z) > 0
		msg = sprintf('%s%.2g+%.2gj', s, real(z), imag(z));
	else
		msg = sprintf('%s%.2g%.2gj', s, real(z), imag(z));
	end


function drawImpulseResponse(G_closedloop)

	scale([-0.5, 3.5, -2, 2]);
	impulse(G_closedloop, 'mcg');
	legend('dx/dt\ntheta\nd(theta)/dt', 'mcg')
	label('Time [s]');


function drawStepResponse(G_closedloop,t,u)

	scale([-0.5, 8, -1, 2]);
	lsim((G_closedloop), -u, t, 'mcg');
	plot(t, u, 'b', 1);
	legend('dx/dt\ntheta\nd(theta)/dt\nTorque-cmd', 'mcgb')
	label('Time [s]');
	
	
function drawBode(G_closedloop)
	
	scale('logdb', [1,100]);
	bodemag(G_closedloop, 'mcg');
	legend('dx/dt\ntheta\nd(theta)/dt', 'mcg')
	label('[Hz]');	


function (K, G_closedloop) = calc_Closedloop(G, q)

	Pc = ctrb(G);
	qA = polyvalm(q, G.A);
	K = (Pc \ qA)(end, :);
	G_closedloop = feedback(G, ss(K));


function drawController(K)

	font = fontset('Font', 'Helvetica', ...
	'Bold', true, ...
	'Size', 12, ...
	'Color', [0,0,1]);
  
	kij = 'Coefficient dx/dt :';
	kij = sprintf('%s\t\t%.1f',kij,K(1));
	text(kij, font);
	
	kij = 'Coefficient theta :';
	kij = sprintf('%s\t\t%.1f',kij,K(2));
	text(kij, font);
	
	kij = 'Coefficient d(theta)/dt :';
	kij = sprintf('%s\t\t%.1f',kij,K(3));
	text(kij, font);
	
	
function (t, u, msg) = dragWave(t, u, ix, x1, y1)

  if isempty(ix)
    cancel;             % not a click over a point
  end  
  if ix == 1
    cancel;				% first element
  end
  if ix == length(t)
    cancel;				% last element
  end

  if x1 < t(ix-1)				% too low
    t(ix) = t(ix-1);
  elseif x1 > t(ix+1)			% too high
    t(ix) = t(ix+1);
  else
    t(ix) = x1;
  end
  
  u(ix) = y1;
  msg = sprintf('u: %g  t: %g', u(ix), t(ix));


function (msg, cursor) = overWave(t, u, ix)
	
	if isempty(ix)
		cancel;
	end
	cursor = true;
	msg = sprintf('u: %g  t: %g', u(ix), t(ix));


@}


figure "Closed-Loop Poles"
	draw drawPoles(q)
	mousedrag (q,_msg) = dragPoles(q,_nb,_z0,_z1)
	mouseover (_msg, _cursor) = overCLPoles(_z0)

figure "Reaction to Impulse disturbance"
	draw drawImpulseResponse(G_closedloop)

figure "Reaction to arbitrary Torque input"
	draw drawStepResponse(G_closedloop,t,u)
	mousedrag (t, u, _msg) = dragWave(t, u, _ix, _x1, _y1)
	mouseover (_msg, _cursor) = overWave(t, u, _ix)
	
figure "Feedback matrix K"
	draw drawController(K)

figure "Bode Magnitude"
	draw drawBode(G_closedloop)