//Filename:  Balance.sci
// Group Members:  Matt Bloem, Clayton Campagna, Lyndsy Smith, John Wenstrom
// Group 11
// EGR 345
//This program simulates the balancing motion of the cart by displaying the position of the cart 
//with time.

// System component values
//l = 0.00137; // height of center of mass above axles 
l = 0.0432;
Mcart = 1.29;
Mweight = 0.5;
Mp = Mcart + Mweight; 
Mc = 0.0; // 0 kg
g = 9.81; // good old gravity 
rw = 0.0762; // 6in diameter wheels
K = 2.451; // motor speed constant 
R = 30; // motor resistance 
J = 0.005608; // 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;
diff_optical = (53/255)*5; // difference between optical sensors
Vzero = (373/1028)*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; // the tolerance for the system to settle
Kp = 15; // Proportional gain
Kd = 1; // Derivative gain
Ki = 1;
Vpwmmax = 12; // PWM output limitations in V
Cpwmmax = 255; // PWM input range
Cdeadpos = 80; // deadband limits
Cdeadneg = 80;
e_sum = 0;

function foo=control(Cdesired)
Cp = Kp* X($, 1)/(2*PI*rw);
Cpe = Cdesired - Cp;
Cpc = Kp * Cpe;
VL = diff_optical * X($,3) + Vzero; // assume the zero angle
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 = Kd * (CL - (Cadmax + Cadmin) / 2);
e_sum = e_sum + (Cp*(10/1000));
CI = Ki * e_sum;
Cc = Cpc + CLc + CI;
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 = 0; // the setpoint 10 turns == 160 pulses
Cd = 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), X(:, 3)], [-2, -5], leg="position@theta (X 10)");
plot2d(t, X(:, 3), 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);