## 3 Control Method
### PID Control
These concepts are specifically defined using the nomenclature used in [14]:  
\[B= \begin{bmatrix}m&0&0\\ 0&m&0\\ 0&0&J\end{bmatrix} \quad C= \begin{bmatrix}0&0&0\\ 0&0&0\\ 0&0&0\end{bmatrix} \quad G= \begin{bmatrix}-mg\sin(\gamma)\\ 0\\ 0\end{bmatrix} \]  
\[Q= \begin{bmatrix}\sum_{i=1}^{n_{i}}F_{x_{i}}(\varepsilon_{r_{i}})\\ \sum_{i=1}^{n_{i}}F_{y_{i}}(\varepsilon_{r_{i}})\\ \sum_{i=1}^{n_{i}}(x_{GP}F_{y_{i}}(\varepsilon_{r_{i}})-y_{GP}F_{x_{i}}( \varepsilon_{r_{i}}))\end{bmatrix} \]  
Achieving the desired configuration \(q_{d}\), which may change over time (trajectory) or stay constant (position), is the aim of control. Equation (2) expresses the intended generalized forces and torques in PID control [23]. This ideal configuration, along with the actual configuration \(q\), defines the error term \(e=q_{d}-q\). \[Q_{d}(q,q)=K_{p}e_{p}+K_{i}e_{i}+K_{d}e_{d}+G \tag{2}\]  
In this case, the terms are the following. First, \(e_{p}\), which coincides with \(e\), is the proportional error term with its coefficient matrix \(K_{p}\). Secondly, \(e_{i}\) is the integral term \(e_{i}=\int_{0}^{t}e(t)dt\) with its coefficient matrix \(K_{i}\). Thirdly, \(e_{d}\) is the derivative term, denoted as \(e_{d}=\frac{de(t)}{dt}\) with its related matrix, \(K_{d}\). Ultimately, \(G\) employed to compensate the gravitational term. The Lyapunov candidate of the system \((V=\frac{1}{2}e_{d}^{T}Be_{d}+\frac{1}{2}e_{p}^{T}K_{p}e_{p})\) should ideally trend to zero given these forces and the \(q_{d}\) constant, guaranteeing system stability. Nevertheless, these forces are not generated by modules that are completely actuated and capable of orienting in every direction. Therefore, at this stage of the process, the objective is to find a combination of orientations for the module axes that minimizes the norm of the difference between the ideal and actual force values (Equation (3)). In practice, all possible k-permutations are taken into consideration because each module can only be oriented in four different ways (\(\varepsilon_{r}\): 0 deg, \(\pm 45\)deg, 90 deg) plus the free sphere condition (\(\varepsilon_{r}\): none; the blocked case was not included), and there are typically few modules in contact (less than ten, for instance). As stated before, for every permutation, the difference with \(Q_{d}\), the difference that is really produced \(Q\), and the norm of this difference are computed. Next, using Equation (3), the set of angles (\(\varepsilon_{r}\)) that yield the smallest norm is chosen. As a result, oscillations around the intended configuration are caused by intrinsic disturbances in the system. Although these variances cannot be completely eliminated, they can be minimized by speeding up the system's reaction and analysis times. \[\varepsilon_{r}:min\{Q_{d}-Q\} \tag{3}\]  
Figure 5 presents a flowchart summarizing the control scheme that implements the aforementioned logic. In addition to the previously defined terms, C represents the control block, while P denotes the system dynamics. This concludes the introduction to the theoretical aspects of the applied controls. In the following subsections, experimental and simulated results obtained from applying the threshold-based and PID controllers will be presented.