Commit a06eaf23 authored by Bindu Sharan's avatar Bindu Sharan
Browse files

Gitlab Runner Update from cstd-aux-files/master:Merge branch...

Gitlab Runner Update from cstd-aux-files/master:Merge branch '1-name-files-according-to-new-cstd-script-3' into 'master'
parent 858668fb
% Exercise 2.4 :: Bass-Gura pole placement
% Exercise 3.4 :: Bass-Gura pole placement
%
% TUHH :: Institut for Control Systems :: Control Systems 2
% Last update: 13.11.2008
% Last update: 19.10.2021
A = [1 -2; 3 -4];
b = [3; 1];
......@@ -19,4 +19,4 @@ f = -p/(C*Ta);
Acl = A+b*f;
figure
Scl = ss(Acl,b,eye(2),0);
step(Scl);
\ No newline at end of file
step(Scl);
% Exercise 2.7 :: Inverted Pendulum
% Exercise 3.7 :: Inverted Pendulum
%
% TUHH :: Institut for Control Systems :: Control Systems 2
% Last update: 13.10.2013
% Last update: 19.10.2021
%definition of useful vectors for simulation
clear all
......@@ -55,4 +55,4 @@ for i=1:length(t),
u(i) = F*X(i,:)';
end
figure(1);plot(t,u);grid;ylabel('control input'); xlabel('time(s)')
figure(2);plot(t,X(:,1:2)');grid;xlabel('time(s)');ylabel('angle(radius)')
\ No newline at end of file
figure(2);plot(t,X(:,1:2)');grid;xlabel('time(s)');ylabel('angle(radius)')
% exercise 3.2@ control design for an inverted pendulum with friction
% exercise 4.2@ control design for an inverted pendulum with friction
%definition of useful vectors for simulation
clear all
......
close all;
load('MinSegPlant\Plant_Parameters.mat');
load('MinSegPlant\Plant_Parameters_VAR.mat');
%Simulation of the Self Balancing Robot for Stabilization
% Design of three feedback controllers:
% - LQR Design
%State-Space Model
sys = ss(A, B, C, D);
%Check Controllability and Observability
disp('Number of controllable states:');
rank(ctrb(sys)) %or rank(ctrb(A,B))
disp('Number of observable states:');
rank(obsv(sys)) %or rank(obsv(A,C))
%% Linear-Quadratic-Regulator Design
% ************** LQR **************
%Tuning Q and R
R = 0.1;
Q = blkdiag(1000,1000,10,1);
%LQR Gain
K_LQR = -lqr(A,B,Q,R); %continuous
%Closed loop state space model
sys_cl = ss(A+B*K_LQR, B, C, D); %continuous
%% LQR Simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
open('Task_2_Simulation_LQR');
%set this values either to 0 or 1
runLinear = 1;
runNonlinear = 1;
%set initial condition
x0 = [0;5*pi/180;0; 0];
x0 = [0;9*pi/180;0; 0];
%Simulation Time
set_param('Task_2_Simulation_LQR', 'StopTime', '2')
% reference sine input for position
sinewave= 1;
if (sinewave)
x0 = [0; 0; 0; 0];
set_param('Task_2_Simulation_LQR', 'StopTime', '30')
end
sim('Task_2_Simulation_LQR.slx');
while(~exist('simout_LQR_linear'))% ~ not operator
pause(1);
end;
%linear Model
t_LIN = simout_LQR_linear.Time;
y_LIN = simout_LQR_linear.Data(:,1:4);
u_LIN = simout_LQR_linear.Data(:,5);
r_LIN = simout_LQR_linear.Data(:,6);
%nonlinear Model
t_NL = simout_LQR_nonlinear.Time;
y_NL = simout_LQR_nonlinear.Data(:,1:4);
u_NL = simout_LQR_nonlinear.Data(:,5);
r_NL = simout_LQR_linear.Data(:,6);
%plot the states
figure;
for k = 1:2
subplot(3,1,k);
hold on;
grid on;
if(sinewave == 1 && k==1)
plot(t_LIN, r_LIN, 'g', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
if(runLinear == 1)
plot(t_LIN, y_LIN(:, k), 'r', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
if(runNonlinear == 1)
plot(t_NL, y_NL(:, k), 'b', 'LineWidth', 2.0, 'DisplayName', 'nonlinear');
end;
switch k
case 1
ylabel('$s(t)$', 'interpreter', 'latex');
case 2
ylabel('$\alpha(t)$', 'interpreter', 'latex');
case 3
ylabel('$\dot{s}(t)$', 'interpreter', 'latex');
case 4
ylabel('$\dot{\alpha}(t)$', 'interpreter', 'latex');
end;
legend('-DynamicLegend', 'Location', 'Northeast');
end;
%plot the input voltage
subplot(3,1,3);
if(runLinear == 1)
plot(t_LIN, u_LIN, 'r', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
hold on;
if(runNonlinear == 1)
plot(t_NL, u_NL, 'b', 'LineWidth', 2.0, 'DisplayName', 'nonlinear');
end;
legend('-DynamicLegend', 'Location', 'best');
ylabel('$u(t)$', 'interpreter', 'latex');
xlabel('Time [s]', 'interpreter', 'latex')
grid on;
if (sinewave)
figure;
if(runLinear == 1)
plot(t_LIN, r_LIN, 'b', 'LineWidth', 2.0, 'DisplayName', 'reference');
hold on;
plot(t_LIN, simout_LQR_linear.Data(:,1), 'r', 'LineWidth', 2.0, 'DisplayName', 'Linear state');
end;
legend('-DynamicLegend', 'Location','Northeast');
ylabel('$s(t)$', 'interpreter', 'latex')
title('Sine wave tracking', 'interpreter', 'latex');
grid
figure;
if(runNonlinear == 1)
plot(t_NL, r_NL, 'b', 'LineWidth', 1.5, 'DisplayName', 'reference');
hold on;
plot(t_NL, simout_LQR_nonlinear.Data(:,1), 'r', 'LineWidth', 1.5, 'DisplayName', 'Nonlinear state');
end;
legend('-DynamicLegend', 'Location', 'Northeast');
title('Sine wave tracking', 'interpreter', 'latex');
ylabel('$s(t)$', 'interpreter', 'latex');
grid
end;
%% Error calculation
error=0;
for i=1:1:length(r_LIN)
error=error+(r_LIN(i,1)- y_LIN(i, 1))^2;
end
error=error/length(r_LIN)
\ No newline at end of file
load('MinisegPlant\Plant_Parameters_CALIBRATION.mat');
Vs = 7.2;% Max Voltage input
Ts = 0.01; %Sampling time
% Sensor parameters use by the Simulink model
encoder_counts=720; % number of counts (if using quad encoding)
RPM_MAX = 170; % spec sheet max RPM
% constants or conversion factors:
RAD2C=encoder_counts/(2*pi); % conversion from radians to counts
C2RAD=1/RAD2C; % conversion from counts to radians
C2DEG=360/encoder_counts;% conversion from counts to degrees
R2D=180/pi;% conversion from radians to degrees
D2R=1/R2D;% conversion from degrees to radians
% Wheel Info
Rw=.02; % radius of wheel in m (small wheel), under load
% combine some constants:
ES=-C2DEG*D2R;
%% MPU5060 GYroscope and Accelerometer
GyroS=1/131; %Gyro rate and number of sampling bits
GS=GyroS*D2R; % Convert from degrees to radians
close all;
load('MinisegPlant\Plant_Parameters.mat');
load('MinisegPlant\Plant_Parameters_VAR.mat');
%Simulation of the Self Balancing Robot for Stabilization
% Design of three feedback controllers:
% - LQR Design
%State-Space Model
sys = ss(A, B, C, D);
%Check Controllability and Observability
disp('Number of controllable states:');
rank(ctrb(sys)) %or rank(ctrb(A,B))
disp('Number of observable states:');
rank(obsv(sys)) %or rank(obsv(A,C))
%% Linear-Quadratic-Regulator Design
% ************** LQR **************
%Tuning Q and R
R = 0.1;
Q = blkdiag(1000,1000,10,1);
%LQR Gain
K_LQR = -lqr(A,B,Q,R); %continuous
%Closed loop state space model
sys_cl = ss(A+B*K_LQR, B, C, D); %continuous
%% LQR Simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
open('Task_2_Simulation_LQR');
%set this values either to 0 or 1
runLinear = 1;
runNonlinear = 1;
%set initial condition
x0 = [0;5*pi/180;0; 0];
x0 = [0;9*pi/180;0; 0];
%Simulation Time
set_param('Task_2_Simulation_LQR', 'StopTime', '2')
% reference sine input for position
sinewave= 1;
if (sinewave)
x0 = [0; 0; 0; 0];
set_param('Task_2_Simulation_LQR', 'StopTime', '30')
end
sim('Task_2_Simulation_LQR.slx');
while(~exist('simout_LQR_linear'))% ~ not operator
pause(1);
end;
%linear Model
t_LIN = simout_LQR_linear.Time;
y_LIN = simout_LQR_linear.Data(:,1:4);
u_LIN = simout_LQR_linear.Data(:,5);
r_LIN = simout_LQR_linear.Data(:,6);
%nonlinear Model
t_NL = simout_LQR_nonlinear.Time;
y_NL = simout_LQR_nonlinear.Data(:,1:4);
u_NL = simout_LQR_nonlinear.Data(:,5);
r_NL = simout_LQR_linear.Data(:,6);
%plot the states
figure;
for k = 1:2
subplot(3,1,k);
hold on;
grid on;
if(sinewave == 1 && k==1)
plot(t_LIN, r_LIN, 'g', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
if(runLinear == 1)
plot(t_LIN, y_LIN(:, k), 'r', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
if(runNonlinear == 1)
plot(t_NL, y_NL(:, k), 'b', 'LineWidth', 2.0, 'DisplayName', 'nonlinear');
end;
switch k
case 1
ylabel('$s(t)$', 'interpreter', 'latex');
case 2
ylabel('$\alpha(t)$', 'interpreter', 'latex');
case 3
ylabel('$\dot{s}(t)$', 'interpreter', 'latex');
case 4
ylabel('$\dot{\alpha}(t)$', 'interpreter', 'latex');
end;
legend('-DynamicLegend', 'Location', 'Northeast');
end;
%plot the input voltage
subplot(3,1,3);
if(runLinear == 1)
plot(t_LIN, u_LIN, 'r', 'LineWidth', 2.0, 'DisplayName', 'linear');
end;
hold on;
if(runNonlinear == 1)
plot(t_NL, u_NL, 'b', 'LineWidth', 2.0, 'DisplayName', 'nonlinear');
end;
legend('-DynamicLegend', 'Location', 'best');
ylabel('$u(t)$', 'interpreter', 'latex');
xlabel('Time [s]', 'interpreter', 'latex')
grid on;
if (sinewave)
figure;
if(runLinear == 1)
plot(t_LIN, r_LIN, 'b', 'LineWidth', 2.0, 'DisplayName', 'reference');
hold on;
plot(t_LIN, simout_LQR_linear.Data(:,1), 'r', 'LineWidth', 2.0, 'DisplayName', 'Linear state');
end;
legend('-DynamicLegend', 'Location','Northeast');
ylabel('$s(t)$', 'interpreter', 'latex')
title('Sine wave tracking', 'interpreter', 'latex');
grid
figure;
if(runNonlinear == 1)
plot(t_NL, r_NL, 'b', 'LineWidth', 1.5, 'DisplayName', 'reference');
hold on;
plot(t_NL, simout_LQR_nonlinear.Data(:,1), 'r', 'LineWidth', 1.5, 'DisplayName', 'Nonlinear state');
end;
legend('-DynamicLegend', 'Location', 'Northeast');
title('Sine wave tracking', 'interpreter', 'latex');
ylabel('$s(t)$', 'interpreter', 'latex');
grid
end;
%% Comparison with experiment
close all;
for i=1:length(expOut.signals.values)
if ( expOut.signals.values(i,6)~=0)
break;
end
end
ExperimentArray=expOut.signals.values(i:end,1);
ExperimentTime=expOut.time(i:end,1)-expOut.time(i);
figure;
plot(t_NL, r_NL, 'b', 'LineWidth', 1.5, 'DisplayName', 'reference');
hold on;
plot(t_NL, simout_LQR_nonlinear.Data(:,1), 'r', 'LineWidth', 1.5, 'DisplayName', 'Nonlinear state');
hold on;
plot(ExperimentTime,ExperimentArray, 'k', 'LineWidth', 1.5, 'DisplayName', 'experiment');
legend('-DynamicLegend', 'Location', 'best');
ylabel('$s(t)$', 'interpreter', 'latex')
title('Sine wave tracking', 'interpreter', 'latex');
grid
close all;
%Set the model parameters and sampling time
%Model from Mohamed Bakr and Mahan Gorji
% states_x = [x; x_dot; alpha; alpha_dot]
% y = [x; x_dot; alpha; alpha_dot]
% u [Voltage]
%**************************************************************************
% 1 Input, 4 Outputs
%**************************************************************************
%% Robot Parameters:
% Constants:
g = 9.81; % Gravity - m/s^2
% Motor parameters:
R = 4.5; % Armature resistance - Ohms
kb = 0.495; % Back EMF coeficient - V*seg/rad
kt = 0.470;% Motor torque constant - Nm/A
% Board
% Masses, lengths and inertias:
L = 11.2/100; % Length from wheel axis to center of mass - m (7.5 - 10)
l = 0/100; % Length for parallel axis theorem - m
rw = 4.32/100/2; % Radius of the whell - m
Mw = 18.3/1000; % Mass of the wheel - kg
Mb = 250.3/1000; % Mass of the board - kg 115.6 - 250.3, board + batteries
Mm = 80/1000; % Mass of the motor - kg
Ma = 30/1000; % Mass of motor fixture - kg
Mc = 12/1000; % Mass of each cable - kg
Mp = Mb + Mm + 1*Ma + Mc; % Mass of the pendulum (board + motors) - kg
Jp = Mp * L^2 + Mp * l^2; % Moment of inertia of the board - kg*m^2
Jw = 7.462e-6; % Moment of Ineria of the wheel - kg*m^2
%Wheel
mw = Mw; %mass
Jw = Jw; %moment of intertia
r = rw; %radius
%Segway (index s)
ms = Mp; %mass
Js = Jp; %moment of intertia
L = L; %length to center
%Build State Space Model
mx = 2*mw + ms + 2*Jw/(r.^2);
Jo = Js + ms*L^2;
detM = mx*Jo - (ms*L)^2;
gamma = 1*kt*kb/(R*r);
a11 = -1*kt*kb/(R*r*detM) * (Jo/r + ms*L);
a12 = 1*kt*kb/(R*r*detM) * (Jo + ms*L*r);
a21 = 1*kt*kb/(R*r*detM) * (mx + ms*L/r);
a22 = -1*kt*kb/(R*r*detM) * (mx*r + ms*L);
a31 = -ms*g*L/detM * ms*L;
a32 = ms*g*L/detM * mx;
b1 = kt/(R*r*detM) * (Jo + ms*L*r);
b2 = -1*kt/(R*r*detM) * (mx*r + ms*L);
A = [0 0 1 0; 0 0 0 1; 0 a31 a11 a12; 0 a32 a21 a22];
B = [0;0; b1; b2];
C = eye(4);
D = zeros(4,1);
%Hardware Voltage and Sampling Time
Ts = 0.01; %Sampling frequency is 100 Hz
Vs = 7.2; %Supply voltage
%%
% create Plant_Parameters.mat
save('Plant_Parameters.mat', ...
'A', 'B', 'C', 'D', 'Ts','Vs');
save('Plant_Parameters_VAR.mat', ...
'mw', 'Jw', 'r', 'ms', 'Js', 'L', 'kb', 'kt', 'R', 'g');
clear all;
load('Plant_Parameters.mat');
load('Plant_Parameters_VAR.mat');
11-11-2016
- lookup option for motor drivers to correct nonlinearities in output (common of darling type h bridges)
- encoders are hard coded to specific pins to allow direct reading of the pin port. DigitalRead() is significantly slower (20-30x) and can cause microprocessor satureation for high resolution encoders or fast motors
- PWM frequency selection is enabled on the encoder blocks
- Driver block is simplified and improved for faster execution
- Driver blocks have all been modified so a positive voltage produces a positive direction from the encoders
- SerialPlot.m is modified to scale correctly for numbers less than 1. Graphing and autoscaling has been improved.
V1.08312016.2015a
Changed SerialPlot.ma
- added automatic autoscale up to the full window witdth, then it must be manual autoscaled with the button
- removed the popup about trying to connect to the com port
- the for loop now adjust the number of samples read/displayed so it refreshes approximately 50 times a second
Removed M1V3 and MinsegMegaV3 since they are not currently made
replaces PWM44, PWM45, PWM46 Sfunctions from MinSegMega Library with the default Arduino PWM Block
removed the "signed PWM" blocks from library since they are redundant with the Motor Driver blocks (same except for a gain to voltage)
Transition from S-Funcitons to System Objects
V1.02032016.2015a
Important: The file/folder structure has changed - this is to begin support of matlab objects to replace s-functions for more universal support (mac)
currently the root folder 'RASPlib', and the and the 'sfunction_blocks' folder are are that are required. the 'blocks', 'src', and 'include' folders are in support of new implementaiton and may be deleted by the user if desired.
Users interested in the new object based version, or mac users who wish to test this functinality and report back are welcome to try the
'MinSegShield_Demo_M1V4_15a.slx' demo which uses the object based version of some of the s-functions.
Updated library and links for 2015a - oldver versions of simulink the library might have components/block move or become unorganized - it should not affect functinality but visual arrangement of the blocks might be moved
Multiple libray changes have taken place to fix bugs, functinality usability of several blocks. Apologies for a lack of a list of specific changes.
V1.01
SerialPlot.m - modified to automatically adjust the number of samples read so the refresh rate of the graph is 30hz.
V1.1
Added some missing mex files for various blocks
\ No newline at end of file