//
// contest.sce
//

// System component values
l = 0.4; // 40cm 
Mp = 1.0; // 1kg
Mc = 0.2; // 200g 
g = 9.81; // good old gravity 
rw = 0.03; // 6cm dia tires 
K = 0.5; // motor speed constant 
R = 7; // motor resistance 
J = 0.005; // rotor inertia 


// System state
x0 = 0;		// initial conditions for position
v0 = 0;
theta0 = 0.0;	// the initial position for the load
omega0 = 0.0;
X=[x0, v0, theta0, omega0];


// The controller definition
PI = 3.14159;
ppr = 16;	// encoder pulses per revolution;
Kpot = 1.72;	// the angle voltage ratio
Vzero = 2.5;	// the voltage when the pendulum is vertical
Vadmax = 5;	// the A/D voltage range
Vadmin = 0;
Cadmax = 255;	// the A/D converter output limits
Cadmin = 0;
tolerance = 0.5;	// the tolerance for the system to settle
Kpp = 20;	// position feedback gain
Ksp = 2;	// sway feedback gain
Vpwmmax = 12; // PWM output limitations in V
Cpwmmax = 255; // PWM input range
Cdeadpos = 100;	// deadband limits
Cdeadneg = 95;

function foo=control(Cdesired)
	Cp = ppr * X($, 1)/(2*PI*rw);
	Cpe = Cdesired - Cp;
	Cpc = Kpp * Cpe;
	VL = Kpot * X($,3) + 2.5; // assume the zero angle is 2.5V
	CL = ((VL - Vadmin) / (Vadmax - Vadmin)) * (Cadmax - Cadmin);
	if CL > Cadmax then CL = Cadmax; end	// check for voltages over limits
	if CL < Cadmin then CL = Cadmin; end
	CLc = Ksp * (CL - (Cadmax + Cadmin) / 2);
	Cc = Cpc + CLc;
	Cpwm = 0;
	if Cc > 0.5 then	// deadband compensation
		Cpwm = Cdeadpos + (Cc/Cpwmmax)*(Cpwmmax - Cdeadpos);
	end
	if Cc <= -0.5 then
		Cpwm = -Cdeadneg + (Cc/Cpwmmax)*(Cpwmmax - Cdeadneg);
	end

	foo = Vpwmmax * (Cpwm / Cpwmmax) ; // PWM output
	if foo > Vpwmmax then foo = Vpwmmax; end // clip voltage if too large
	if foo < -Vpwmmax then foo = -Vpwmmax; end
endfunction

// The motion profile generator
function foo=motion(t_start, t_end, t_now, C_start, C_end)
	if t_now < t_start then
		foo = C_start;
	elseif t_now > t_end then
		foo = C_end;
	else
		foo = C_start + (C_end - C_start) * (t_now - t_start ) / (t_end - t_start);
	end
endfunction


// define the state matrix function
term1 = Mc*rw^2 + J;	// Precalculate these terms to save time
term2 = R*term1;
Xd = 10;	// the setpoint 10 turns == 160 pulses
Cd = ppr * Xd / (2 * PI * rw) ;
function foo=derivative(state,t, h)
	Vs=control(motion(0, 6, t($, 1), 0, Cd));
//	Vs=control(Cd);
	term3 = cos(state($,3));	// precalculate some repeated terms to save time
	term4 = sin(state($,3));
	term5 = term1 + Mp * (term4 * rw)^2;

//printf("%f   %f \n", Cd, Vs);
	v_dot = -state($,2)*(K^2) / (term5 * R) ...		// d/dt v
		+ term3*term4*Mp*g*rw^2 / term5 ...
		+ state($,4)^2 * Mp*l*term4*rw^2 / term5 ...
		+ Vs*K*rw / term5;

	foo = [ state($,2), ...		// d/dt x = v
		v_dot, ...
		state($, 4), ...	// d/dt theta
		-g * term4 / l - state($, 2) * term3 / l ...	// d/dt omega
		]; 
endfunction


// Integration Set the time length and step size for the integration
steps = 5000;	// The number of steps to use
t_start = 0;	// the start time - normally use zero
t_end = 10;	// the end time
h = (t_end - t_start) / steps;	// the step size
t = [t_start];	// an array to store time values
for i=1:steps,
	t = [t ; t($,:) + h];
	X = [X ; X($,:) + h * derivative(X($,:), t($,:), h)];	// first order
end


// Graph the values for part e)
plot2d(t, [X(:,1) + l*sin(X(:,3))], [-2], leg="mass position");
//plot2d(t, [X(:,1), 10*X(:, 3)], [-2, -5], leg="position@theta (X 10)");
xtitle('Time (s)');


// printf the values over time
intervals = 20;
for time_count=1:intervals,
	i = int((time_count - 1)/intervals * steps + 1);
	xmass = X(i,1) + l*sin(X(i,3));
	printf("Mass location at t=%f  x=%f \n", i * h, xmass);
	// printf("Point at t=%f  x=%f, v=%f, theta=%f, omega=%f \n", i * h, X(i, 1), X(i, 2), X(i, 3), X(i, 4));
end


// find the settling time in the results array
ts = 0;
for i = 1:steps,
	xmass = X(i,1) + l*sin(X(i,3));
	if xmass > (Cd + tolerance) then ts = i*h + t_start;
	end
	if xmass < (Cd - tolerance) then ts = i*h + t_start;
	end
end
printf("\nTheoretical settling time %f \n", ts);