MODEL 4.

POSITION CONTROL FOR DC SERVO MOTORS

4.1 Contents
✓ Empirical modeling of a single axis servo system.
✓ Using Matlab/Simulink to illustrate the system’s performance.
✓ Programming using STM32F103 to read encoder signals and implement
the PI/PD controller.
✓ Verify position control of a DC servo motor in a real model.
4.2. One axis servo system using dc servo motors
4.2.1 DC motor dynamics

𝐽𝑀

𝑇𝑀

Fig. 4.1: The equivalent circuit of a DC servo motor

di
The electrical system: u (t ) = Ri + L +e (4.1)
dt
where, e = K e
d
The mechanical system: J M + b = TM (4.2)
dt
where, TM = K mi
where,
u: voltage input [V]; : angular velocity [rad/s]
R: armature resistance []; L: armature inductance [H]
Ke: the back emf constant [V/rad/s]; Km: torque constant [Nm/A]
JM: inertial moment of motor shaft [kg.m2]
TM: torque of motor [Nm]
b: viscous damping [Nm.s]
The block diagram of a DC servo motor:

107
 TL

u 1 i TM 1 
Km
Ls + R Ls + R

Ke

Fig. 4.2: The block diagram of a DC servo motor

The transfer function of a motor is obtained:
 ( s) Km
G( s) = = (4.3)
U (s) ( Ls + R)(Js+ b) + K m K e

Km
 ( s) Rb K
G ( s) =  = (4.4)
U (s) L  J  ( e s + 1)( m s + 1)
 s + 1 s + 1
R  b 
L
where, e = : electrical time constant (s)
R
J
m = : mechanical time constant (s)
b
normally,  m e

Then we can approximate eq. (4) by a first order transfer function

 ( s) K
G( s) =  (4.5)
U (s)  ms +1
4.2.2. One axis servo system using DC servo motor

Fig. 4.3: The model of one-axis servo systems

4.2.3 The general diagram

108
 G p (s)
yd (mm)
pwm  y (mm)
Gc1 ( s ) Gc 2 ( s ) GH ( s ) GM ( s ) GL ( s )
 0 − 100%

velocity loop

position loop

Fig. 4.4: The general diagram of the controlled systems

where, Gc1: P/PD controller; Gc2: PI controller
GH: The transfer function of the H-bridge
GM: The transfer function of the motor
GL: The transfer function of the load.
4.3. Quadrature encoder
4.3.1 Incremental encoder
Rotary encoder is a sensor attached to a rotating object (such as a shaft or motor) to
measure rotation. By measuring rotation, we can determine any displacement, velocity,
acceleration, or the angle of a rotating object.

Fig. 4.5: Appearance of a DC servo motor and encoder output signals

➢ 1X Encoding: using 1 external interrupt for channel A or B

Fig. 4.6: Pulse diagram of 1x encoding

➢ 2X Encoding: using 1 external interrupt for channel A or B

109
 Fig. 4.7: Pulse diagram of 2x encoding
➢ 4X Encoding: using 2 external interrupts for 2 channels A and B

Fig. 4.8: Pulse diagram of 4x encoding

From the above pulses, we can draw out a table that describes the transition states of
the two channels A and B:
Table 4.2: The transition states of channel A and B
State Clockwise-Transition Counter-Clockwise
(CW) Transition (CCW)
0, 0 (0, 1) to (0, 0) (1, 0) to (0, 0)
1, 0 (0, 0) to (1, 0) (1, 1) to (1, 0)
1, 1 (1, 0) to (1, 1) (0, 1) to (1, 1)
0, 1 (1, 1) to (0, 1) (0, 0) to (0, 1)
The state transition diagram:

110
 Fig. 4.9: The state transition diagram

The figure below (Fig. 4.10) is the code example of 4x encoder reading for channel
A. To complete reading encoder in 4x mode, we have to add another external interrupt
for channel B, the code for channel B is almost similar.
Note that: - Channel A is connected to GPIOB, PIN 4
- Channel B is connected to GPIOB, PIN 6
4.3.2. Calculating velocity/position
We use the following equation for estimating velocity of the motor:
60  CountValue
= (RPM) (4.6)
T  MaxCnt
where, CountValue is the number of pulses counted in T (s). T is also timer interrupt
for reading encoder, and in this experiment, T is configured as 0.005 (s). MaxCnt equals
to encoder resolution multiplied by its mode (x1, x2 or x4).

111
 Fig. 4.10: Code example of channel A encoding in 4x mode
From Fig. (4.10), we can see that PosCnt is the number of rounds of the motor at the
moment. CountValue is the number of pulses of the present rotation, CountValue will
be reset when the rotation is completed. Therefore, we can calculate the position of the
motor as follow:
CountValue
pos = PosCnt  2 +  2 (rad ) (4.7)
MaxCnt
Based on the rotation of the motor (rad), Eq. (4.7), students have to calculate the
translation of the load (mm).
4.4. PID controller

112
4.4.1 PID calculation
The mathematic equation of the PID controller:
t
de(t)
u(t) = K P e ( t ) + K I  e(t)d(t) + K D (4.8)
0
dt

When implementing the PID controller in practice, the input variable (error) is obtained
by sampling the plant’s output at the sample rate. Then, the PID algorithm is also
calculated at the same rate. At the step kth, we have:
u k = u Pk + u Dk + u Ik (4.9)

KP

e(t ) t u (t ) uˆ(t )
K I  e( )d
0

de(t )
KD
dt

Fig 4.11: Block diagram of a PID controller

➢ P Calculation
u P k = K Pek (4.10)
➢ D calculation
ek − ek −1
u Dk = K D (4.11)
T
➢ I calculation

e(t) e(t) e(t)

ek ek ek
ek-1 ek-1 ek-1

t t t
(k-1)T kT (k-1)T kT (k-1)T kT

a) b) c)

Fig 4.12: Integral approximation methods

a) Backward rectangular approximation (backward Euler):

113
 k
u Ik = K I  Tei = u kI −1 + K ITek (4.12)
i =1

b) Forward rectangular approximation (forward Euler):

k
u Ik = KI  Tei−1 = u kI −1 + K ITek −1 (4.13)
i =1

c) Trapezoidal approximation:
k
ei−1 + ei e +e
u Ik = KI T 2
= u kI −1 + K IT k −1 k
2
(4.14)
i =1

Code example:

Fig 4.13: Code example of a simple PID algorithm

4.4.2 Low pass filter for D term:
D
E (s) U D (s) 1 U f (s)
KDs
Ns + 1

Fig 3.14: Low pass filter for D term

N T
u D f (k) = u D f (k − 1) + u D (k) (4.15)
N+T N+T
T
where,  = (0    1) : coefficient of low pass filter
N +T
From (4.15), we derive the equation of low-pass filter calculation:

114
  u D f (k) = (1 − )u D f (k − 1) + u D (k) (4.16)
4.4.3 Anti-windup

PID
e u û
e
Ki
 edt

ereset
Kb

Fig 4.15: Block diagram of an anti-windup structure

From the block, we draw out the equation to calculate the anti-windup for I term
t
u (t) =   K Ie( ) + K be reset ( )  d
I

 u I k = u I k −1 + K I Te k + K bTe reset k (4.17)

4.5. Experiments
4.5.1 Programming using STM32F103
Board STM32F103RX

115
 Fig 4.17: Schematic of all I/O pinouts

Programmer/Debugger

Fig 4.18: Schematic and pinouts of SWD

H-Bridge

116
 Fig 4.19: H-bridge pinouts

Specifications:

✓ Input voltage: 8V-32V; Continuous current: 16A

✓ Input level: 3.3-5V

✓ The PWM frequency max: 20 KHz

✓ Duty cycle: 0-100%

Hardware layout and wiring diagram

Investigating the components and hardware connection of the system, each group
has to draw its schematic and include it in the report.

4.5.2 Microcontroller programming

Using STM32CUBEMX to create a project
Step 1: Create a project using STM32F103RCT

117
Step 2: Clock configuration. Note that it is only an example, students can set up clocks
with other parameters but in that case, from now on, all calculations related to clocks
have to be modified.

118
Step 3: SWD configuration for programming

Step 4: Hardware declaration

We use two external interrupts to read encoder signals (Ch. A, Ch. B). In this
example, PB4 and PB6 will be connected and configured as the below figure.

Enable external interrupt to read encoder

119
 PWM settings: A PWM channel is used for the H-bridge. In this case, TIM3_CH2
(PC7) is configured as the following figures:

The PWM frequency is calculated:

Timer_clock (APB1)
Timer_tick = (3.17)
Prescaller + 1
Timer_tick
f PWM = (3.18)
Counter Period + 1
Example: Timer_clock (APB1) = 24 Mhz (Clock configuration)
Prescaller = 11; Counter Period =100
24  106
Timer_tick = = 2  106 (Hz)
11 + 1
2  106
 f PWM =  19.8 (KHz)
100 + 1
TIM3_CH2 is configured as follows:

120
USART settings: Using UART to communicate with computer

121
 Timer Interrupt: Timer 4 is adopted to configure a cyclic interrupt 5 (ms). Using
Eq. 3.17 to calculate the setting parameters as follow

Enable TIM4 interrupt

122
Step 4: Code generation

4.5.3 Position control

Students have to modify the given code including:
✓ 2 external interrupts for encoder reading (x4 mode): EXTI4_IRQHandler,
EXTI9_5_IRQHandler. Using sample code in Fig. 4.10.
✓ Position in TIM4_IRQHandler using Eq. (4.7).

123
 ✓ HAL_UART_RxCpltCallback: based on the sample code to communicate with
Visual C# interface (appendix).
✓ Open the Visual studio program, choose parameters for RS232 connection. Note
that the BaudRate has to be compatible with the one already configured in
USART1.
✓ Identify the transfer functions of the system including H-bridge and the motor
(similar to the experiment of velocity control) and the load.
✓ Write a PI algorithm with anti-windup (similar to the experiment of velocity
control) for the inner loop (velocity control loop) and PD controller with low pass
filter for the outer loop (position control loop).

3.6. Report
Student have to show all their results including the hardware (schematic), obtained
transfer functions, calculations for PI/PD parameters, the code and the system
responses.

124
 APPENDIX
USART1 COMUNICATION
int16_t DesiredSpeed;
char Rx_indx, Rx_Buffer[20],Rx_data[2];
float DesiredPos;
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#define GETCHAR_PROTOTYPE int fgetc(FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,100);
return ch;
}
// Ham ngat Uart
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
uint8_t i;
if(huart->Instance == USART1) { //uart1

if(Rx_indx==0) {for (i=0;i<20;i++) Rx_Buffer[i] = 0;}

switch(Rx_data[0]) {
/* dung dong co */
case 'e':
run =false;
break;
/* dong co chay */
case 'r':
run = true;
break;
case 'b':

125
// reset();
break;
case 's':
DesiredPos = atoi(Rx_Buffer);
memset(Rx_Buffer, 0, sizeof(Rx_Buffer));
Rx_indx = 0;
break;
case 'v':
DesiredSpeed = atoi(Rx_Buffer);
memset(Rx_Buffer, 0, sizeof(Rx_Buffer));
Rx_indx = 0;
break;
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
case '.':
case '-':
Rx_Buffer[Rx_indx++] |= Rx_data[0];
break;
default:
break;
}
HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);
}
}

126