7. MRM Software System
7.16 Servo Control Modules: Software Routines
The second byte received by the servo communication module is the address of the transmitter.
This is cross referenced against the register of active addresses. The feedback data is stored in a character buffer and when feedback has been received from all active addresses the module switches to feedback transmit mode.
Feedback Transmit Mode
In feedback transmit mode all buffered feedback from servo control modules is transmitted to the host PC via USART and USB for analysis. Once all data is transmitted the servo communication module is switched back to instruction receive mode. The module is then ready to repeat the processes for the next set of instructions. If feedback has not been received by the host PC for a period equal to two interpolation cycle times an error report is displayed on the machine GUI.
7.16 Servo Control Modules: Software Routines
The software system on servo control modules is concerned with the execution of position control instructions that have been received from the host PC. A position control instruction is composed of an axial increment and a time for the motion to be achieved. The cycle of periodically updating these instructions is known as an interpolation cycle, and the time specified for the increment to be achieved corresponds to the interpolation cycle time. Operations associated with position control include:
the receiving and interpretation of instructions,
the periodic execution of the position control algorithm,
the monitoring of encoder feedback,
the generation of a PWM signal to H-bridge motor drivers based on the output of the position control algorithm and
collision detection.
Position Control
Figure 7.11: Cascade Structure for Closed Loop Servo Control
The position control of an axis is achieved by the cascaded control system of Figure 7.11. The outer loop is the position control loop while the inner loop is the speed control loop. Speed control is therefore inherent in achieving position control, and the feed rates prescribed through user programs are preserved. The efficiency of the cascaded structure for closed loop servo control depends primarily on the speed and stability of the inner control loop. A slow response on the inner loop reduces machining accuracy around circles and corners, where a rapid change in the position reference is expected.
The transfer function for a typical axis P(s), is given by equation 7.19 where K is an electromotive constant, J is the moment of inertia of the mechanical system about the axis of the motor, b is a damping coefficient of the mechanical system, L is the motor inductance and R is the electrical resistance [73]. The values of these constants for the DC motors used in the MRM platform were unknown, but were not necessary for the control of the system.
109 | P a g e
7.16 Servo Control Modules: Software Routines
𝑃 𝑠 = 𝐾
𝐽𝑠 + 𝑏 𝐿𝑠 + 𝑅 + 𝐾2 (7.19) The position control loop in the system contained a proportional controller Kpp while a PID controller was implemented on the inner speed control loop. The transfer function of the PID controller is defined by equation 7.20, where Kp, Ki and Kd are the gains of the controller (coarse tuned by the Ziegler-Nichols method and fine tuned by trial).
𝐺 𝑠 = 𝐾𝑝+1
𝑠𝐾𝑖+ 𝑠𝐾𝑑 (7.20) The controller was reduced to a discrete format for implementation in software. The z-transform of the controller was obtained by determining the z-transforms of the individual terms:
𝐾𝑝⇒ 𝐾𝑝 (7.21) 𝐾𝑖
𝑠 ⇒ 𝐾𝑖𝑇
1 − 𝑧−1 (7.22) 𝐾𝑑𝑠 ⇒𝐾𝑑 1 − 𝑧−1
𝑇 (7.23) The transfer function of the controller in the discrete time domain is given by equation 7.24, where T is the sampling time for the control loop:
𝐺 𝑧 = 𝐾𝑝+ 𝐾𝑖𝑇
1 − 𝑧−1+𝐾𝑑 1 − 𝑧−1
𝑇 (7.24) The transfer function may be written in the more convenient form of equation 7.25, where the constants K0, K1 and K2 are calculated by equations 7.26-28.
𝐺 𝑧 =𝐾0+ 𝐾1 𝑧−1+ 𝐾2 𝑧−2
1 − 𝑧−1 (7.25) 𝐾0= 𝐾𝑝+ 𝐾𝑖𝑇 +𝐾𝑑
𝑇 (7.26) 𝐾1= −𝐾𝑝−2𝐾𝑑
𝑇 (7.27) 𝐾2=𝐾𝑑
𝑇 (7.28) The error e is the input to the PID controller and u is the output, therefore:
𝐺 𝑧 =𝑢
𝑒 =𝐾0+ 𝐾1 𝑧−1+ 𝐾2 𝑧−2
1 − 𝑧−1 (7.29) By manipulating equation 7.29:
𝑢 𝑛 − 𝑢 𝑛 − 1 = 𝐾0𝑒 𝑛 + 𝐾1𝑒 𝑛 − 1 + 𝐾2𝑒 𝑛 − 2
110 | P a g e
7.16 Servo Control Modules: Software Routines
The output for the PID controller at the nth iteration is therefore calculated by equation 7.30:
𝑢 𝑛 = 𝑢 𝑛 − 1 + 𝐾0𝑒 𝑛 + 𝐾1𝑒 𝑛 − 1 + 𝐾2𝑒 𝑛 − 2 (7.30) By substituting the constants K0, K1 and K2 into equation 7.30 the output of the controller is then given by equation 7.31:
𝑢 𝑛 = 𝑢 𝑛 − 1 + 𝐾𝑝 𝑒 𝑛 − 𝑒 𝑛 − 1 + 𝐾𝑖𝑇𝑒 𝑛 +𝐾𝑑
𝑇 𝑒 𝑛 − 2 𝑒 𝑛 − 1 + 𝑒 𝑛 − 2 (7.31) Equation 7.31 is recursive, where the output of the controller u(n) is dependent on the output of the previous iteration u(n-1). Based on the condition that u(0) = 0 and e(0)=0, equation 7.31 is further reduced:
𝑢 𝑛 = 𝐾𝑝𝑒 𝑛 + 𝐾𝑖𝑇 𝑒(𝑖)
𝑛 𝑖=1
+𝐾𝑑
𝑇 𝑒 𝑛 − 𝑒 𝑛 − 1 (7.32) Equation 7.32 was implemented on servo control modules for position control. The output u(n) was used to set the duty cycle of PWM signals to motor drivers , which ultimately regulates the speed of the motors. The variable e(n), which is the input to the PID controller is calculated by equation 7.33 :
𝑒 𝑛 = 𝐾𝑝𝑝. 𝐸 𝑛 −∆𝑋𝑚 𝑛 − 1
𝑇 (7.33) E(n) is the error in the existing position of an axis with regard to the reference position that is specified in position control instructions. This is calculated by equation 7.34, where i is the iteration number and DXm is the axial increment that was measured for that iteration.
𝐸 𝑛 = 𝑅𝑒𝑓 − ∆𝑋𝑚 𝑖
𝑛−1 𝑖=1
(7.34)
The reference input Ref, to the position control algorithm is dependent on the type of motion required. Rapid motions are used to move a machine axis from one point to another quickly, without the observance of a target feed rate. In this instance the reference is provided by the step function of equation 7.35. For interpolated motions the program feed rate is observed and the reference is provided by the discrete ramp function of equation 7.36. Figure 7.12 illustrates both step and discrete ramp references as functions of time.
𝑅𝑒𝑓 = 0 (𝑡 < 𝑇𝑠𝑡𝑒𝑝 ) (7.35) 𝑅𝑒𝑓 = ∆𝑋𝑖𝑑𝑒𝑎𝑙 (𝑡 ≥ 𝑇𝑠𝑡𝑒𝑝 )
𝑅𝑒𝑓 = ∆𝑋𝑜 𝑖
𝑛 𝑖=1
(7.36)
111 | P a g e
7.16 Servo Control Modules: Software Routines
Figure 7.12: Reference to Position Control Loop
Signal Sampling
The ATmega 8L chip on servo control modules was responsible for the monitoring of feedback from position encoders. Encoder pulses were counted on an external interrupt basis and each interrupt incremented the pulse count by one. For all MRM axes, 512 pulses corresponded to one full revolution of a motor shaft. Equation 3.37 and 3.38 relate the pulse count to linear and angular increments respectively, where L is the pitch of a power screw (mm) and N is a reduction ratio.
∆𝑋𝑚 = 𝐿𝑐𝑜𝑢𝑛𝑡
512 (7.37) ∆𝜃𝑚 = 360𝑁𝑐𝑜𝑢𝑛𝑡
512 (7.38) Pulse counts were transmitted from the ATmega 8L chip to the ATmega 32L chip every 100 ms for position calculations. The position control algorithm was therefore executed every 100 ms upon the receiving the data. The transmission was driven by a 16-bit timer counter on the ATmega 8L chip that triggered software interrupt. The software interrupt transmitted the updated pulse count via USART.
PWM Signal Generation
The output of the position control algorithm u(n) was used to regulate the duty cycle of the PWM signal to H-Bridge motor drivers, thus enabling the control of motor speed and ultimately axis position. Phase and frequency correct PWM was executed on the ATmega 32L chip, by using the 16-bit timer/counter unit. The Input Capture Register (ICR) was used to set the TOP value, which defines the frequency of the PWM signal. The frequency of the PWM is calculated by equation 7.39, where N is a pre-scalar and fclk_I/O is the operating frequency of the chip [67].
𝑓𝑃𝑊𝑀 = 𝑓𝑐𝑙𝑘𝐼/𝑂
2. 𝑁. 𝑇𝑂𝑃 (7.39) The operating frequency of the chip was 4 MHz, which was pre-scaled by eight. The TOP value on the ATmega 32 chip was set to 10000 resulting in a PWM frequency of 25 Hz. The Output Compare Register (OCR1A) was used to set the duty cycle of the PWM according to equation 7.40. The equation relates the integer value of the register to u(n), and K is a scaling factor. The duty cycle is then given as a percentage by equation 7.41.
Time Ref
Step
Discrete Ramp
Time Ref
112 | P a g e