# Vibration Suppression of Collaborative Robot Servo with Dual Encoders
## 4 Software Implementation of Cooperative Robot Servo  
The servo motor control process is illustrated in Figure 4, featuring three closed-loop control loops: the position, speed, and current control loop. The diagram exclusively presents the primary motor control framework, with the DSP being the commonly employed main control chip, exemplified in this paper. The DSP samples the position data from the motor encoder, enabling the calculation of motor speed through differential methods. Typically, this calculation involves dividing the sampled position deviation by the sampled time interval. In practical applications, precise knowledge of the electrical angle is crucial, usually supplied by the motor manufacturer. The manufacturer's electrical angle may deviate by 0, 90, or 180 degrees from the magnetic pole angle. If the electrical angle deviation is unknown, it must be determined through testing. One approach is to set a specific value for the excitation current \(\mathrm{I_{d}}\) (excitation current in dq coordinate) and set the torque current \(\mathrm{I_{q}}\) (torque current in dq coordinate) to zero, aligning the electrical angle with the d-axis. Another method aligns the electrical angle with the d-axis current and resets the encoder's single-turn value to 0, provided the encoder allows this reset. Encoders typically support reset. The DSP captures motor currents using high-precision milliohm-level sampling resistors. It can precisely measure the voltage across these resistors. Typically, the voltage values undergo amplification via operational amplifiers before being directed to the DSP's  
Figure 4: Flow chart of motor control by servo (* represents the set value of the module). Figure 3: Composition of collaborative robots. Analog-to-Digital Converter (ADC) module. At times, the DSP's SDFM is employed to swiftly filter currents, improving motor control. This approach necessitates a specialized SDFM signal processing chip. Moreover, the motor currents detailed in this paper incorporate hardware protection features. By configuring hardware current protection values through comparator circuits, any current surpassing the predefined threshold triggers the immediate shutdown of the PWM driver chip, ensuring timely safeguarding of the servo drive. For instance, with a set threshold of 25 ampere, if the motor current exceeds this value, the comparator of SDFM can generate an interrupt signal, and the DSP will enable the system to turn off the PWM wave within a single control cycle [10]. As shown in Figure 4, the motor currents \(\mathrm{I_{a}}\), \(\mathrm{I_{b}}\), and \(\mathrm{I_{c}}\) need to be transformed into two-phase rotating currents \(\mathrm{I_{\alpha}}\) and \(\mathrm{I_{\beta}}\) through Clark transformation. Then, it is converted into two-phase static currents \(\mathrm{I_{d}}\) and \(\mathrm{I_{d}}\) through Park transformation. \(\mathrm{I_{d}}\) is the feedback of the d-axis current loop, and \(\mathrm{I_{q}}\) is the feedback of the q-axis current loop. After passing through the current loop control, it is necessary to perform inverse transformation and Space Vector Pulse Width Modulation (SVPWM) [11; 12] and finally form the on and off voltage for controlling IPM. As shown in Figure 4, \(\mathrm{\theta^{*}}\) is the position setting of the main controller, \(\mathrm{\theta}\) is the position sampled from the motor encoder, and \(\mathrm{\omega}\) is the speed calculated from the motor encoder position. \(\mathrm{\omega^{*}}\) is the output of the position loop and also the set value of the speed loop. \(\mathrm{I_{q}^{*}}\) is the set value of the q-axis current loop, and \(\mathrm{I_{d}^{*}}\) is the set value of the d-axis current loop. There are four proportional-integral (PI) control loops in the figure, namely the position PI loop, velocity PI loop, and two current PI loops. This article mainly studies the permanent magnet synchronous motor (PMSM), which sets the excitation current \(\mathrm{I_{d}}\) to zero during the control process.