MODEL 4.

VELOCITY 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
✓ Velocity control of a DC servo motor

4.2. ONE AXIS SERVO SYSTEM USING DC SERVO MOTORS

4.2.1 DC servo 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
dt
(4.2)
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]

126
The block diagram of a DC servo motor:
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

127
Table 4.1. The characteristics of the DC servo motor
No. Parameters Symbol Units DCM50205
1 Continuous Torque (Max) TC N.m 0.25
2 Peak Torque (Stall) TPK N.m 1.59
4 Rated Speed SR rpm 3400
5 Rotor Inertia JM kg.m2 3.11 x 10-5
10 Rated Voltage E V 24
11 Rated Current I A 2.95
12 Torque Constant KT N.m/A 52 x 10-3
13 Resistance RT Ω 0.8
15 Peak Current (Stall) IP A 21.6
16 Encoder Resolution - Steps/rev. 1000
The rotary load with the parameters:

▪ Weight: m = 0.81774 kg
▪ Material: Steel CT3
▪ Radius: R =0.025 m

Therefore, the inertia moment of load is calculated:

1 2 1
JL = mr = 0.81774  0.0252 = 2.56 10 −4 [kg.m2]
2 2
4.2.3 The general diagram
G p (s)
u pwm  (rad/s)
Gc ( s ) GH ( s ) GM ( s )
 0 − 100%

Fig. 4.4: The general diagram of the controlled systems

where, Gc: The PID controller
GH: The transfer function of the H-bridge
GM: The transfer function of the motor and 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.

128
 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

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

129
From the above pulses, we can draw out a table that describes the transition states of the
two channels A and B:
Table 4.3: The transition states of channel A and B
State Clockwise Counter-Clockwise
Transition Transition
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:

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

For simplicity, 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).

130
 Fig. 4.10: Code example of channel A encoding in 4x mode

4.4. PID CONTROLLER

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.7)
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.8)

131
 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.9)
➢ D calculation
ek − ek −1
u Dk = K D (4.10)
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):
k
u Ik = K I  Tei = u kI −1 + K ITek (4.11)
i =1

b) Forward rectangular approximation (forward Euler):

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

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

Code example:

132
 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 4.14: Low pass filter for D term

N T
u D f (k) = u D f (k − 1) + u D (k) (4.14)
N+T N+T
T
where,  = (0    1) : coefficient of low pass filter
N +T
From (4.27), we derive the equation of low-pass filter calculation:
 u D f (k) = (1 − )u D f (k − 1) + u D (k) (4.15)
4.4.3 Anti-windup
PID
e u û
e
Ki
 edt

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
133
 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.16)

4.5. EXPERIMENTS
4.5.1 Programming using STM32F103
Hardware connection
L

N MCB

PC 5V DC 24V DC

GND 5V +5V
+24V GND 24V

PA9 PA10 H-Bridge

(TX) (RX)
B+(2)
STM32 P- M+
D-
PWM
TIM3_CH2 (PC7) P+ M-
PC3 DIR D+ B-(1)

PB6 PB4
EXTI9_5 EXTI4

GND 24V
encoder

Fig 4.16: Hardware layout and wiring diagram

Board STM32F103RX

Fig 4.17: Schematic of all I/O pinouts

134
Programmer/Debugger

Fig 4.18: Schematic and pinouts of SWD

H-Bridge

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 hardware and implementing the connection as the following figure.
Be careful before turning the MCB on. If there is any abnormal thing, please
inform the instructor

135
4.5.2 Microcontroller programming
Using STM32CUBEMX to create a project
Step 1: Create a project using STM32F103RCT

136
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.

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.

137
 Enable external interrupt to read encoder

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:

138
 Timer_clock (APB1)
Timer_tick = (4.17)
Prescaller + 1
Timer_tick
f PWM = (4.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:

USART settings: Using UART to communicate with computer

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

140
Enable TIM4 interrupt

Step 4: Code generation

141
142
4.5.3 Velocity 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.12
✓ Velocity estimation in TIM4_IRQHandler using Eq. (4.19)
✓ 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 function of the H-bridge and the motor by applying the pwm
signal 100% and plotting the velocity response. From the response, calculating
the parameters of the transfer function including gain (K) and time constant (  )
✓ Write a PID algorithm based on the sample code in Fig. 4.15. However, students
must improve the algorithm by adding the filter for D-term (Eq. 4.28) or anti-
windup for I-term (Eq. 4.29)

4.6. Report
Student have to show all their results including the obtained transfer function, PID
parameters, PID code and the controlled velocity response.

143
 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;

144
 /* dong co chay */
case 'r':
run = true;
break;
case 'b':
// 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:

145
 break;
}
HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);
}
}

146