8 Conclusions and Future Works
8.2 Future work
During this project, a prototype of the robotic landing gear has been designed and built. This first version has been tested in laboratory experiments, collecting data and improving the design. However, there are many areas in which the system can be developed further.
The software simulations and laboratory pulley tests have allowed to implement several control techniques on the landing gear, to collect data in order to analyse the system performance, and improve the sensor integration and leg design. During these tests, the system was plugged to the mains and to a lab PC, although it is equipped with on-board batteries and wireless system to transmit data. The next logic step would be to test the system in a real outdoors flight. The most crucial factor here is safety, as the system needs to be able to land even in the event of a power failure on the landing gear. There are examples of experimental setups to do so in a small-medium size model helicopter using an auxiliary structure for safety like in [22] (see Figure 2-9) or [83]. Flight tests can provide important information on how the control system and the sensors perform in the presence of vibration introduced by the rotor of the helicopter.
On the practical side, a necessary feature that needs to be implemented for further development of the project, is a safety mechanism that allows the
128
helicopter to land safely even in the case of a power failure in the robotic landing gear. This can be implemented by adding a mechanical blocking mechanism of brake system to the legs design. This brake system would also allow to cut the power of the servo motors, and maintain the position after landing finishes. Very recently, several systems have appeared with a leg design that uses a
four-bar linkage [22] [21]. By modifying the leg design to mechanically constrain the motion in the vertical direction, the number of motors in the system could be reduced to one per leg, potentially reducing the weight of the system, its, complexity, and the probability of electrical failure.
During a flight, it is not always easy for the pilot to assess the inclination or possible obstacles on the terrain. As an additional feature, a slope detection system could be implemented using range sensors, without increasing significantly the complexity and cost of the system. By using an array of 4 distance sensors, one on each leg, instead of one, the distance to the ground and the slope can be known before landing. This would allow to pre-adjust the legs before landing, or to inform the pilot if the slope is too high. The feet can also be replaced by wheels to increase the ground handling capability of the system.
On the modelling side, the effort was placed on developing the mathematical model of the legged system, while the helicopter model was reduced to a thrust force model to control the descent rate. An area of improvement can be the development of more realistic helicopter models that better describe the dynamics and behaviour of the system, and combine them with the landing gear model. Although some examples were found in the literature review like in [84] [83], these area was left out of the scope of this project.
On the control side, it would be interesting to try a different actuator technology on future versions of the prototype. This would allow to implement different control approaches like model-based control or virtual model control, and to compare the results with the actual control system.
129
Appendix A
Planar Model
As described in section 3.1., the full-body planar model of the system is obtained by applying Newton-Euler equations to each individual link. There are three equations of motion for each of the five links: summation of horizontal forces, summation of vertical forces, and summation of torques about the centre of mass. By eliminating the internal reaction forces the system can be reduced to seven equations.
Equations 3.3-3.5, 3.12-3.17 and 3.22-3.27, describe the dynamics of each link on each degree of freedom. Equations 3.8-3.11, 3.18-3.21, and 3.28-3.31, describe the kinematic relations between the accelerations of the links and the generalised coordinates. By introducing the kinematic equations into the dynamic equations, the equations of motion can be expressed in terms of the generalised coordinates only. This allows to calculate the joint torques if the external forces and robot’s kinematics are known.
The equations are expressed in matrix form as
𝐌(𝐪)𝐪̈ + 𝐂(𝐪, 𝐪̇)𝐪̇ + 𝐆(𝐪) = 𝛕 + 𝐉𝐓(𝐪)𝐟 (A.1) where q is the generalised coordinates vector, 𝐪 = [𝑦𝐵, 𝑧𝐵, 𝜃, 𝑞1, 𝑞2, 𝑞3, 𝑞4]𝑇, M is the
inertia matrix, C is the matrix of centrifugal and Coriolis terms, G is the vector of gravity terms, 𝛕 = [0, 0, 0, 𝜏1, 𝜏2, 𝜏3, 𝜏4]𝑇 is the vector of joint actuator torques, JT is the
transpose of the Jacobian matrix, and f is the vector of external forces. The sub-indices
Hr, Kr, Hl, and Kl, used in the equations in chapter 3 have been substituted in the
appendix for 1, 2, 3, and 4 respectively for more clarity of the equations. The fully extended equations are given below:
130
Equation 1: angular acceleration of the main body
[2(𝑚𝑈 + 𝑚𝐿)𝐷𝑧𝑐𝜃 − 𝑚𝑙𝐶𝑈(𝑠𝛽1+ 𝑠𝛽3) − 𝑚𝐿𝑙𝐶𝐿(𝑠𝛽2+ 𝑠𝛽4)]𝑦̈𝐵 + [2(𝑚𝑈+ 𝑚𝐿)𝐷𝑧𝑠𝜃 + 𝑚𝑙𝐶𝑈(𝑐𝛽1+ 𝑐𝛽3) + 𝑚𝐿𝑙𝐶𝐿(𝑐𝛽2+ 𝑐𝛽4)]𝑧̈𝐵 + [𝐼𝐵+ 2𝐼𝑈 + 2𝐼𝐿+ (𝑚𝑈+ 𝑚𝐿)(2𝐷𝑧2+ 2𝐷 𝑦2) + 2𝑚𝐿(𝑙𝑈2 + 𝑙𝐶𝐿 2 + 𝑙 𝑈𝑙𝐶𝐿(𝑐𝑞2 + 𝑐𝑞4)) + 2𝑚𝑈𝑙𝐶𝑈 2 + 𝑚𝐿𝑙𝐿(𝐷𝑧(𝑠(𝜃 − 𝛽2) + 𝑠(𝜃 − 𝛽4)) + 𝐷𝑦(𝑐(𝜃 − 𝛽2) − 𝑐(𝜃 − 𝛽4))) + 𝑚𝑙𝑢(𝐷𝑦(𝑐𝑞1− 𝑐𝑞3) − 𝐷𝑧(𝑠𝑞1+ 𝑠𝑞3))] 𝜃̈ + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝑈𝑙𝐶𝑈 2 + 𝑚 𝐿(𝑙𝑈2 + 𝑙𝐶𝐿 2 + 2𝑙 𝑈𝑙𝐶𝐿𝑐𝑞2) + 𝑚𝑙𝐶𝑈(𝑅1𝑐𝛽1− 𝑅2𝑠𝛽1) + 𝑚𝐿𝑙𝐶𝐿(𝑅1𝑐𝛽2− 𝑅2𝑠𝛽2)]𝑞̈1 + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶2𝐿 + 𝑙𝑈𝑙𝐶𝐿𝑐𝑞2) + 𝑚𝐿𝑙𝐶𝐿(𝑅1𝑐𝛽2 − 𝑅2𝑠𝛽2)]𝑞̈2 + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝑈𝑙𝐶2𝑈 + 𝑚𝐿(𝑙𝑈2 + 𝑙𝐶2𝐿 + 2𝑙𝑈𝑙𝐶𝐿𝑐𝑞4) + 𝑚𝑙𝐶𝑈(𝑅3𝑐𝛽3− 𝑅4𝑠𝛽3) + 𝑚𝐿𝑙𝐶𝐿(𝑅3𝑐𝛽4− 𝑅4𝑠𝛽4)]𝑞̈3 + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶𝐿 2 + 𝑙 𝑈𝑙𝐶𝐿𝑐𝑞4) + 𝑚𝐿𝑙𝐶𝐿(𝑅3𝑐𝛽4− 𝑅4𝑠𝛽4)]𝑞̈4 − [𝑚𝑙𝑢(𝑅1𝑠𝛽1+ 𝑅2𝑐𝛽1)𝑞̇1+ 𝑚𝐿𝑙𝐿(𝑅1𝑠𝛽2+ 𝑅2𝑐𝛽2)(𝑞̇1+ 𝑞̇2) + 𝑚𝐿𝑙𝑈𝑙𝐿𝑠𝛽2𝑞̇2+ 𝑚𝑙𝑢(𝑅3𝑠𝛽3+ 𝑅4𝑐𝛽3)𝑞̇3 + 𝑚𝐿𝑙𝐿(𝑅3𝑠𝛽4+ 𝑅4𝑐𝛽4)(𝑞̇3+ 𝑞̇4) + 𝑚𝐿𝑙𝑈𝑙𝐿𝑠𝛽4𝑞̇4]𝜃̇ − [𝑚𝑙𝐶𝑈(𝑅1𝑠𝛽1+ 𝑅2𝑐𝛽1) + 𝑚𝐿𝑙𝐶𝐿(𝑅1𝑠𝛽2+ 𝑅2𝑐𝛽2)]𝑞̇1 2 − [𝑚𝐿𝑙𝐶𝐿(𝑙𝑢𝑠𝑞2+ 𝑅1𝑠𝛽2+ 𝑅2𝑐𝛽2)](2𝑞̇1+ 𝑞̇2)𝑞̇2 − [𝑚𝑙𝐶𝑈(𝑅3𝑠𝛽3+ 𝑅4𝑐𝛽3) + 𝑚𝐿𝑙𝐶𝐿(𝑅3𝑠𝛽4+ 𝑅4𝑐𝛽4)]𝑞̇32 − [𝑚𝐿𝑙𝐶𝐿(𝑙𝑢𝑠𝑞4+ 𝑅3𝑠𝛽4+ 𝑅4𝑐𝛽4)](2𝑞̇3+ 𝑞̇4)𝑞̇4+ 2(𝑚𝑈+ 𝑚𝐿)𝑔𝐷𝑧𝑠𝜃 + 𝑚𝑔𝑙𝐶𝑈(𝑐𝑞1+ 𝑐𝑞3) + 𝑚𝐿𝑔𝑙𝐶𝐿(𝑐𝑞2+ 𝑐𝑞4) = (𝐷𝑧𝑐𝜃 − 𝐷𝑦𝑠𝜃 − 𝑙𝑈𝑠𝛽1− 𝑙𝐿𝑠𝛽2)𝐹𝑥𝑅+ (𝐷𝑧𝑐𝜃 + 𝐷𝑦𝑠𝜃 − 𝑙𝑈𝑠𝛽3 − 𝑙𝐿𝑠𝛽4)𝐹𝑥𝐿 + (𝐷𝑧𝑠𝜃 + 𝐷𝑦𝑐𝜃 + 𝑙𝑈𝑐𝛽1+ 𝑙𝐿𝑐𝛽2)𝐹𝑦𝑅+ (𝐷𝑧𝑠𝜃 − 𝐷𝑦𝑐𝜃 + 𝑙𝑈𝑐𝛽3+ 𝑙𝐿𝑐𝛽4)𝐹𝑦𝐿 (A.2) where 𝑅1= 𝐷𝑧𝑠𝜃 + 𝐷𝑦𝑐𝜃 ; 𝑅2= 𝐷𝑧𝑐𝜃 − 𝐷𝑦𝑠𝜃 ; 𝑅3= 𝐷𝑧𝑠𝜃 − 𝐷𝑦𝑐𝜃 ; 𝑅4= 𝐷𝑧𝑐𝜃 + 𝐷𝑦𝑠𝜃
131
Equation 2: horizontal linear acceleration of the main body
[𝑚𝐵+ 2𝑚𝑈+ 2𝑚𝐿]𝑦̈𝐵 + [2(𝑚𝑈+ 𝑚𝐿)𝐷𝑧𝑐𝜃 + 𝑚𝑙𝐶𝑈(𝑠𝛽1+ 𝑠𝛽3) + 𝑚𝐿𝑙𝐶𝐿(𝑠𝛽2+ 𝑠𝛽4)]𝜃̈ − [𝑚𝑙𝐶𝑈𝑠𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2]𝑞̈1 − 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2𝑞̈2− [𝑚𝑙𝐶𝑈𝑠𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4]𝑞̈3 − 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4𝑞̈4 + [2(𝑚𝑈+ 𝑚𝐿)𝐷𝑧𝑠𝛼𝜃̇ − 𝑚𝐿𝑙𝐿𝑐𝛽2𝑞̇2 − (𝑚𝑙𝐶𝑈𝑐𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2)(𝜃̇ + 2𝑞̇1) − 𝑚𝐿𝑙𝐿𝑐𝛽4𝑞̇4 − (𝑚𝑙𝐶𝑈𝑐𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4)(𝜃̇ + 2𝑞̇3)]𝜃̇ − [𝑚𝑙𝐶𝑈𝑐𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2]𝑞̇12 − 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2(𝑞̇2+ 2𝑞̇1)𝑞̇2− [𝑚𝑙𝐶𝑈𝑐𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4]𝑞̇32 − 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4(𝑞̇4+ 2𝑞̇3)𝑞̇4 = 𝐹𝑥𝑅+ 𝐹𝑥𝐿 (A.3)
Equation 3: vertical linear acceleration of the main body
[𝑚𝐵+ 2𝑚𝑈+ 2𝑚𝐿]𝑧̈𝐵 + [2(𝑚𝑈+ 𝑚𝐿)𝐷𝑧𝑠𝜃 + 𝑚𝑙𝐶𝑈(𝑐𝛽1+ 𝑐𝛽3) + 𝑚𝐿𝑙𝐶𝐿(𝑐𝛽2 + 𝑐𝛽4)]𝜃̈ + [𝑚𝑙𝐶𝑈𝑐𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2]𝑞̈1+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2𝑞̈2+ [𝑚𝑙𝐶𝑈𝑐𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4]𝑞̈3 + 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4𝑞̈4 + [2(𝑚𝑈+ 𝑚𝐿)𝐷𝑧𝑐𝜃𝜃̇ − 𝑚𝐿𝑙𝐿𝑠𝛽2𝑞̇2 − (𝑚𝑙𝐶𝑈𝑠𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2)(𝜃̇ + 2𝑞̇1) − 𝑚𝐿𝑙𝐿𝑠𝛽4𝑞̇4 − (𝑚𝑙𝐶𝑈𝑠𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4)(𝜃̇ + 2𝑞̇3)]𝜃̇ − [𝑚𝑙𝐶𝑈𝑠𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2]𝑞̇12 − 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2(𝑞̇2+ 2𝑞̇1)𝑞̇2− [𝑚𝑙𝐶𝑈𝑠𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4]𝑞̇32− 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4(𝑞̇4 + 2𝑞̇3)𝑞̇4+ (𝑚𝐵+ 2𝑚𝑈+ 2𝑚𝐿)𝑔 = 𝐹𝑦𝑅+ 𝐹𝑦𝐿+ 𝐹𝑡ℎ (A.4)
132
Equation 4: right hip joint angle
−[𝑚𝑙𝐶𝑈𝑠𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽2]𝑦̈𝐵+ [𝑚𝑙𝐶𝑈𝑐𝛽1+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2]𝑧̈𝐵 + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝐿(𝑙𝑈2 + 𝑙 𝐶𝐿 2 + 2𝑙 𝑈𝑙𝐶𝐿𝑐𝑞2) + 𝑚𝑈𝑙𝐶𝑈 2 + 𝑚𝑙𝐶𝑈(𝐷𝑦𝑐𝑞1− 𝐷𝑧𝑠𝑞1) + 𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑠(𝜃 − 𝛽2) + 𝐷𝑦𝑐(𝜃 − 𝛽2))]𝜃̈ + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝐿(𝑙𝑈2 + 𝑙𝐶𝐿 2 + 2𝑙 𝑈𝑙𝐶𝐿𝑐𝑞2) + 𝑚𝑈𝑙𝐶𝑈 2 ]𝑞̈ 1 + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶2𝐿+ 𝑙𝑈𝑙𝐶𝐿𝑐𝑞2)]𝑞̈2 + [[𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑐(𝜃 − 𝛽2) − 𝐷𝑦𝑠(𝜃 − 𝛽2)) + 𝑚𝑙𝐶𝑈(𝐷𝑧𝑐𝑞1+ 𝐷𝑦𝑠𝑞1)]𝜃̇ + 𝑚𝐿𝑙𝑈𝑙𝐿𝑠𝑞2𝑞̇2]𝜃̇ − 𝑚𝐿𝑙𝑈𝑙𝐶𝐿𝑠𝑞2(2𝑞̇1+ 𝑞̇2)𝑞̇2+ 𝑚𝑔𝑙𝑈𝑐𝛽1 + 𝑚𝐿𝑔𝑙𝐶𝐿𝑐𝛽2 = 𝜏1− (𝑙𝑈𝑠𝛽1+ 𝑙𝐿𝑠𝛽2)𝐹𝑥𝑅+ (𝑙𝑈𝑐𝛽1+ 𝑙𝐿𝑐𝛽2)𝐹𝑦𝑅 (A.5)
Equation 5: right knee joint angle
−𝑚𝐿𝑙𝐶𝐿𝑠𝛽2𝑦̈𝐵+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽2𝑧̈𝐵 + [𝐼𝐿+ 𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑠(𝜃 − 𝛽2) + 𝐷𝑦𝑐(𝜃 − 𝛽2) + 𝑙𝑈𝑐𝑞2+ 𝑙𝐶𝐿)]𝜃̈ + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶𝐿 2 + 𝑙 𝐶𝐿𝑙𝑈𝑐𝑞2)]𝑞̈1+ [𝐼𝐿+ 𝑚𝐿𝑙𝐶𝐿 2 ]𝑞̈ 2 + [[𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑐(𝜃 − 𝛽2) − 𝐷𝑦𝑠(𝜃 − 𝛽2))]𝜃̇ + 𝑚𝐿𝑙𝐶𝐿𝑙𝐿𝑠𝑞2(𝜃̇ + 2𝑞̇1)] 𝜃̇ + 𝑚𝐿𝑙𝑈𝑙𝐶𝐿𝑠𝑞2𝑞̇12+ 𝑚𝐿𝑔𝑙𝐶𝐿𝑐𝛽2 = 𝜏2− 𝑙𝐿𝑠𝛽2𝐹𝑥𝑅+ 𝑙𝐿𝑐𝛽2𝐹𝑦𝑅 (A.6)
Equation 6: left hip joint angle
−[𝑚𝑙𝐶𝑈𝑠𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑠𝛽4]𝑦̈𝐵+ [𝑚𝑙𝐶𝑈𝑐𝛽3+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4]𝑧̈𝐵 + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝐿(𝑙𝑈2 + 𝑙𝐶2𝐿 + 2𝑙𝑈𝑙𝐶𝐿𝑐𝑞4) + 𝑚𝑈𝑙𝐶2𝑈 + 𝑚𝑙𝐶𝑈(𝐷𝑦𝑐𝑞3− 𝐷𝑧𝑠𝑞3) + 𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑠(𝜃 − 𝛽4) − 𝐷𝑦𝑐(𝜃 − 𝛽4))]𝜃̈ + [𝐼𝑈+ 𝐼𝐿+ 𝑚𝐿(𝑙𝑈2 + 𝑙𝐶𝐿 2 + 2𝑙 𝑈𝑙𝐶𝐿𝑐𝑞4) + 2𝑚𝑈𝑙𝐶𝑈 2 ]𝑞̈ 3 + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶𝐿 2 + 𝑙 𝑈𝑙𝐶𝐿𝑐𝑞4)]𝑞̈4 + [[𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑐(𝜃 − 𝛽4) + 𝐷𝑦𝑠(𝜃 − 𝛽4)) + 𝑚𝑙𝐶𝑈(𝐷𝑧𝑐𝑞3− 𝐷𝑦𝑠𝑞3)]𝜃̇ + 𝑚𝐿𝑙𝑈𝑙𝐿𝑠𝜃4𝑞̇4]𝜃̇ − 𝑚𝐿𝑙𝑈𝑙𝐶𝐿𝑠𝑞4(2𝑞̇3+ 𝑞̇4)𝑞̇4+ 𝑚𝑔𝑙𝑈𝑐𝛽3 + 𝑚𝐿𝑔𝑙𝐶𝐿𝑐𝛽4 = 𝜏3 − (𝑙𝑈𝑠𝛽3+ 𝑙𝐿𝑠𝛽4)𝐹𝑥𝑅+ (𝑙𝑈𝑐𝛽3+ 𝑙𝐿𝑐𝛽4)𝐹𝑦𝑅 (A.7)
133
Equation 7: left knee joint angle
−𝑚𝐿𝑙𝐶𝐿𝑠𝛽4𝑦̈𝐵+ 𝑚𝐿𝑙𝐶𝐿𝑐𝛽4𝑧̈𝐵 + [𝐼𝐿+ 𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑠(𝜃 − 𝛽4) − 𝐷𝑦𝑐(𝜃 − 𝛽4) + 𝑙𝑈𝑐𝑞4+ 𝑙𝐶𝐿)]𝜃̈ + [𝐼𝐿+ 𝑚𝐿(𝑙𝐶2𝐿 + 𝑙𝐶𝐿𝑙𝑈𝑐𝑞4)]𝑞̈3+ [𝐼𝐿+ 𝑚𝐿𝑙𝐶2𝐿]𝑞̈4 + [[𝑚𝐿𝑙𝐶𝐿(𝐷𝑧𝑐(𝜃 − 𝛽4) + 𝐷𝑦𝑠(𝜃 − 𝛽4))]𝜃̇ + 𝑚𝐿𝑙𝐶𝐿𝑙𝐿𝑠𝑞4(𝜃̇ + 2𝑞̇3)] 𝜃̇ + 𝑚𝐿𝑙𝑈𝑙𝐶𝐿𝑠𝑞4𝑞̇3 2+ 𝑚 𝐿𝑔𝑙𝐶𝐿𝑐𝛽4 = 𝜏4− 𝑙𝐿𝑠𝛽4𝐹𝑥𝑅+ 𝑙𝐿𝑐𝛽4𝐹𝑦𝑅 (A.8)
134
Appendix B
Centroidal Momentum Matrix
This Appendix includes the Matlab script used to calculate the Centroidal Momentum Matrix that maps the inertia of each individual link into the system’s CoM. First, the location of the CoM in the z-axis is computed. Then, the inertia tensor of the whole system is calculated by aligning the principal moments of inertia of each link with the axes of the main body fixed frame, and then applying the parallel axis theorem to express the moments of inertia around the CoM.
%% PRELIMINARIES
%This section defines the main system parameters % Main body dimensions (m)
Dx=100/1000; %x dimension Dy=50/1000; %y dimension Dz=100/1000; %z dimension! % Links length (m) l_u=0.0935; l_l=0.1045; % Masses (kg)
mb=2.5; %main body (base)
mu=0.1; %upper link
ml=0.15; %lower link
thick=0.03; %Links thickness
%Main rotor parameters
%https://www.align-trex.co.uk/425-carbon-fiber-blades-hd420f.html
m_mr=0.12; %mass of rotor blades (120g/set)
r_mr=0.489; %main rotor radius (diameter of Trex500L=978mm) %Total system mass
m=mb+4*mu+4*ml+m_mr;
%Joint angles for different legs positions %Landing position
th_u_L=-pi; th_l_L=-pi/2; th_u_R=0; th_l_R=-pi/2;
%Left retracted - Right extended % th_u_L=-3.979;
% th_l_L=-1.867; % th_u_R=-0.75; % th_l_R=-1.326;
%Right retracted - Left extended % th_u_L=-2.39;
% th_l_L=-1.815; % th_u_R=0.837; % th_l_R=-1.274;
135
%%--- CoM LOCATION--- %This section computes the location of the system CoM in the z-direction taking as a reference the CoM of the base link
z_b=0;% z-ccordinate of the base link z_mr=Dz;% z-ccordinate of the main rotor
z_u_R=-Dz+(l_u/2)*sin(th_u_R); % z-coordinate of the upper link(Right Side) z_u_L=-Dz+(l_u/2)*sin(th_u_L); % z-coordinate of the upper link(Left Side) z_l_L=-Dz+l_u*sin(th_u_L)+(l_l/2)*sin(th_l_L); % z-coordinate of the lower link(Left Side)
z_l_R=-Dz+l_u*sin(th_u_R)+(l_l/2)*sin(th_l_R); % z-coordinate of the lower link(Right Side)
%Distance from the base link CoM to the system’s CoM
d=-(mb*z_b+m_mr*z_mr+mu*(2*z_u_R+2*z_u_L)+ml*(2*z_l_L+2*z_l_R))/m;
%% ---INERTIA MOMENTS OF THE MAIN BODY--- %Inertia wrt principal axes
Ixx_b=mb*((2*Dy)^2 + (2*Dz)^2)/12; Iyy_b=mb*((2*Dz)^2 + (2*Dx)^2)/12; Izz_b=mb*((2*Dy)^2 + (2*Dx)^2)/12; % Distance base-CoM xg_b=0; yg_b=0; zg_b=d;
% Moments of Inertia referred to the base CoM
Ixxg_b=Ixx_b + mb*(xg_b^2 + zg_b^2); Iyyg_b=Iyy_b + mb*(zg_b^2 + yg_b^2); Izzg_b=Izz_b + mb*(xg_b^2 + yg_b^2);
% Inertia Tensor
I_b=[Ixxg_b 0 0;0 Iyyg_b 0;0 0 Izzg_b];
%% ---INERTIA MOMENTS OF THE MAIN ROTOR--- %Inertia wrt principal axes
Ixx_mr=Izz_mr/2; Iyy_mr=Izz_mr/2; Izz_mr=(m_mr*r_mr^2)/3; % Distance mr-CoM xg_mr=0; yg_mr=0; zg_mr=Dz+d;
% Moments of Inertia referred to the base CoM
Ixxg_mr=Ixx_mr + m_mr*(yg_mr^2 + zg_mr^2); Iyyg_mr=Iyy_mr + m_mr*(xg_mr^2 + zg_mr^2); Izzg_mr=Izz_mr + m_mr*(xg_mr^2 + yg_mr^2);
% Inertia Tensor
I_mr=[Ixxg_mr 0 0;0 Iyyg_mr 0;0 0 Izzg_mr];
%%---INERTIA MOMENTS OF THE UPPER LEG SEGMENTS--- %Inertia wrt principal axes
Ixx_u=mu*((thick)^2 + (l_u)^2)/12; Iyy_u=mu*((thick)^2 + (thick)^2)/12; Izz_u=mu*((thick)^2 + (l_u)^2)/12;
% Distance u_R-CoM (Right Side)
xg_u_RF=Dx; % Right front leg xg_u_RB=-Dx;% Right back leg
yg_u_R=Dy+(l_u/2)*cos(th_u_R);
136 % Distance u_L-CoM (Left Side)
xg_u_LF=Dx; xg_u_LB=-Dx;
yg_u_L=Dy+(l_u/2)*cos(th_u_L); zg_u_L=-(Dz-d)+(l_u/2)*sin(th_u_L);
% Rotated Moments of Inertia (Right Side)
Ixx_Ru=Ixx_u;
Iyy_Ru=Iyy_u*cos(th_u_R)^2+Izz_u*sin(th_u_R)^2; Izz_Ru=Iyy_u*sin(th_u_R)^2+Izz_u*cos(th_u_R)^2; Iyz_Ru=(Iyy_u-Izz_u)*cos(th_u_R)*sin(th_u_R); Izy_Ru=Iyz_Ru;
% Rotated Moments of Inertia (Left Side)
Ixx_Lu=Ixx_u;
Iyy_Lu=Iyy_u*cos(th_u_L)^2+Izz_u*sin(th_u_L)^2; Izz_Lu=Iyy_u*sin(th_u_L)^2+Izz_u*cos(th_u_L)^2; Iyz_Lu=(Iyy_u-Izz_u)*cos(th_u_L)*sin(th_u_L); Izy_Lu=Iyz_Lu;
% Moments of Inertia referred to the CoM (Right Front Leg)
Ixxg_RFu=Ixx_Ru + mu*(yg_u_R^2 + zg_u_R^2); Iyyg_RFu=Iyy_Ru + mu*(xg_u_RF^2 + zg_u_R^2); Izzg_RFu=Izz_Ru + mu*(xg_u_RF^2 + yg_u_R^2); Ixyg_RFu=-mu*xg_u_RF*yg_u_R; Iyxg_RFu=Ixyg_RFu; Ixzg_RFu=-mu*xg_u_RF*zg_u_R; Izxg_RFu=Ixzg_RFu; Iyzg_RFu=Iyz_Ru- mu*yg_u_R*zg_u_R; Izyg_RFu=Iyzg_RFu; % Inertia Tensor
I_RFu=[Ixxg_RFu Ixyg_RFu Ixzg_RFu;Iyxg_RFu Iyyg_RFu Iyzg_RFu;Izxg_RFu Izyg_RFu Izzg_RFu];
% Moments of Inertia referred to the CoM (Right Back Leg)
Ixxg_RBu=Ixx_Ru + mu*(yg_u_R^2 + zg_u_R^2); Iyyg_RBu=Iyy_Ru + mu*(xg_u_RB^2 + zg_u_R^2); Izzg_RBu=Izz_Ru + mu*(xg_u_RB^2 + yg_u_R^2); Ixyg_RBu=-mu*xg_u_RB*yg_u_R; Iyxg_RBu=Ixyg_RBu; Ixzg_RBu=-mu*xg_u_RB*zg_u_R; Izxg_RBu=Ixzg_RBu; Iyzg_RBu=Iyz_Ru- mu*yg_u_R*zg_u_R; Izyg_RBu=Iyzg_RBu; % Inertia Tensor
I_RBu=[Ixxg_RBu Ixyg_RBu Ixzg_RBu;Iyxg_RBu Iyyg_RBu Iyzg_RBu;Izxg_RBu Izyg_RBu Izzg_RBu];
% Moments of Inertia referred to the base CoM (Left Front Leg)
Ixxg_LFu=Ixx_Lu + mu*(yg_u_L^2 + zg_u_L^2); Iyyg_LFu=Iyy_Lu + mu*(xg_u_LF^2 + zg_u_L^2); Izzg_LFu=Izz_Lu + mu*(xg_u_LF^2 + yg_u_L^2); Ixyg_LFu=-mu*xg_u_LF*yg_u_L; Iyxg_LFu=Ixyg_LFu; Ixzg_LFu=-mu*xg_u_LF*zg_u_L; Izxg_LFu=Ixzg_LFu; Iyzg_LFu=Iyz_Lu- mu*yg_u_L*zg_u_L; Izyg_LFu=Iyzg_LFu; % Inertia Tensor
I_LFu=[Ixxg_LFu Ixyg_LFu Ixzg_LFu;Iyxg_LFu Iyyg_LFu Iyzg_LFu;Izxg_LFu Izyg_LFu Izzg_LFu];
137
% Moments of Inertia referred to the base CoM (Left Back Leg)
Ixxg_LBu=Ixx_Lu + mu*(yg_u_L^2 + zg_u_L^2); Iyyg_LBu=Iyy_Lu + mu*(xg_u_LB^2 + zg_u_L^2); Izzg_LBu=Izz_Lu + mu*(xg_u_LB^2 + yg_u_L^2); Ixyg_LBu=-mu*xg_u_LB*yg_u_L; Iyxg_LBu=Ixyg_LBu; Ixzg_LBu=-mu*xg_u_LB*zg_u_L; Izxg_LBu=Ixzg_LBu; Iyzg_LBu=Iyz_Lu- mu*yg_u_L*zg_u_L; Izyg_LBu=Iyzg_LBu; % Inertia Tensor
I_LBu=[Ixxg_LBu Ixyg_LBu Ixzg_LBu;Iyxg_LBu Iyyg_LBu Iyzg_LBu;Izxg_LBu Izyg_LBu Izzg_LBu];
%% ---INERTIA MOMENTS OF THE LOWER LEG SEGMENTS--- %Inertia wrt principal axes
Ixx_l=ml*((thick)^2 + (l_l)^2)/12; Iyy_l=ml*((thick)^2 + (thick)^2)/12; Izz_l=ml*((thick)^2 + (l_l)^2)/12;
% Distance u_R-CoM (Right Side)
xg_l_RF=Dx; xg_l_RB=-Dx;
yg_l_R=Dy+(l_l/2)*cos(th_l_R);
zg_l_R=-(Dz-d)+(l_l/2)*sin(th_l_R);
% Distance l_L-CoM (Left Side)
xg_l_LF=Dx; xg_l_LB=-Dx;
yg_l_L=Dy+(l_l/2)*cos(th_l_L); zg_l_L=-(Dz-d)+(l_l/2)*sin(th_l_L);
% Rotated Moments of Inertia (Right Side)
Ixx_Rl=Ixx_l;
Iyy_Rl=Iyy_l*cos(th_l_R)^2+Izz_l*sin(th_l_R)^2; Izz_Rl=Iyy_l*sin(th_l_R)^2+Izz_l*cos(th_l_R)^2; Iyz_Rl=(Iyy_l-Izz_l)*cos(th_l_R)*sin(th_l_R); Izy_Rl=Iyz_Rl;
% Rotated Moments of Inertia (Left Side)
Ixx_Ll=Ixx_l;
Iyy_Ll=Iyy_l*cos(th_l_L)^2+Izz_l*sin(th_l_L)^2; Izz_Ll=Iyy_l*sin(th_l_L)^2+Izz_l*cos(th_l_L)^2; Iyz_Ll=(Iyy_l-Izz_l)*cos(th_l_L)*sin(th_l_L); Izy_Ll=Iyz_Ll;
% Moments of Inertia referred to the base CoM (Right Front Leg)
Ixxg_RFl=Ixx_Rl + ml*(yg_l_R^2 + zg_l_R^2); Iyyg_RFl=Iyy_Rl + ml*(xg_l_RF^2 + zg_l_R^2); Izzg_RFl=Izz_Rl + ml*(xg_l_RF^2 + yg_l_R^2); Ixyg_RFl=-ml*xg_l_RF*yg_l_R; Iyxg_RFl=Ixyg_RFl; Ixzg_RFl=-ml*xg_l_RF*zg_l_R; Izxg_RFl=Ixzg_RFl; Iyzg_RFl=Iyz_Rl- ml*yg_l_R*zg_l_R; Izyg_RFl=Iyzg_RFl; % Inertia Tensor
I_RFl=[Ixxg_RFl Ixyg_RFl Ixzg_RFl;Iyxg_RFl Iyyg_RFl Iyzg_RFl;Izxg_RFl Izyg_RFl Izzg_RFl];
138
% Moments of Inertia referred to the base CoM (Right Back Leg)
Ixxg_RBl=Ixx_Rl + ml*(yg_l_R^2 + zg_l_R^2); Iyyg_RBl=Iyy_Rl + ml*(xg_l_RB^2 + zg_l_R^2); Izzg_RBl=Izz_Rl + ml*(xg_l_RB^2 + yg_l_R^2); Ixyg_RBl=-ml*xg_l_RB*yg_l_R; Iyxg_RBl=Ixyg_RBl; Ixzg_RBl=-ml*xg_l_RB*zg_l_R; Izxg_RBl=Ixzg_RBl; Iyzg_RBl=Iyz_Rl- ml*yg_l_R*zg_l_R; Izyg_RBl=Iyzg_RBl; % Inertia Tensor
I_RBl=[Ixxg_RBl Ixyg_RBl Ixzg_RBl;Iyxg_RBl Iyyg_RBl Iyzg_RBl;Izxg_RBl Izyg_RBl Izzg_RBl];
% Moments of Inertia referred to the base CoM (Left Front Leg)
Ixxg_LFl=Ixx_Ll + ml*(yg_l_L^2 + zg_l_L^2); Iyyg_LFl=Iyy_Ll + ml*(xg_l_LF^2 + zg_l_L^2); Izzg_LFl=Izz_Ll + ml*(xg_l_LF^2 + yg_l_L^2); Ixyg_LFl=-ml*xg_l_LF*yg_l_L; Iyxg_LFl=Ixyg_LFl; Ixzg_LFl=-ml*xg_l_LF*zg_l_L; Izxg_LFl=Ixzg_LFl; Iyzg_LFl=Iyz_Ll- ml*yg_l_L*zg_l_L; Izyg_LFl=Iyzg_LFl; % Inertia Tensor
I_LFl=[Ixxg_LFl Ixyg_LFl Ixzg_LFl;Iyxg_LFl Iyyg_LFl Iyzg_LFl;Izxg_LFl Izyg_LFl Izzg_LFl];
% Moments of Inertia referred to the base CoM (Left Back Leg)
Ixxg_LBl=Ixx_Ll + ml*(yg_l_L^2 + zg_l_L^2); Iyyg_LBl=Iyy_Ll + ml*(xg_l_LB^2 + zg_l_L^2); Izzg_LBl=Izz_Ll + ml*(xg_l_LB^2 + yg_l_L^2); Ixyg_LBl=-ml*xg_l_LB*yg_l_L; Iyxg_LBl=Ixyg_LBl; Ixzg_LBl=-ml*xg_l_LB*zg_l_L; Izxg_LBl=Ixzg_LBl; Iyzg_LBl=Iyz_Ll- ml*yg_l_L*zg_l_L; Izyg_LBl=Iyzg_LBl; % Inertia Tensor
I_LBl=[Ixxg_LBl Ixyg_LBl Ixzg_LBl;Iyxg_LBl Iyyg_LBl Iyzg_LBl;Izxg_LBl Izyg_LBl Izzg_LBl];
%%---TOTAL MOMENT OF INERTIA---
139
Appendix C
Arduino Code
This Appendix includes the full code used in the prototype during the tests. The code is divided in four sections. In the first one, the library files are included and the variables are defined. The second one includes the code for initialization of the serial port, sensors and servo motors. The third section consists in the main program, which includes the program logic and decision flowchart. The last section includes all the functions that have been created and used in the main program.
/*INCLUDE LIBRARIES AND VARIABLES DEFINITION*/
//Include library files
#include <Wire.h>
#include <VL53L0X.h>
#include "I2Cdev.h" #include "MPU6050.h" #include "Kalman.h"
//Define timers and time variables
uint32_t start, finish, timer1, currMillis, prevMillis=0;
double duration, t_delay, Ts=0.05;
//Distance sensor variables VL53L0X sensor;
uint16_t sensorRange;
double filter, prevFilter, gain=0.25;
boolean approach=0;
//Force sensors and force controller variables
int sensorPinLF=1, sensorPinLB=7, sensorPinRF=2, sensorPinRB=6, tot,
sensorValueLF = 0, sensorValueLB = 0, sensorValueRF = 0, sensorValueRB = 0;
double e_f, e1_lf, e1_rf, e1_lb, e1_rb, f_pd, incr=0.002, incr2=0.0002;
double kp_lf=0.00025, kd_lf=0.000025, kp_rf=0.00025, kd_rf=0.000025,
kp_lb=0.00025, kd_lb=0.000025, kp_rb=0.00025, kd_rb=0.000025, alpha1=0.75;
boolean contactLF, contactLB, contactRF, contactRB, touchedLF, touchedLB,
touchedRF, touchedRB;
//PID variables
double e1_roll, delta_u_roll=0, e1_pitch, delta_u_pitch=0, e_roll, sp_r=0,
e_pitch, sp_p=0.02;
//SMC variables
double s_r, s_p, psi_r=0.2, psi_p=0.2, sat_s, u_sat=0, u_vel_r=0,
u_vel_p=0, Kd_r=0.1, Kd_p=0.035;
double c2r=0, c1r=0.2, c2p=0, c1p=0.05, wr, wp, u_st;
//Attitude controller Output
140
//Inertial Measurement Unit
MPU6050 imu; //Define sensor
int16_t ax, ay, az;
int16_t gx, gy, gz;
Kalman kalmanX, kalmanY; //Define Kalman object
double gyroXrate, gyroYrate, gyroX, gyroY, roll, pitch, roll2, pitch2;
double accX, accY, accZ, alpha=0.5, dt;
//Angles Initial Positions
float th_LFh_0=-3.14/2 + 1.571, th_LFk_0=-1.571, th_RFh_0=-3.14/2 + 1.571, th_RFk_0=-1.571; float th_LBh_0=-3.14/2 + 1.571, th_LBk_0=-1.571, th_RBh_0=-3.14/2 + 1.571, th_RBk_0=-1.571; double th_LFh, th_LFk, th_RFh, th_RFk, th_LBh, th_LBk, th_RBh, th_RBk; double th_LFh1, th_LFk1, th_RFh1, th_RFk1, th_LBh1, th_LBk1, th_RBh1, th_RBk1;
//Legs Initial Position
double Y_max=-0.16, Y_min=-0.035, L1=0.093, L2=0.096;
double x_lf_0=-(L1*cos(th_LFh_0)+L2*cos(th_LFh_0+th_LFk_0)),
y_lf_0=L1*sin(th_LFh_0)+L2*sin(th_LFh_0+th_LFk_0), x_lf=x_lf_0,
y_lf=y_lf_0;
double x_rf_0=L1*cos(th_RFh_0)+L2*cos(th_RFh_0+th_RFk_0),
y_rf_0=L1*sin(th_RFh_0)+L2*sin(th_RFh_0+th_RFk_0), x_rf=x_rf_0,
y_rf=y_rf_0;
double x_lb_0=-(L1*cos(th_LBh_0)+L2*cos(th_LBh_0+th_LBk_0)),
y_lb_0=L1*sin(th_LBh_0)+L2*sin(th_LBh_0+th_LBk_0), x_lb=x_lb_0,
y_lb=y_lb_0;
double x_rb_0=L1*cos(th_RBh_0)+L2*cos(th_RBh_0+th_RBk_0),
y_rb_0=L1*sin(th_RBh_0)+L2*sin(th_RBh_0+th_RBk_0), x_rb=x_rb_0,
y_rb=y_rb_0;
//Encoders positions
int enc_LFh, enc_LFk, enc_LBh, enc_LBk, enc_RFh, enc_RFk, enc_RBh, enc_RBk;
boolean landed, ledState=0, appr=0;
double incrLF, incrRF, incrLB, incrRB;
int balanced, led_pin = 14, i=20;
//Define dynamixel motors variables
#include <DynamixelWorkbench.h>
#define DXL_BUS_SERIAL1 "1" //Dynamixel on Serial1(USART1) <-OpenCM9.04
#define BAUDRATE 1000000 #define GOAL_SPEED1 150 #define GOAL_SPEED2 300 //Motors ID's #define LF_h 1 #define LF_k 2 #define LB_h 5 #define LB_k 6 #define RF_h 3 #define RF_k 4 #define RB_h 7 #define RB_k 8 DynamixelWorkbench dxl_wb; /*INITIALIZATION*/
141
void setup() {
// Initialize serial port, sensors and motors
Serial.begin(BAUDRATE);
//Serial2.begin(115200); //(Wireless)
Wire.begin(); imu.initialize();
sensor.init(); //Distance sensor
sensor.setTimeout(500); sensor.startContinuous();
dxl_wb.begin(DXL_BUS_SERIAL1, BAUDRATE); //Servo motors
servosInit(); //Initialize motors
servosSpeed(GOAL_SPEED1); //Set motors operational speed
servosLimits(); //Set motors position limits
}
/*MAIN PROGRAM*/
void loop() {
start=millis(); // read start time
distanceSensor(); // Execute sensors sub-routines
footSensors(); KalmanAngle();
// If distance>500mm put legs in flying position
if (filter>500){ flightPose(); goalAngles(); setPosition(); resetvalues(); }
// Below 500mm move legs to landing position
else if (filter<500 && filter>300){ landingPose();
goalAngles(); setPosition(); if (appr==1){
servosSpeed(GOAL_SPEED1); //Set motors speed
appr=0; } }
// If distance>300mm initialize landing procedure
else if (filter<300){ if (appr==0){
appr=1;
servosSpeed(GOAL_SPEED2); //Set motors speed
}
//Check how many legs are in ground contact?
if (tot<4){
tot=touchedLF + touchedLB + touchedRF + touchedRB; }
//Check body attitude. Is it balanced?
if (roll>-0.01 && roll<0.01 && pitch>0.01 && pitch<0.03){ balanced=balanced + 1; //Increase counter if it’s balanced
}
else {balanced=0;} //Restart counter if not //Landing started. Execute controllers. Stage 1
142
if (tot>=1 && tot<4 && landed==0){
led_blink(); //LED blinking indicates landing started
levelcontroller1(); //PD attitude controller //SMC(); //SMC controller
//STSMC(); //Super-Twisting controller
landing(); }
else if (tot==4 && landed==0){
if (balanced>=i){ //If all condition met --> landing finished
fixPosition(); //fix legs to the current position
landed=1;
led_on(); //LED on indicates landing finished
servosSpeed(GOAL_SPEED1); }
else if (balanced<i){ //keep executing level controller. Stage 2
led_blink();
levelcontroller2();//PD attitude controller //SMC(); //SMC controller //STSMC(); //Super-Twisting controller landing2(); } } else if (filter>250){ landingPose(); resetvalues(); led_off(); }
if (landed==0){ //while landing is not finished
goalFeetXY(); //execute leg kinematics
goalAngles();
setPosition(); //and convert to motors position commands
}
readmotors(); //read motors positions
finish=millis(); // read finish time
//Calculate cycle time duration
duration=(double)finish - (double)start;
/*To obtain a fixed sample time, the cycle time duration is subtracted from the sample time, and the difference is added to the code as a delay*/ t_delay=Ts*1000-duration;
if (t_delay>0){delay(t_delay);} }
/*FUNCTIONS*/
//This function provides the feet Cartesian coordinates for flight position
void flightPose(){ y_lf=Y_min; y_rf=Y_min; y_lb=Y_min; y_rb=Y_min; x_lf=-0.067; x_rf=0.067; x_lb=-0.067; x_rb=0.067; }
143 void landingPose(){ y_lf=y_lf_0; y_rf=y_rf_0; y_lb=y_lb_0; y_rb=y_rb_0; x_lf=x_lf_0; x_rf=x_rf_0; x_lb=x_lb_0; x_rb=x_rb_0; }
//This function initialises the 8 servo motors
void servosInit(){ dxl_wb.ping(LF_h); dxl_wb.ping(LF_k); dxl_wb.ping(RF_h); dxl_wb.ping(RF_k); dxl_wb.ping(LB_h); dxl_wb.ping(LB_k); dxl_wb.ping(RB_h); dxl_wb.ping(RB_k); dxl_wb.jointMode(LF_h); dxl_wb.jointMode(LF_k); dxl_wb.jointMode(RF_h); dxl_wb.jointMode(RF_k); dxl_wb.jointMode(LB_h); dxl_wb.jointMode(LB_k); dxl_wb.jointMode(RB_h); dxl_wb.jointMode(RB_k); }
/*This function reads the encoder of the 8 servo motors and returns the angular position in radians*/
void readmotors(){
th_LFh1=enc2angl(dxl_wb.itemRead(LF_h, "Present_Position")); th_LFk1=enc2angl(dxl_wb.itemRead(LF_k, "Present_Position")); th_LBh1=enc2angl(dxl_wb.itemRead(LB_h, "Present_Position")); th_LBk1=enc2angl(dxl_wb.itemRead(LB_k, "Present_Position")); th_RFh1=enc2angl(dxl_wb.itemRead(RF_h, "Present_Position")); th_RFk1=enc2angl(dxl_wb.itemRead(RF_k, "Present_Position")); th_RBh1=enc2angl(dxl_wb.itemRead(RB_h, "Present_Position")); th_RBk1=enc2angl(dxl_wb.itemRead(RB_k, "Present_Position"));
}
/*This function sets the moving speed to the 8 servo motors*/
void servosSpeed(int vel){
dxl_wb.itemWrite(LF_h, "Moving_Speed", vel); dxl_wb.itemWrite(LF_k, "Moving_Speed", vel); dxl_wb.itemWrite(RF_h, "Moving_Speed", vel); dxl_wb.itemWrite(RF_k, "Moving_Speed", vel); dxl_wb.itemWrite(LB_h, "Moving_Speed", vel); dxl_wb.itemWrite(LB_k, "Moving_Speed", vel); dxl_wb.itemWrite(RB_h, "Moving_Speed", vel);
dxl_wb.itemWrite(RB_k, "Moving_Speed", vel); }
/*This function sets the upper and lower position limits of the 8 servo motors*/
144
void servosLimits(){
//check EXCEL file: MX_AX_AngleLimits.xlsx
dxl_wb.itemWrite(LF_h, "CW_Angle_Limit", 611); dxl_wb.itemWrite(LF_h, "CCW_Angle_Limit", 963); dxl_wb.itemWrite(LF_k, "CW_Angle_Limit", 51); dxl_wb.itemWrite(LF_k, "CCW_Angle_Limit", 512); dxl_wb.itemWrite(RF_h, "CW_Angle_Limit", 61); dxl_wb.itemWrite(RF_h, "CCW_Angle_Limit", 413); dxl_wb.itemWrite(RF_k, "CW_Angle_Limit", 512); dxl_wb.itemWrite(RF_k, "CCW_Angle_Limit", 973); dxl_wb.itemWrite(LB_h, "CW_Angle_Limit", 611); dxl_wb.itemWrite(LB_h, "CCW_Angle_Limit", 963); dxl_wb.itemWrite(LB_k, "CW_Angle_Limit", 51); dxl_wb.itemWrite(LB_k, "CCW_Angle_Limit", 512); dxl_wb.itemWrite(RB_h, "CW_Angle_Limit", 61); dxl_wb.itemWrite(RB_h, "CCW_Angle_Limit", 413); dxl_wb.itemWrite(RB_k, "CW_Angle_Limit", 512); dxl_wb.itemWrite(RB_k, "CCW_Angle_Limit", 973); }
/*This function reads the pressure from the force sensors and determines the state of each foot.
For each foot, the “touched” flag is set to high if at any moment during the landing the pressure at this foot reaches the threshold value.
The “contact” flag indicates wether at the present moment, a leg is on contact or not.*/
void footSensors(){
sensorValueLF = analogRead(sensorPinLF)*alpha1+sensorValueLF*(1-alpha1); if (touchedLF==0){if (sensorValueLF>300){touchedLF=1;}}
else if (touchedLF==1){
if (contactLF==0){if (sensorValueLF>300){contactLF=1;}} else {if (sensorValueLF<300){contactLF=0; e1_lf=0;}}