51 lines
942 B
Matlab
51 lines
942 B
Matlab
% Model of self balancing robot for PID controller
|
|
% See scheme anf formulas on notebook
|
|
|
|
clear all
|
|
clc
|
|
|
|
load('params.mat')
|
|
|
|
|
|
I_wheel = M_wheel*(r^2); %placeholder
|
|
I_body = M_body*(l^2)/3; %placeholder
|
|
|
|
nums1 = -(M_body + 2*(I_wheel/r^2) + 2*M_wheel + M_body*l/r);
|
|
nums0 = -2*b/r;
|
|
num = [nums1 nums0];
|
|
|
|
dens3 = I_body * M_body + 2*I_wheel*(I_body*M_body*l^2)/(r^2) + 2*I_body*M_wheel + 2*M_wheel*M_body*l^2;
|
|
dens2 = (2*b/r)*(I_body+M_body*g);
|
|
dens1 = -( ((M_body^2)*g*l) + 2*(I_wheel*M_body*g*l)/(r^2) + 2*M_body*M_wheel*g*l );
|
|
dens0 = -2*(b*M_body*g*l) / r;
|
|
den = [dens3 dens2 dens1 dens0];
|
|
|
|
w = tf(num, den)
|
|
|
|
sys = zpk(w);
|
|
sys.DisplayFormat='roots'
|
|
|
|
%sys.Z{1}
|
|
% sys.P{1}(1)
|
|
|
|
K = sys.K
|
|
p1 = sys.P{1}(1)
|
|
p2 = sys.P{1}(3)
|
|
p3 = sys.P{1}(2)
|
|
|
|
%display('Kd must be [' + num2str(p3/K) + ', 0]');
|
|
|
|
% Scelgo Kd così
|
|
kd = -0.001
|
|
kponkd = (p1+p2);
|
|
kionkd = (p1*p2);
|
|
kp = kponkd*kd
|
|
ki = kionkd*kd
|
|
|
|
% rlocus(sys, -sys)
|
|
%{
|
|
bode(w)
|
|
figure;
|
|
nyquist(w)
|
|
%}
|