# Bio-Inspired Design of an Underwater Robot Exploiting Fin Undulation Propulsion
## 2 Materials and Methods
### Control Algorithm  
The control algorithm is designed to keep the two fins synchronized and to guarantee the desired phase shift between the mechanisms of each fin. During forward swimming, the phase shift between mechanisms is the same for both fins and each mechanism is synchronized with the corresponding one on the other fin. During a turn maneuver, the phase shift between mechanisms is the opposite for the two fins, the two central mechanisms are again synchronized, but the front mechanisms are synchronized with the back mechanisms of the opposite fin. For each motor, the velocity is calculated as the sum of a feedforward part and of a feedback part. The feedforward part is the reference velocity of motors (\(\omega_{ref}\)), which is proportional to the frequency of fin motion, it is equal for all motors and it is provided by the user. The feedback part is a correction needed to maintain the desired phase shift. A Proportional-Integral (PI) control logic is adopted to correct the phase shift between two mechanisms: the angular position of each motor is measured, and the actual phase shift among all motors is computed; then, the phase shift is corrected accelerating the motor in delay and slowing the motor in advance, as depicted in the block diagram in Figure 18.  
The block diagram of the complete control algorithm for forward swimming is shown in Figure 19. Since the mechanisms are mounted in a mirrored configuration, the motors of different fins rotate in opposite directions. Therefore, the phase shift between the same mechanisms for the same fins is calculated as the sum of the two angles instead of their difference. Figure 19: Block diagram of the control algorithm for forward swimming. Figure 18: Block diagram of the control algorithm correcting the phase shift between mechanisms of the same fin. Considering, for example, the central mechanism of the right fin, the input velocity given to the motor is calculated as follows:  
\[\omega_{1}=\omega_{ref}+\bigg{(}k_{p}^{(m)}+\frac{k_{i}^{(m)}}{s}\bigg{)}\Big{(} \Delta\psi_{0,1}-\Delta\psi_{ref}\Big{)}-\bigg{(}k_{p}^{(m)}+\frac{k_{i}^{(m)}} {s}\bigg{)}\Big{(}\Delta\psi_{1,2}-\Delta\psi_{ref}\Big{)}+\bigg{(}k_{p}^{(f)}+ \frac{k_{i}^{(f)}}{s}\bigg{)}\Delta\psi_{1,A}, \tag{10}\]  
where \(\omega_{ref}\) is the reference velocity given by the input, \(\Delta\psi_{ref}\) is the desired phase shift between the mechanisms, and \(\psi_{k}\) is the measured angular position of the motor. The numeration of motors is the same as in Figure 19. The gains \(k_{p}^{(m)}\) and \(k_{i}^{(m)}\) control the phase shift between mechanisms of the same fin, and the gains \(k_{p}^{(f)}\) and \(k_{i}^{(f)}\) control the synchronization between the two fins. The control algorithm acting on all six motors can be summarized in a matrix form, shown in Equation (11) for forward swimming and in Equation (12) for a left turn. (In both equations, the integral contribution is omitted to simplify the notation.) \[ \begin{bmatrix}\omega_{0}\\ \omega_{1}\\ \omega_{2}\\ \omega_{3}\\ \omega_{4}\\ \omega_{5}\end{bmatrix} = \begin{cases}\omega_{ref}\\ \omega_{ref}\\ \omega_{ref}\\ \omega_{ref}\\ -\omega_{ref}\\ -\omega_{ref}\\ -\omega_{ref}\end{cases} + \begin{bmatrix}-2k_{p}^{(m)}-k_{p}^{(f)}&k_{p}^{(m)} &k_{p}^{(m)}&-k_{p}^{(f)}&0&0\\ k_{p}^{(m)}&-2k_{p}^{(m)}-k_{p}^{(f)}&k_{p}^{(m)}&0&-k_{p}^{(f)}&0\\ k_{p}^{(m)}&k_{p}^{(m)}&-2k_{p}^{(m)}-k_{p}^{(f)}&0&0&-k_{p}^{(f)}\\ -k_{p}^{(f)}&0&0&-2k_{p}^{(m)}-k_{p}^{(f)}&k_{p}^{(m)}&k_{p}^{(m)}\\ 0&-k_{p}^{(f)}&0&k_{p}^{(m)}&-2k_{p}^{(m)}-k_{p}^{(f)}&k_{p}^{(m)}\\ 0&0&-k_{p}^{(f)}&k_{p}^{(m)}&k_{p}^{(m)}&-2k_{p}^{(m)}-k_{p}^{(f)}\end{bmatrix} \begin{bmatrix}\psi_{0}\\ \psi_{1}\\ \psi_{2}\\ \psi_{3}\\ \psi_{4}\\ \psi_{5}\end{bmatrix} + \begin{bmatrix}-3k_{p}^{(m)}\\ 3k_{p}^{(m)}\\ 3k_{p}^{(m)}\\ -3k_{p}^{(m)}\end{bmatrix} \Delta\psi_{ref} \tag{11}\]  
In both cases, the state matrices have a zero eigenvalue because the reference is given in terms of difference in the angular positions and no absolute reference position is given to the motors.