Repository navigation
Expand file tree
/
Copy pathtype2_type3.py
More file actions
43 lines (35 loc) · 1.52 KB
/
Copy pathtype2_type3.py
File metadata and controls
43 lines (35 loc) · 1.52 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
# type2_type3.py - demonstration for type2 versus type3 control comparing
# tracking and disturbance rejection for two proposed controllers
# Gunnar Ristroph, 15 January 2010
from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import * # MATLAB-like functions
from scipy import pi
integrator = tf( [0, 1], [1, 0] ) # 1/s
# Parameters defining the system
J = 1.0
b = 10.0
Kp = 110.
Ki = Kp/2.
Kii = Ki
# Plant transfer function from torque to rate
inertia = integrator*1/J
friction = b # transfer function from rate to torque
P = inertia # friction is modelled as a separate block
# Gyro transfer function from rate to rate
gyro = 1. # for now, our gyro is perfect
# Controller transfer function from rate error to torque
C_type2 = (1. + Ki*integrator)*Kp*1.5
C_type3 = (1. + Ki*integrator)*(1. + Kii*integrator)*Kp
# System Transfer Functions
# tricky because the disturbance (base motion) is coupled in by friction
closed_loop_type2 = feedback(C_type2*feedback(P,friction),gyro)
disturbance_rejection_type2 = P*friction/(1.+P*friction+P*C_type2)
closed_loop_type3 = feedback(C_type3*feedback(P,friction),gyro)
disturbance_rejection_type3 = P*friction/(1.+P*friction+P*C_type3)
# Bode plot for the system
figure(1)
bode(closed_loop_type2, logspace(0,2)*2*pi, dB=True, Hz=True) # blue
bode(closed_loop_type3, logspace(0,2)*2*pi, dB=True, Hz=True) # green
figure(2)
bode(disturbance_rejection_type2, logspace(0,2)*2*pi, Hz=True) # blue
bode(disturbance_rejection_type3, logspace(0,2)*2*pi, Hz=True) # green