AE4610
  • Welcome
  • Experiments
  • Lab 0: MATLAB & SIMULINK
  • Lab 1: Rotary Servo Base
    • A. Integration (Week 1)
    • B. Modeling (Week 1)
    • C. Control Design (Week 2)
    • D. Controller Implementation & Evaluation (Week 3)
  • Lab 2: 3 DOF Gyroscope
    • A & B. Modeling and Control Design (Week 1)
    • C. Controller Implementation (Week 2)
  • Lab 3: Rotary Flexible Link
    • A. System Identification (Week 1)
    • B & C. Control Design and Controller Implementation (Week 2)
  • Lab 4: Rotary Inverted Pendulum
    • A & B. Modeling and Balance Control
    • C. Swing-up Control (Demo)
  • Final Project
  • Archive
    • Old Lab 1 Model Validation
    • Problem Set
    • DC Motor
    • Lab 1: Rotary Servo Base (Older Version)
      • A. Integration (Week 1)
      • B. Modelling (Week 1)
      • C. Position Control (Week 2)
    • Quadcopter
    • Gyroscope
    • Inverted Pendulum
    • 3DOF Helicopter
    • Torsional Pendulum
    • Aero LQR
      • A. System Identification
      • A. System Identification (Week 1) last check
      • B. Control Design & Implementation (Week 2)
    • Rotary Gyro
      • Rotary Gyro
    • 2 DOF AERO
      • A. System Identification
      • B. Control Design
      • C. Controller Implementation
    • Copy of Lab 4: Rotary Inverted Pendulum
      • Week 1
      • Week 2
Powered by GitBook
On this page
  • 1 Background
  • 1.1 Desired Position Control Response
  • 1.2 PD Controller Design
  • 1.3 PID Controller Design
  • 2 Pre-Lab Questions
  • ↓↓↓ In Lab Exercise ↓↓↓
  • 3.1 Step Response using PD Controller
  • 3.1.1 Simulation
  • 3.1.2 Experiment
  • 3.2 Ramp Response using PD Controller
  • 3.2.1 Simulation
  • 3.2.2 Experiment
  • 3.3 Ramp Response with No Steady-State Error
  • 4 Results for the Report

Was this helpful?

Export as PDF
  1. Archive
  2. Lab 1: Rotary Servo Base (Older Version)

C. Position Control (Week 2)

PreviousB. Modelling (Week 1)NextQuadcopter

Last updated 2 years ago

Was this helpful?

1 Background

1.1 Desired Position Control Response

The block diagram shown in Figure 16 is a general unity feedback system with a compensator (controller) C(s)C(s)C(s) and a transfer function representing the plant, P(s)P(s)P(s). The measured output, Y(s)Y(s)Y(s), is supposed to track the reference signal R(s)R(s)R(s) and the tracking has to match certain desired specifications.

The output of this system can be written as:

Y(s)=C(s)P(s)(R(s)−Y(s))(3.1)Y (s) = C(s)P(s) (R(s) - Y (s)) \qquad \qquad \qquad \tag{3.1}Y(s)=C(s)P(s)(R(s)−Y(s))(3.1)

By solving for Y(s)Y(s)Y(s), we can find the closed-loop transfer function:

Y(s)R(s)=C(s)P(s)1+C(s)P(s)(3.2)\frac{Y(s)}{R(s)} = \frac{C(s)P(s)}{1 + C(s)P(s)} \qquad \qquad \qquad \tag{3.2}R(s)Y(s)​=1+C(s)P(s)C(s)P(s)​(3.2)

Recall in the Integration laboratory experiment, the Rotary Servo Base Unit voltage-to-speed transfer function was derived. To find the voltage-to-position transfer function, we can put an integrator (1/s)(1/s)(1/s) in series with the speed transfer function (effectively integrating the speed output to get position). Then, the resulting open-loop voltage-to-load gear position transfer function becomes:

P(s)=Ks(τs+1)(3.3)P(s) = \frac{K}{s(\tau s + 1)} \qquad \qquad \qquad \tag{3.3}P(s)=s(τs+1)K​(3.3)

As you can see from this equation, the plant is a second-order system. In fact, when a second-order system is placed in series with a proportional compensator in the feedback loop as in Figure 16, the resulting closed-loop transfer function can be expressed as:

Y(s)R(s)=ωn2s2+2ζωns+ωn2(3.4)\frac{Y(s)}{R(s)} = \frac{\omega_n^2}{s^2 + 2\zeta \omega_n s + \omega_n^2} \qquad \qquad \qquad \tag{3.4}R(s)Y(s)​=s2+2ζωn​s+ωn2​ωn2​​(3.4)

where ωn\omega_nωn​ is the natural frequency and ζ\zetaζ is the damping ratio. This is called the standard second-order transfer function. Its response depends on the values of ωn\omega_nωn​ and ζ\zetaζ.

1.1.1 Peak Time and Overshoot

Consider a second-order system as shown in Equation 3.4 subject to a step input given by

R(s)=R0s(3.5)R(s) = \frac{R_0}{s} \qquad \qquad \qquad \tag{3.5}R(s)=sR0​​(3.5)

with a step amplitude of R0=1.5R_0 = 1.5R0​=1.5. The system response to this input is shown in Figure 17, where the red trace is the response (output), y(t)y(t)y(t), and the blue trace is the step input r(t)r(t)r(t).

The maximum value of the response is denoted by the variable ymaxy_{\text{max}}ymax​ and it occurs at a time tmaxt_{\text{max}}tmax​. The initial value of the response is denoted as y0y_0y0​. For a response similar to Figure 17, the percent overshoot is found using

PO=100(ymax−R0)R0(3.6)\text{PO} = \frac{100 (y_{\text{max}} - R_0)}{R_0} \qquad \qquad \qquad \tag{3.6}PO=R0​100(ymax​−R0​)​(3.6)

From the initial step time, t0t_0t0​, the time it takes for the response to reach its maximum value is

tp=tmax−t0(3.7)t_p = t_{\text{max}} - t_0 \qquad \qquad \qquad \tag{3.7}tp​=tmax​−t0​(3.7)

This is called the peak time of the system.

In a second-order system, the amount of overshoot depends solely on the damping ratio parameter and it can be calculated using the equation

PO=100e(−πζ1−ζ2)(3.8)\text{PO} = 100 e^{\left(-\frac{\pi \zeta}{\sqrt{1 - \zeta^2}}\right)} \qquad \qquad \qquad \tag{3.8}PO=100e(−1−ζ2​πζ​)(3.8)

The peak time depends on both the damping ratio and natural frequency of the system and it can be derived as:

tp=πωn1−ζ2(3.9)t_p = \frac{\pi}{\omega_n \sqrt{1 - \zeta^2}} \qquad \qquad \qquad \tag{3.9}tp​=ωn​1−ζ2​π​(3.9)

Generally speaking, the damping ratio affects the shape of the response while the natural frequency affects the speed of the response.

1.1.2 Steady State Error

Steady-state error is illustrated in the ramp response given in Figure 18 and is denoted by the variable esse_{\text{ss}}ess​. It is the difference between the reference input and output signals after the system response has settled. Thus, for a time ttt when the system is in steady-state, the steady-state error equals

ess=rss(t)−yss(t)(3.10)e_{\text{ss}} = r_{\text{ss}}(t) - y_{\text{ss}}(t) \qquad \qquad \qquad \tag{3.10}ess​=rss​(t)−yss​(t)(3.10)

where rss(t)r_{\text{ss}}(t)rss​(t) is the value of the steady-state input and yss(t)y_{\text{ss}}(t)yss​(t) is the steady-state value of the output.

We can find the error transfer function E(s)E(s)E(s) in Figure 16 in terms of the reference R(s)R(s)R(s), the plant P(s)P(s)P(s), and the compensator C(s)C(s)C(s). The Laplace transform of the error is

E(s)=R(s)−Y(s)(3.11)E(s) = R(s) - Y (s) \qquad \qquad \qquad \tag{3.11}E(s)=R(s)−Y(s)(3.11)

Solving for Y(s)Y (s)Y(s) from Equation 3.3 and substituting it in Equation 3.11 yields

E(s)=R(s)1+C(s)P(s)(3.12)E(s) = \frac{R(s)}{1 + C(s)P(s)} \qquad \qquad \qquad \tag{3.12}E(s)=1+C(s)P(s)R(s)​(3.12)

We can find the steady-state error of this system using the final-value theorem:

ess=lim⁡s→0sE(s)(3.13)e_{\text{ss}} = \lim_{s \to 0} sE(s) \qquad \qquad \qquad \tag{3.13}ess​=s→0lim​sE(s)(3.13)

In this equation, we need to substitute the transfer function for E(s)E(s)E(s) from Equation 3.12. The E(s)E(s)E(s) transfer function requires, R(s)R(s)R(s), C(s)C(s)C(s) and P(s)P(s)P(s). For simplicity, let C(s)=1C(s) = 1C(s)=1 as a compensator. The P(s)P(s)P(s) and R(s)R(s)R(s) were given by equations Equation 3.3 and Equation 3.5, respectively. Then, the error becomes:

E(s)=R(s)s(1+Ks(τs+1))(3.14)E(s) = \frac{R(s)}{s\left(1 + \frac{K}{s(\tau s + 1)}\right)} \qquad \qquad \qquad \tag{3.14}E(s)=s(1+s(τs+1)K​)R(s)​(3.14)

Applying the final-value theorem gives

ess=R0(lim⁡s→0(τs+1)sτs2+s+K)(3.15)e_{\text{ss}} = R_0\left(\lim_{s \to 0} \frac{(\tau s+1)s}{\tau s^2 + s + K}\right) \qquad \qquad \qquad \tag{3.15}ess​=R0​(s→0lim​τs2+s+K(τs+1)s​)(3.15)

When evaluated, the resulting steady-state error due to a step response is

ess=0(3.16)e_{\text{ss}} = 0 \qquad \qquad \qquad \tag{3.16}ess​=0(3.16)

Based on this zero steady-state error for a step input, we can conclude that the Rotary Servo Base Unit is a Type 1 system.

1.1.3 Time-Domain Control Specifications

The desired time-domain specifications for controlling the position of the Rotary Servo Base Unit load shaft are:

ess=0(3.17)e_{\text{ss}} = 0 \qquad \qquad \qquad \tag{3.17}ess​=0(3.17)
tp=0.20 s(3.18)t_p = 0.20 ~\text{s} \qquad \qquad \qquad \tag{3.18}tp​=0.20 s(3.18)

and

PO=5.0%(3.19)\text{PO} = 5.0 \% \qquad \qquad \qquad \tag{3.19}PO=5.0%(3.19)

Thus, when tracking the load shaft reference, the transient response should have a peak time less than or equal to 0.20 seconds, an overshoot less than or equal to 5%, and the steady-state response should have no error.

1.2 PD Controller Design

1.2.1 Closed Loop Transfer Function

The proportional-derivative (PD) compensator to control the position of the Rotary Servo Base Unit has the following structure

Vm(t)=kp(θd(t)−θl(t))−kd(ddtθl(t))(3.20)V_m(t) = k_p(\theta_d(t) - \theta_l(t)) - k_d \left(\frac{d}{dt} \theta_l(t)\right) \qquad \qquad \qquad \tag{3.20}Vm​(t)=kp​(θd​(t)−θl​(t))−kd​(dtd​θl​(t))(3.20)

where kpk_pkp​ is the proportional control gain, kdk_dkd​ is the derivative control gain, θd(t)\theta_d(t)θd​(t) is the setpoint or reference load shaft angle, θl(t)\theta_l(t)θl​(t) is the measured load shaft angle, and Vm(t)V_m(t)Vm​(t) is the Rotary Servo Base Unit motor input voltage. The block diagram of the PD control is given in Figure 19.

This is a variation of the classic PD control where the D-term is in the feedback path as opposed to in the forward path.

We need to find the closed-loop transfer function Θl(s)/Θd(s)\Theta_l(s)/\Theta_d(s)Θl​(s)/Θd​(s) for the closed-loop position control of the Rotary Servo Base Unit. Taking the Laplace transform of Equation 3.20 gives

Vm(s)=kp(Θd(s)−Θl(s))−kdsΘl(s)(3.21)V_m(s) = k_p(\Theta_d(s) - \Theta_l(s)) - k_d s \Theta_l(s) \qquad \qquad \qquad \tag{3.21}Vm​(s)=kp​(Θd​(s)−Θl​(s))−kd​sΘl​(s)(3.21)

From the Plant block in Figure 19 and Equation 3.3, we can write

Θl(s)Vm(s)=Ks(τs+1)(3.22)\frac{\Theta_l(s)}{V_m(s)} = \frac{K}{s(\tau s + 1)} \qquad \qquad \qquad \tag{3.22}Vm​(s)Θl​(s)​=s(τs+1)K​(3.22)

Substituting Equation 3.21 into Equation 3.22 and solving for Θl(s)/Θd(s)\Theta_l(s)/\Theta_d(s)Θl​(s)/Θd​(s) gives the Rotary Servo Base Unit position closed-loop transfer function as:

Θl(s)Vm(s)=Kkpτs2+(1+Kkd)s+Kkp(3.23)\frac{\Theta_l(s)}{V_m(s)} = \frac{K k_p}{\tau s^2 + (1 + K k_d)s + K k_p} \qquad (3.23)Vm​(s)Θl​(s)​=τs2+(1+Kkd​)s+Kkp​Kkp​​(3.23)

1.2.2 Controller Gain Limits

In control design, a factor to be considered is saturation. This is a nonlinear element and is represented by a saturation block as shown in Figure 20. In a system like the Rotary Servo Base Unit, the computer calculates a numeric control voltage value. This value is then converted into a voltage, Vdac(t)V_{\text{dac}}(t)Vdac​(t), by the digital-to-analog converter of the data-acquisition device in the computer. The voltage is then amplified by a power amplifier by a factor of KaK_aKa​. If the amplified voltage, Vamp(t)V_{\text{amp}}(t)Vamp​(t), is greater than the maximum output voltage of the amplifier or the input voltage limits of the motor (whichever is smaller), then it is saturated (limited) at VmaxV_{\text{max}}Vmax​. Therefore, the input voltage Vm(t)V_m(t)Vm​(t) is the effective voltage being applied to the Rotary Servo Base Unit motor.

The limitations of the actuator must be taken into account when designing a controller. For instance, the voltage entering the Rotary Servo Base Unit motor should never exceed

Vmax=10.0V(3.24)V_{\text{max}} = 10.0 V \qquad \qquad \qquad \tag{3.24}Vmax​=10.0V(3.24)

1.2.3 Ramp Steady State Error Using PD Control

From our previous steady-state analysis, we found that the closed-loop Rotary Servo Base Unit system is a Type 1 system. In this section, we will investigate the steady-state error due to a ramp input when using PD controller.

Given the following ramp setpoint (input)

R(s)=R0s2(3.25)R(s) = \frac{R_0}{s^2} \qquad \qquad \qquad \tag{3.25} R(s)=s2R0​​(3.25)

we can find the error transfer function by substituting the Rotary Servo Base Unit closed-loop transfer function in Equation 3.23 into the formula given in Equation 3.11. Using the variables of the Rotary Servo Base Unit, this formula can be rewritten as E(s)=Θd(s)−Θl(s)E(s) = \Theta_d(s) - \Theta_l(s)E(s)=Θd​(s)−Θl​(s). After rearranging the terms we find:

E(s)=Θd(s)s(τs+1+Kkd)τs2+s+Kkp+Kkds(3.26)E(s) = \frac{\Theta_d(s) s(\tau s + 1 + K k_d)}{\tau s^2 + s + K k_p + K k_ds } \qquad \qquad \qquad \tag{3.26}E(s)=τs2+s+Kkp​+Kkd​sΘd​(s)s(τs+1+Kkd​)​(3.26)

Substituting the input ramp transfer function Equation 3.25 into the Θd(s)\Theta_d(s)Θd​(s) variable gives

E(s)=R0(τs+1+Kkd)s(τs2+s+Kkp+Kkds)(3.27)E(s) = \frac{R_0(\tau s + 1 + K k_d)}{s(\tau s^2 + s + K k_p + K k_ds)} \qquad \qquad \qquad \tag{3.27}E(s)=s(τs2+s+Kkp​+Kkd​s)R0​(τs+1+Kkd​)​(3.27)

1.3 PID Controller Design

Adding integral control can help eliminate steady-state error. The proportional-integral-derivative (PID) algorithm to control the position of the Rotary Servo Base Unit is shown in Figure 1.6. The motor voltage will be generated by the PID according to:

Vm(t)=kp(θd(t)−θl(t))+ki∫(θd(t)−θl(t))dt−kd(ddtθl(t))(3.28)V_m(t) = k_p(\theta_d(t) - \theta_l(t)) +k_i \int (\theta_d(t) - \theta_l(t))dt - k_d \left(\frac{d}{dt} \theta_l(t)\right) \qquad \qquad \qquad \tag{3.28}Vm​(t)=kp​(θd​(t)−θl​(t))+ki​∫(θd​(t)−θl​(t))dt−kd​(dtd​θl​(t))(3.28)

where kpk_pkp​ is the proportional control gain, kik_iki​ is the integral gain, kdk_dkd​ is the derivative control gain, θd(t)\theta_d(t)θd​(t) is the setpoint or reference load shaft angle, θl(t)\theta_l(t)θl​(t) is the measured load shaft angle, and Vm(t)V_m(t)Vm​(t) is the Rotary Servo Base Unit motor input voltage.

This is a variation of the standard PID control with the D-term in the feedback path as opposed to the feedforward path.

We need to find the closed-loop transfer function Θl(s)/Θd(s)\Theta_l(s)/\Theta_d(s)Θl​(s)/Θd​(s) for the closed-loop position control of the Rotary Servo Base Unit. Taking the Laplace transform of Equation 3.28 gives

Vm(s)=(kp+kis)(Θd(s)−Θl(s))−kdsΘl(s)(3.29)V_m(s) = \left(k_p + \frac{k_i}{s}\right)(\Theta_d(s) - \Theta_l(s)) - k_d s \Theta_l(s) \qquad \qquad \qquad \tag{3.29}Vm​(s)=(kp​+ski​​)(Θd​(s)−Θl​(s))−kd​sΘl​(s)(3.29)

From the Plant block in Figure 21 and Equation 3.3, we can write

Θl(s)Vm(s)=K(τs+1)s(3.30)\frac{\Theta_l(s)}{V_m(s)} = \frac{K}{(\tau s + 1)s} \qquad \qquad \qquad \tag{3.30}Vm​(s)Θl​(s)​=(τs+1)sK​(3.30)

Substituting Equation 3.29 into Equation 3.30 and solving for Θl(s)/Θd(s)\Theta_l(s)/\Theta_d(s)Θl​(s)/Θd​(s) gives the Rotary Servo Base Unit position closed-loop transfer function as:

Θl(s)Θd(s)=K(kps+ki)τs3+(1+Kkd)s2+Kkps+Kki(3.31)\frac{\Theta_l(s)}{\Theta_d(s)} = \frac{K (k_p s + k_i)}{\tau s^3 + (1 + K k_d)s^2 + K k_p s + K k_i} \qquad \qquad \qquad \tag{3.31}Θd​(s)Θl​(s)​=τs3+(1+Kkd​)s2+Kkp​s+Kki​K(kp​s+ki​)​(3.31)

1.3.1 Ramp Steady-State Error using PID Controller

To find the steady-state error of the Rotary Servo Base Unit for a ramp input under the control of the PID substitute the closed-loop transfer function from Equation 3.31 into Equation 3.11

E(s)=Θd(s)s2(τs+1+Kkd)τs3+s2+Kkps+Kki+Kkds2(3.32)E(s) = \frac{\Theta_d(s) s^2(\tau s + 1 + K k_d)}{\tau s^3 + s^2 + K k_p s + K k_i + K k_d s^2} \qquad \qquad \qquad \tag{3.32}E(s)=τs3+s2+Kkp​s+Kki​+Kkd​s2Θd​(s)s2(τs+1+Kkd​)​(3.32)

Then, substituting the reference ramp transfer function Equation 3.25 into the Θd(s)\Theta_d(s)Θd​(s) variable gives

E(s)=R0(τs+1+Kkd)s(τs3+s2+Kkps+Kki+Kkds2)(3.33)E(s) = \frac{R_0(\tau s + 1 + K k_d)}{s(\tau s^3 + s^2 + K k_p s + K k_i + K k_d s^2)} \qquad \qquad \qquad \tag{3.33}E(s)=s(τs3+s2+Kkp​s+Kki​+Kkd​s2)R0​(τs+1+Kkd​)​(3.33)

1.3.2 Integral Gain Design

It takes a certain amount of time for the output response to track the ramp reference with zero steady-state error. This is called the settling time and it is determined by the value used for the integral gain.

In steady-state, the ramp response error is constant. Therefore, to design an integral gain the velocity compensation (the V signal) can be neglected. Thus, we have a PI controller left as:

Vm(t)=kp(θd(t)−θl(t))+ki∫(θd(t)−θl(t))dt(3.34)V_m(t) = k_p(\theta_d(t) - \theta_l(t)) +k_i \int (\theta_d(t) - \theta_l(t))dt \qquad \qquad \qquad \tag{3.34}Vm​(t)=kp​(θd​(t)−θl​(t))+ki​∫(θd​(t)−θl​(t))dt(3.34)

When in steady-state, the expression can be simplified to

Vm(t)=kpess+ki∫0tiessdt(3.35)V_m(t) = k_p e_{\text{ss}} + k_i \int_0^{t_i} e_{\text{ss}} dt \qquad \qquad \qquad \tag{3.35}Vm​(t)=kp​ess​+ki​∫0ti​​ess​dt(3.35)

where the variable tit_iti​ is the integration time.

2 Pre-Lab Questions

Do problems 1 - 7

  1. In the PD controlled system, for a reference step of π/4\pi/4π/4 (i.e. 45 deg step) starting from θl(t)=0\theta_l(t) = 0θl​(t)=0 position, calculate the maximum proportional gain that would lead to providing the maximum voltage to the motor. Ignore the derivative control (kd=0k_d = 0kd​=0). Can the desired specifications be obtained based on this maximum available gain and what you calculated in Question 4? (Need answer from Q4)

  2. For the PD controlled closed-loop system, find the steady-state error and evaluate it numerically given a ramp with a slope of R0=3.36R_0 = 3.36R0​=3.36 rad/s. Use the control gains found in Question 4. (Need answer from Q4)

  3. What should the integral gain kik_iki​ be so that when the Rotary Servo Base Unit is supplied with the maximum voltage of Vmax=10V_{\text{max}} = 10Vmax​=10V it can eliminate the steady-state error calculated in Question 6 in 1 second? (Need answers from Q4 and Q6) Hint: Start from Equation 3.35 and use ti=1t_i = 1ti​=1, Vm(t)=10V_m(t) = 10Vm​(t)=10, the kpk_pkp​ you found in Question 4 and esse_{\text{ss}}ess​ found in Question 6. Remember that esse_{\text{ss}}ess​ is constant.

↓↓↓ In Lab Exercise ↓↓↓

The main goal of this laboratory is to explore position control of the Rotary Servo Base Unit load shaft using PD and PID controllers. In this laboratory, you will conduct the following experiments:

  1. Step response with PD controller,

  2. Ramp response with PD controller, and

  3. Ramp response with PID controller.

In each experiment, you will first simulate the closed-loop response of the system. Then, you will implement the controller using the Rotary Servo Base Unit hardware and software to compare the real response to the simulated one.

Position Control MATLAB/SIMULINK Files

3.1 Step Response using PD Controller

3.1.1 Simulation

First, you will simulate the closed-loop response of the Rotary Servo Base Unit with a PD controller to step input. Our goals are to confirm that the desired response specifications in an ideal situation are satisfied and to verify that the motor is not saturated. Then, you will explore the effect of using a high-pass filter, instead of a direct derivative, to create the velocity signal in the controller.

Simulation Setup

  1. Open the setup_servo_pos_cntrl.m file to open the setup script for the position control Simulink models.

  2. Replace the default model parameters (K and tau) with the values calculated from the Modelling section in the setup script.

  3. Run the script, which will generate the model parameters, specifications, and default PID gains. Note: The calculated PID gains are all set to zero by default and they need to be changed over the course of the experiment.

Simulation: Closed-loop Response with PD Controller

  1. Enter the proportional and derivative control gains found in Pre-Lab Question 4 in the Matlab command window as kp and kd.

  2. Set the integral gain to 0 in the Matlab window, denoted as ki.

  3. To generate a step reference, ensure the Signal Generator is set to the following:

    • Signal type = square

    • Amplitude = 1

    • Frequency = 0.4 Hz

  4. In the Simulink diagram, set the Amplitude (rad) gain block to 0.4 (rad) to generate a step with an amplitude of 45.8 degrees (i.e. square wave goes between ±0.4\pm0.4±0.4 which results in a step amplitude of 0.8 rad).

  5. Set the Manual Switch such that the velocity of the motor load shaft is fed back directly.

  6. Open the servo position Position (rad) scope and the motor input voltage Vm (V) scope.

  7. Start the simulation. By default, the simulation runs for 5 seconds. The scopes should be displaying responses similar to Figure 23a and Figure 23b. Note that in the theta_l (rad) scope, the yellow trace is the setpoint position while the purple trace is the simulated position (generated by the Rotary Servo Base Unit Model block). This simulation is called the Ideal PD response as it uses the PD compensator with the derivative block.

  8. After each simulation run, each scope automatically saves its response to a variable in the MATLAB workspace. That is, the Position (rad) scope saves its response to the variable called data_pos and the Vm (V) scope saves its data to the data_vm variable. The data_pos variable has the following structure: data_pos(:,1) is the time vector, data_pos(:,2) is the setpoint, and data_pos(:,3) is the simulated angle. For the data_vm variable, data_vm(:,1) is the time and data_vm(:,2) is the simulated input voltage.

  9. Save Vm and data_pos data and name it suitably (e.g. posnctrl_section#_Group#_step). Result: Generate a MATLAB figure showing the Ideal PD position response and the ideal input voltage.

Simulation: Using a Filtered Derivative

When implementing a controller on actual hardware, it is generally not advised to take the direct derivative of a measured signal. Any noise or spikes in the signal becomes amplified and gets multiplied by a gain and fed into the motor which may lead to damage. To remove any high-frequency noise components in the velocity signal, a low-pass filter is placed in series with the derivative, i.e. taking the high-pass filter of the measured signal. However, as with a controller, the filter must also be tuned properly. In addition, the filter has some adverse effects (ex. filter could add some delays to the system).

  1. Set the Manual Switch block to the down position to enable the derivative and filter.

  2. Start the simulation. The response in the scopes should still be similar to Figure 23a and Figure 23b. This simulation is called the Filtered PD response as it uses the PD controller with the high-pass filter block. Result: Generate a MATLAB figure showing the Filtered PD position and input voltage responses.

3.1.2 Experiment

In this experiment, we will control the angular position of the Rotary Servo Base Unit load shaft, i.e. the disc load, using the PD controller. Measurements will then be taken to ensure that the specifications are satisfied.

Experimental Setup

  1. From the extracted files, open q_servo_pos_cntrl Simulink file.

  2. Configure DAQ: Double-click on the HIL Initialize block in the Rotary Servo Interface subsystem (which is located inside the Rotary Servo Interface - Position subsystem) Simulink diagram and ensure it is configured for the DAQ device that is installed in your system (e.g. Q2-USB).

  3. Open setup_servo_pos_cntrl.m file and run the script.

Experiment

  1. Enter the proportional and derivative control gains found in Pre-Lab Question 4.

  2. Set Signal Type in the Signal Generator block to square to generate a step reference.

  3. Set the Amplitude (rad) gain block to 0.4 to generate a step with an amplitude of 45.8 degrees.

  4. Open the load shaft position scope, Position (rad), and the motor input voltage scope, Vm (V).

  5. When a suitable response is obtained, click on the Stop button in the SIMULINK diagram toolbar to stop running the code. As in the s_servo_pos_cntrl SIMULINK diagram, when the controller is stopped each scope automatically saves its response to a variable in the MATLAB workspace. Thus the theta_l (rad) scope saves its response to the data_pos variable and the Vm (V) scope saves its data to the data_vm variable.

  6. Save the data in the MATLAB workspace with a suitable name. Result: Generate a MATLAB figure showing the PD position response and its input voltage.

  7. Click the Stop button on the SIMULINK diagram toolbar to stop the experiment.

3.2 Ramp Response using PD Controller

3.2.1 Simulation

In this simulation, the goal is to verify that the system with the PD controller can meet the zero steady-state error specification without saturating the motor.

  1. Clear the workspace and run the setup_servo_pos_cntrl.m script in MATLAB.

  2. Enter the proportional and derivative control gains found in Pre-Lab Question 4 and set the integral gain to 0.

  3. Set the Signal Generator parameters to the following to generate a triangular reference (which corresponds to a ramp input):

    • Signal Type = triangle

    • Amplitude = 1

    • Frequency = 0.8 Hz

  4. Setting the frequency to 0.8 Hz will generate an increasing and decreasing ramp signal with the same slope used in the Pre-Lab Question 6. The slope is calculated from the Triangular Waveform amplitude, Amp, and frequency, f, using the expression: R0=4 Amp fR_0 = 4~\text{Amp~f}R0​=4 Amp f

  5. In the SIMULINK diagram, set the Amplitude (rad) gain block to π/3\pi/3π/3.

  6. Inside the PID Control subsystem, set the Manual Switch to the down position so that the High-Pass Filter block is used.

  7. Open the load shaft position scope, Position (rad), and the motor input voltage scope, Vm (V).

  8. Start the simulation. The scopes should display responses similar to Figure 26a and Figure 26b.

  9. Save the data in the workspace using a suitable name. Result: Generate a MATLAB figure showing the Ramp PD position response and its corresponding input voltage trace.

  10. Measure the steady-state error. Result: Compare the simulation measurement with the steady-state error calculated in Pre-Lab Question 6.

3.2.2 Experiment

In this experiment, we will control the angular position of the Rotary Servo Base Unit load shaft, i.e. the disc load, using a PD controller. The goal is to examine how well the system can track a triangular (ramp) position input. Measurements will then be taken to ensure that the specifications are satisfied.

  1. Clear the workspace and run the setup_servo_pos_cntrl.m script in MATLAB.

  2. Enter the proportional and derivative control gains found in Pre-Lab Question 4.

  3. Set the Signal Generator parameters to the following to generate a triangular reference (i.e. ramp reference):

    • Signal Type = triangle

    • Amplitude = 1

    • Frequency = 0.8 Hz

  4. In the Simulink diagram, set the Amplitude (rad) gain block to π/3\pi/3π/3.

  5. Open the load shaft position scope, Position (rad), and the motor input voltage scope, Vm (V).

  6. Save the data in the workspace using a suitable name. Result: Generate a MATLAB figure showing the Ramp PD position response and its corresponding input voltage trace.

  7. Measure the steady-state error. Result: Compare it with the steady-state error calculated in Pre-Lab Question 6.

3.3 Ramp Response with No Steady-State Error

Design an experiment to see if the steady-state error can be eliminated when tracking a ramp input. First, simulate the response, then implement it using the Rotary Servo Base Unit system.

  1. List the independent and dependent variables of your proposed controller. Explain their relationship.

  2. Your proposed controller, like the PD compensator, is a model-based controller. This means that the control gains generated are based on a mathematical representation of the system. Given this, list the assumptions you are making in this control design. State the reasons for your assumptions.

  3. For each case, generate a Matlab figure showing the position response of the system and its corresponding input voltage.

  4. In each case, measure the steady-state error.

4 Results for the Report

  1. Pre-lab calculations.

  2. Model parameters (KKK and τ\tauτ) and controller gains (kpk_pkp​, kdk_dkd​ and kik_iki​) implemented during the In-Lab Simulations and Experiments.

Calculate the maximum overshoot of the response (in radians) given a step setpoint of 45 deg and the overshoot specification given in . Hint: By substituting ymax=θ(tp)y_{\text{max}} = \theta(t_p)ymax​=θ(tp​) and step setpoint R0=θd(t)R_0 = \theta_d(t)R0​=θd​(t) into Equation 3.6, we can obtain θ(tp)=θd(t)(1+PO100)\theta(t_p) = \theta_d(t) \left(1 + \frac{\text{PO}}{100} \right)θ(tp​)=θd​(t)(1+100PO​). Recall that the desired response specifications include 5% overshoot.

The Rotary Servo Base Unit closed-loop transfer function was derived in Equation 3.23 in . Find the control gains kpk_pkp​ and kdk_dkd​ in terms of ωn\omega_nωn​ and ζ\zetaζ. Hint: Remember the standard second-order system equation.

Calculate the minimum damping ratio and natural frequency required to meet the specifications given in .

Based on the nominal Rotary Servo Base Unit model parameters, KKK and τ\tauτ, found in the Modeling laboratory experiment, calculate the control gains needed to satisfy the time-domain response requirements given in . (Use K=1.53K = 1.53K=1.53 rad/(Vs) and τ=0.0217\tau = 0.0217τ=0.0217) (Need answers from Q2 and Q3)

The s_servo_pos_cntrl SIMULINK diagram shown in Figure 22 will be used to simulate the closed-loop position control response with the PD and PID controllers. The Rotary Servo Base Unit Model uses a Transfer Fcn block from the SIMULINK library. The PID Control subsystem contains the PID controller detailed in . When the integral gain is set to zero, it essentially becomes a PD controller.

Download the and extract the files to the desktop.

Measure the steady-state error, the percent overshoot and the peak time of the simulated response. Result: Does the response satisfy the specifications given in ? Hint: You can use the Cursor Measurements tool in the Simulink scopes or save the data and use MATLAB function : stepinfo().

Measure the steady-state error, peak time, and percent overshoot. Result: Are the specifications still satisfied without saturating the actuator? Recall that the peak time and percent overshoot should not exceed the values given in . Discuss the changes from the ideal response. Hint: The difference in the response is minor. Make sure you use Cursor Measurements tool in the Simulink scope to take precise measurements.

The q_servo_pos_cntrl SIMULINK diagram shown in Figure 24 is used to implement the position control experiments. The Rotary Servo Interface - Position subsystem contains QUARC blocks that interface with the DC motor and sensors of the Rotary Servo Base Unit system, as discussed in the Integration laboratory experiment. The PID Control subsystem implements the PID controller detailed in , except a low-pass filter is placed in series with the derivative to remove the noise.

Build the model by clicking on the down arrow on Monitor & Tune under the Hardware tab and then select Build for monitoring . This compiles the Simulink diagram.

Click Connect button under Monitor & Tune and then run SIMULINK by clicking Start to begin running the controller. The scopes should display responses similar to Figure 25a and Figure 25b.

Measure the steady-state error, the percent overshoot, and the peak time of the Rotary Servo Base Unit load gear. Result: Does the response satisfy the specifications given in ?

Similar to the Step Response experiment in , in this experiment you need to use the s_servo_pos_cntrl Simulink diagram shown in Figure 22 in again.

As in the Step Response experiment in , in this experiment, you also need to use the q_servo_pos_cntrl SIMULINK diagram shown in Figure 24 to implement the position control experiments.

Build the model by clicking on the down arrow on Monitor & Tune under the Hardware tab and then select Build for monitoring .

Click Connect button under Monitor & Tune and then run SIMULINK by clicking Start to begin running the controller. The scopes should display responses similar to Figure 27a and Figure 27b.

How can the PD controller be modified to eliminate the steady-state error in the ramp response? State your hypothesis and describe the anticipated cause-and-effect leading to the expected result. Hint: Look through .

Give a brief, general overview of the steps involved in your experimental procedure for two cases: (1) Simulation, and (2) Experimental implementation. Note: In case of time constraints, follow the same steps for Simulation as per and Experiment as per , except the value of ki (i.e., ki ≠\ne= 0).

For each case comment on whether the steady-state specification given in was satisfied without saturating the actuator.

Response plots from In-Lab Simulations and Experiments (: , ; : Step 8; : Step 9; : Step 8; : Step 5).

Corresponding measured peak time, percent overshoot and steady-state error for the response plots mentioned above (: , ; : Step 9; : Step 10; : Step 9; : Step 6). Note: Peak time and percent overshoot are required only for step response simulation and experiment, not ramp response.

Answer all the questions asked in all the In-Lab simulations and experiments (: , ; : Step 9; : Step 10; : Step 9; : Steps 1, 2, 3 and 7).

Section 1.1.3
Section 1.2.1
Section 1.1.3
Section 1.1.3
Section 1.3
PositionControl_files.zip
Section 1.1.3
Section 1.1.3
Section 1.3
Section 1.1.3
Section 3.1
Section 3.1.1
Section 3.1
Section 1
Section 3.2.1
Section 3.2.2
Section 1.1.3
Section 3.1.1
Step 9
Step 2
Section 3.1.2
Section 3.2.1
Section 3.2.2
Section 3.3
Section 3.1.1
Step 10
Step 3
Section 3.1.2
Section 3.2.1
Section 3.2.2
Section 3.3
Section 3.1.1
Step 10
Step 3
Section 3.1.2
Section 3.2.1
Section 3.2.2
Section 3.3
57KB
PositionControl_files.zip
archive
Figure 16. Unity feedback system
Figure 17. Standard second-order step response.
Figure 18. Steady-state error in ramp response.
Figure 19. Block diagram of Rotary Servo Base Unit PD position control.
Figure 20. Actuator saturation.
Figure 21. Block diagram of PID Rotary Servo Base Unit position control.
Figure 22. Simulink model used to simulate the Rotary Servo Base Unit closed-loop position response.
Figure 23. Simulated PD control response using direct derivative
Figure 24. Simulink/QUARC model used with to run the PID position controller on the Rotary Servo Base Unit.
Figure 25. PD control response on Rotary Servo Base Unit
Figure 26. Simulated ramp response using PD control
Figure 27. Ramp response using PD control on Rotary Servo Base Unit