 In this chapter the concept first test will be presented. Using the hardware and software described in chapter \ref{chap:exp} along with a mechanical structure, an inverted pendulum. An objective of this step is to achieve the initial conditions necessary for the application of stimuli. These initial conditions need to be achieved automatically by the system; repeatable and customizable. Another objective is to find controlling problems inherent to this concept and to the physical structure.
 
 %To create the initial conditions to apply stimuli the servos motors must adjust the ropes tensile force to a desired value and assure that both ropes have the similar tensions.
 
 \section{Inverted Pendulum}
 
 An inverted pendulum, figure \ref{figure:inverted_pendulum}, was chosen because it's simple system that only has one degree of freedom, this will help building the control necessary to keep the ropes similarly tensioned with a customizable magnitude. The ropes have to be tensioned so that the applied stimulus can be replicated more than once. The controller will only mind the sensors information, yet it is known that if the pendulum isn't in the upright position the ropes won't be similarly tensioned, the downward force will be mostly absorbed by the rigid pole and the remaining forces will pull one rope and relieve the other. This effect is aggravated as the pendulum gets closer to the horizontal. To counteract this effect elastics are used to keep the upright position and also to emulate the reaction of attempting to remain upright. The weigh is used to emulate inertia.
 
%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.5]{Imagens/pendulo.png}
% 	\caption{Inverted pendulum.}
% 	\label{figure:inverted_pendulum}
% 	\end{figure}
	
	\begin{figure}[!ht]
	\centering
	\includegraphics[scale=0.25]{Imagens/pendulo_low.jpg}
	\caption{Inverted pendulum. 1 - Weight; 2 - Tensile force sensors; 3 - Elastic units; 4 - Servo motor and sheave; 5 - Traction ropes. }
	\label{figure:inverted_pendulum}
	\end{figure}
	
%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.6]{Imagens/pendulum_forces.png}
% 	\caption{Pendulum force distribution: left picture shows the forces from the A rope perspective; right picture shows the forces from the B rope perspective.}
% 	\label{figure:pendulum_forces}
% 	\end{figure}
	
 
%  This inverted pendulum has the possibility to have elastics that balance it, as it can be seen in figure \ref{figure:inverted_pendulum}, with this the control can be tested with more safety and the stimulus can be applied to a reacting system, giving more data. Weight can also be added to the pendulum to provide more testing scenarios, see figure \ref{figure:added_mass}.
%  
%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[angle=-90]{Imagens/peso.jpg}
% 	\caption{Mass.}
% 	\label{figure:added_mass}
% 	\end{figure}
	
 To tension the ropes a sheave was designed, see appendix \ref{annex:sheave}, taking in consideration that the RX-64 servos only allow 300\degree{} rotation(in position control mode) and that PHUA's maximum range in the four planar directions is 50 mm the sheave's inner radius is 25 mm which provides a linear translation of about 131 mm for 300\degree, keeping in mind that only half of that translation can be used since the servos have to rotate clockwise (CW) and counter clockwise (CCW). This radius can't be much higher because it can hinder on it's base or other objects that in the future can be near the servo.
 
%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[angle=-90]{Imagens/roldana.jpg}
% 	\caption{Sheave.}
% 	\label{figure:roldana}
% 	\end{figure} 
 
 
%  Since this system took more time than anticipated to build and it has many non-linearities a Simulink model wasn't done and the control was right away tested on the real system. Wood was not the best option since it causes more non-linearities but it was faster and easier to construct the pendulum.
%  
%\FloatBarrier
 
 \subsection{Control with Dynamixel RX-64}
 
 With these servos there are two types of control that can be designed, position control or torque control. Even though the sheave was designed to produce 131 mm of linear translation this is not enough because of extreme situations that will require more rope than that. An example is depicted in figure \ref{figure:extreme_pendulum} this situation needs a translation of 300 mm.
 
  	\begin{figure}[!ht]
	\centering
	\includegraphics[scale=0.2]{Imagens/extreme_pendulum_v1.png}
	\caption{Pendulum at an extreme position.}
	\label{figure:extreme_pendulum}
	\end{figure} 
 
 Torque control was explored: for this, the servos have to be programmed to what Dynamixel calls ``wheel mode''. To set the servos to this mode all it needs to be done is to define as 0 the counter clock-wise and clock-wise angle limits. To make the servo produce torque a value different from 0 or 1024, and between 1 and 2047 needs to be written in the goal speed parameter (this is misleading because speed is not being controlled, only toque is controlled in the ``wheel mode''). Values between 1 and 1023 make the servos rotate CCW and values between 1025 and 2047 make the servos rotate CW.
 
 To get familiarised with this mode, a simple experiment was done: it consisted of raising a mass with different torque values. %The objective of this experiment wasn't to obtain the relation between servo torque values and real output values, because this would be time consuming since the torque output is dependent on the instruction and on applied voltage. 
 The procedure for this experiment was to fasten a servo and have it raise a cup with some weights while recording tensile force, commanded torque and load parameter. All the results were the same for every different weight lifted so only one graphic is shown in figure \ref{graphic:torque_trial}.
 
 
 \begin{figure}[!httb]\centering
  \pgfplotstableread[col sep = comma]{Graficos/torque_analysis_lite_v2.csv}\torqueanalcsv
  \tikzsetnextfilename{torqueanal}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\textwidth,
	  height=0.5\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  %ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=north west, % customize legend
	  ]
	  \addplot[mark=none, color=green, thin] table[x index = {0}, y index = {1},]{\torqueanalcsv};
	  \addplot[mark=none, color=red, thick] table[x index = {0}, y index = {2}]{\torqueanalcsv};
	  \addplot[mark=none, color=blue, thin] table[x index = {0}, y index = {3}]{\torqueanalcsv};    
	  \legend{Force (x 10 N), Commanded Torque (dynamixel untis), Torque Reading (dynamixel untis)}%, Command torque, Torque reading}
	\end{axis}
      \end{tikzpicture}
  \caption{Results from the mass raising experiments. Nine different torque values were sent to the servomotor.}
  \label{graphic:torque_trial}
\end{figure}
 
 
 
% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.25]{Imagens/torque_analisys.png}
% 	\caption{Results from the mass raising experiment.}
% 	\label{figure:torque_trial}
% 	\end{figure}
	
From sample 0 to $\approx$15000 the servo didn't raise the cup, it started to be able to raise the cup if the commanded torque was equal or higher than 105. The higher the torque the less time the trial takes and also some errors are observed in the load parameter readings as expected. This load parameter was recorded just to ensure that it can't be used for torque measurement.

With these trials it's predictable that the controller will need an integral term to increase or decrease the servo torque if the rope's tensile force is below or above the desired value since the servos can get stalled.

\subsection{Proportional and Integral Controller}

A proportional and integral controller is a control loop feedback algorithm that will be used to achieve the pendulum's state of equilibrium. The proportional term will produce an output value that is proportional to the current error between the present sensor reading and the desired tensile force value. The integrative term output will be proportional to the magnitude and duration of the error, meaning that it will be the sum of the errors between the sensor reading and the desired tensile force value. %The latter term may accumulate too much error and produce positive feedback, making the controller unstable.

These terms need to be multiplied by a constant to adjust their effect and produce values compatible with RX-64 units which are integers between 0 and 2047.

The first control algorithm tested can be seen in equation \ref{equation:first_PI}. Each servo is controlled with it's own equation, the sensor input comes from the sensor that's attached to the servo rope. Like this each servo is controlled independently from the other, but still get influenced by the other servo actions because the ropes connect them physically. $Torque(n)$ is the torque value calculated, at time $n$, that is going to be sent to the servo; $Tension_{limit}$ is the customizable value for the rope tension; $Sensor(n)$ is the measured tensile force on the servo's rope, at time $n$; $K_P$ is a constant to convert the proportional term to dynamixel values and to adjust the term's influence on the output ($Torque(n)$); $K_I$ is a constant to convert the integral term to dynamixel values and to adjust the term's influence on the output ($Torque(n)$).

\begin{equation}
 Torque(n) = K_P.(Tension_{limit} - Sensor(n)) + K_I.\sum_{i=0}^{n}(Tension_{limit} - Sensor(n_i)) \label{equation:first_PI}
\end{equation}

This controller caused positive feedback because it accumulated too much error making the system unstable. The rotation direction is based on the output being greater or lesser than zero; this works well when the ropes are both below or above the tension limit, but if one rope is tensioned above the limit and the other is below the control will aggravate this state of unbalance. To understand this effect better, the control equation can be broken down to a simple set of conditions, if the sensor value is below the limit, the servo has to wind the rope to raise the tension and if the sensor value is above the limit the servo has to unwind rope to lower the tension. This means that the control for each servo needs one more term that considers the state of the other servo's sensor.

A new term was designed to oppose the positive feedback that takes in consideration the sensor of the opposite servo and acts like a proportional term. This term is the difference between the servo's rope tensile force ($Sensor(n)$) and the opposite servo's rope tensile force ($Sensor_{opposite}(n)$), at time $n$; $K_P$ is a constant to convert the term to dynamixel values and to adjust the term's influence on the output ($Torque(n)$). Equation \ref{equation:second_PI} shows the controller with the new term.

\begin{align}
 Torque(n) &= K_{P1}.(Tension_{limit} - Sensor(n)) \nonumber\\
 &+ K_{P2}.(Sensor(n) - Sensor_{opposite}(n)) \nonumber \\
 &+ K_I.\sum_{i=0}^{n_i}(Tension_{limit} - Sensor(n_i)) \label{equation:second_PI}
\end{align}

The result of this controller can be seen on figure \ref{graphic:PI_rx_64}. It's not a smooth control because of the non-linearities of the physical system and because of the resolution and internal friction of the servos. It can be seen, between some samples, that even though the servo torque was being raised by the controller the tension kept decreasing until the torque could overcome internal friction and caused the oscillation in the sensor values. Another problem inherent to this dynamixel control mode is that when the applied torque has to change it's direction the tension on the rope pulls the servo causing instability, this can be seen in figure \ref{graphic:torque_dir_change}.



\begin{figure}[!ht]\centering
  \pgfplotstableread[col sep = comma ]{Graficos/PIcontrol_lite.csv}\PIcontrolcsv
  \subfloat[Servo 1 side.]{
  \tikzsetnextfilename{PIcontrol1}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\linewidth,
	  height=0.4\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=south east, % customize legend 
	  ]
	  \addplot[mark=none, color=red, ultra thick] table[x index = {0}, y index = {1}]{\PIcontrolcsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {2}]{\PIcontrolcsv};
	  \addplot[mark=none, color=green, thick] table[x index = {0}, y index = {3}]{\PIcontrolcsv};
	  \legend{Desired tensile force, Sensor 1 reading, Servo 1 commanded torque x 0.1}
	\end{axis}
      \end{tikzpicture}
      %\label{subfigure:ramp_actforce}
  } %subfloat
  
   \subfloat[Servo 2 side.]{
   \tikzsetnextfilename{PIcontrol2}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\linewidth,
	  height=0.4\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=south east, % customize legend 
	  ]
	  \addplot[mark=none, color=red, ultra thick] table[x index = {0}, y index = {1}]{\PIcontrolcsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {4}]{\PIcontrolcsv};
	  \addplot[mark=none, color=green, thick] table[x index = {0}, y index = {5}]{\PIcontrolcsv};
	  \legend{Desired tensile force, Sensor 2 reading, Servo 2 commanded torque x 0.1}
	\end{axis}
      \end{tikzpicture}
      %\label{subfigure:ramp_angle}
} %subfloat
  \caption{Controller test.}
  \label{graphic:PI_rx_64}
  
\end{figure}
%\let\PIcontrolcsv\relax


% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.35]{Imagens/PI_rx_64.png}
% 	\caption{Controller test.}
% 	\label{figure:PI_rx_64}
% 	\end{figure}



\begin{figure}[!ht]\centering
  \pgfplotstableread[col sep = comma ]{Graficos/dir_change_v3.csv}\dirchangecsv
  \subfloat[Servo 1 side.]{
  \tikzsetnextfilename{dirchange1}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\linewidth,
	  height=0.4\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=south west, % customize legend 
	  ]
	  \addplot[mark=none, color=yellow, ultra thick] table[x index = {0}, y index = {7}]{\dirchangecsv};
	  \addplot[mark=none, color=red, thick] table[x index = {0}, y index = {1}]{\dirchangecsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {2}]{\dirchangecsv};
	  \addplot[mark=none, color=green, thick] table[x index = {0}, y index = {3}]{\dirchangecsv};
	  \legend{Desired tensile force, CCW Torque x 0.1, CW Torque x 0.1, Sensor 1 reading}
	\end{axis}
      \end{tikzpicture}
      %\label{subfigure:ramp_actforce}
  } %subfloat
  
   \subfloat[Servo 2 side.]{
   \tikzsetnextfilename{dirchange2}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\linewidth,
	  height=0.4\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=south west, % customize legend 
	  ]
	  \addplot[mark=none, color=yellow, ultra thick] table[x index = {0}, y index = {7}]{\dirchangecsv};
	  \addplot[mark=none, color=red, thick] table[x index = {0}, y index = {4}]{\dirchangecsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {5}]{\dirchangecsv};
	  \addplot[mark=none, color=green, thick] table[x index = {0}, y index = {6}]{\dirchangecsv};
	  \legend{Desired tensile force, CCW Torque x 0.1, CW Torque x 0.1, Sensor 2 reading}
	\end{axis}
      \end{tikzpicture}
      %\label{subfigure:ramp_angle}
} %subfloat
  \caption{Effects of the torque direction change.}
  \label{graphic:torque_dir_change}
  
\end{figure}

% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.35]{Imagens/torque_dir_change.png}
% 	\caption{Effects of the torque direction change.}
% 	\label{figure:torque_dir_change}
% 	\end{figure}
	
Because of this problem these servos aren't ideal to control this system. So the servos were switched with the Dynamixel MX-106R because they were available, have more functions, better resolution and can be fitted in the physical system, their specifications can be seen in section \ref{section:MX106}.
 
 \section{Dynamixel MX-106R} \label{section:MX106}
 
 In this section, the dynamixel MX-106R will be briefly presented, emphasizing it's advantages over the RX-64 that are relevant to this control. Figure \ref{figure:mx106} shows the new servos. Table \ref{table:mx_specs} shows some of the servo specifications; compared to the previous servos these have more detailed information. One of the best improvement is the contactless absolute encoder that allows a full turn; this means that position control is now an option. The communication with these servos is done the same way as the others.
 
 The MX series allows three types of control: wheel mode, joint mode and multi-turn mode. 
 
 While on wheel mode it is possible to control torque and the servo will behave just like RX-64's wheel mode, or it is possible to control its speed and the servo will rotate in the commanded direction maintaining the desired speed.
 
 Joint mode is not very interesting, the servos move to position commands restricted to 360\degree.
 
 The more interesting mode that the MX series offers is the multi-turn which allows the servo to rotate more than one turn while controlling its position, more specifically: seven turns in each direction if the resolution divider parameter is set to one; fourteen turns in each direction if the resolution divider parameter is set to two; twenty one turns in each direction if the resolution divider parameter is set to three and twenty eight turns in each direction if the resolution divider parameter is set to four.
 
 
 	\begin{figure}[!ht]
	\centering
	\includegraphics{Imagens/mx-106r.jpg}
	\caption{Dynamixel MX-106R servo.}
	\label{figure:mx106}
	\end{figure}

	
\begin{table}[!ht]\centering
\ra{1.3}
\caption{Dynamixel MX-106R specifications.}
\label{table:mx_specs}
\begin{tabular}{@{}lllll@{}}\toprule
Weight(g)                       & \multicolumn{3}{l}{153g}                                             \\ 
Dimension(mm)                   & \multicolumn{3}{l}{40.2 $\times$ 65.1 $\times$ 46}                   \\ 
Gear Reduction Ratio            & \multicolumn{3}{l}{225 : 1}                                          \\ 
Applied Voltage (V)             & 11.1                    & {|}12                    & {|}14.8         \\ 
Consumed Current (A)            & 4.8                     & {|}5.2                   & {|}6.3          \\ 
Stall Torque (N.m)              & 8.0                     & {|}8.4                   & {|}10.0         \\ 
No Load Speed (rpm)             & 41                      & {|}45                    & {|}55           \\ 
Resolution (\degree)            & \multicolumn{3}{l}{0.088}                                            \\ 
Speed (rpm/unit)                & \multicolumn{3}{l}{$\sim$ 0.114}                                     \\ 
Running Degree (\degree)        & \multicolumn{3}{l}{0 $\sim$ 360, Endless Turn}                       \\ 
Voltage (V)                     & \multicolumn{3}{l}{10 $\sim$ 14.8 (Recommended 12)}                  \\ 
Running Temperature (\degree C) & \multicolumn{3}{l}{-5 $\sim$ +80}                                    \\ 
Standby Current (mA)            & \multicolumn{3}{l}{100}                                              \\ 
Position Sensor                 & \multicolumn{3}{l}{Contactless absolute encoder (12BIT,360\degree )} \\ 
\bottomrule
\end{tabular}
\end{table}

%\FloatBarrier

\subsection{Learning Dynamixel MX-106R} \label{subsection:MX106_learn}

Before trying to use these new servos to control the system some small experiments were carried out to learn how to use some of the MX's modes. This revealed that the desired Multi-turn mode wasn't functioning properly. After a quick search through some online forums, one person's comment stated that this mode could only be used with the latest dynamixel firmware. Robotis was contacted and confirmed that the firmware version required for this purpose was 36, these servos had the version 31. Coincidently in LAR there are two robotic arms controlled with dynamixel servos that possess two usb2dynamixel, seen in figure \ref{figure:Usb2dynamixel}, which are needed to upgrade the firmware. Using Roboplus, the upgrade was quickly done without any problem. Roboplus is a software given by Robotis that communicates and controls their products, but it needs their physical interface (usb2dynamixel) to communicate with the servos.

 	\begin{figure}[!ht]
	\centering
	\includegraphics{Imagens/USB2dynamixel.jpg}
	\caption{Usb2dynamixel.}
	\label{figure:Usb2dynamixel}
	\end{figure}

%After this another simple experiment was done to see if the servos were capable of performing all functions which concluded that they are able to perform the functions stated on their online manual \cite{mx106_manual}.
	
After this, another simple experiment was done to see if the servos were capable of performing all functions which concluded that they are able to perform the functions stated on their online manual. This experiment consisted of using the DETI dynamixel libraries to control the servos in the modes presented in section \ref{section:MX106}.

%\FloatBarrier

\subsection{Control with Dynamixel MX-106R} \label{subsection:MX106_control}

To control MX-106's with the previous algorithm, equation \ref{equation:second_PI}, only the constants ($K_{P1}$, $K_{P2}$ and $K_{I}$) were adjusted so the output would be position increments (\textit{Position}$_{increment}(n)$); basically, they were all lowered. These increments can be positive or negative, a positive increments means that the servo should rotate CCW and a negative increment means the opposite. The new equation can be seen in \ref{equation:position_PI}. %$Posiotion_{increment}(n)$ _{increment}(n)

  \begin{align}
  Position_{increment}(n) &= K_{P1}.(Tension_{limit} - Sensor(n)) \nonumber \\
  &+ K_{P2}.(Sensor(n) - Sensor_{opposite}(n)) \nonumber \\
  &+ K_I.\sum_{i=0}^{n}(Tension_{limit} - Sensor(n_i))  
  \label{equation:position_PI}
  \end{align}


To adjust this controller, several trials were done, until a good balance between the gains was found. $K_{P1}$ has an important role to the systems response speed, $K_I$ makes the system find the desired force and finally $K_{P2}$ gives the system the right direction to find balance. On an initial phase if the pendulum is tilted, producing a high force difference between the sensors, $K_{P2}$ will make the servo with the higher tension wind the rope bringing it to the upright position. While the other two gains only consider their servo tension and produce the necessary output to bring the tension to the desired value. For these trials, the desired tension was 5 N because if an overshoot occurs it's less likely to damage the load cells.



The first trials adjusting the gains produced the outcome shown in figure \ref{graphic:bad_behaviour}. It never achieved the desired 5 N tension because it would overshoot.


%This behaviour seemed to be provoked by outdated information and the only cause for this is the filtering process so filtered and unfiltered tension was collected and analysed during one of these overshoots. Figure \ref{graphic:filt_VS_unfilt} reveals that the delay provoked by the filter caused, in this example, a momentary tensile force error of about 4 N. This is unacceptable to control a system that requires low response time so the filter was removed and the experiments were carried out with the same procedure.


 \begin{figure}[!httb]\centering
  \pgfplotstableread[col sep = comma]{Graficos/BAD_behavior_v2.csv}\badbehaviorcsv
  \tikzsetnextfilename{BADbehaviour}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\textwidth,
	  height=0.5\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=north east, % customize legend
	  ]
	  \addplot[mark=none, color=green, thin] table[x index = {0}, y index = {1},]{\badbehaviorcsv};
	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\badbehaviorcsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {3}]{\badbehaviorcsv};    
	  \legend{Sensor 1 reading, Sensor 2 reading, Desired tensile force}%, Command torque, Torque reading}
	\end{axis}
      \end{tikzpicture}
  \caption{Example of the system's behaviour with the first trials.}
  \label{graphic:bad_behaviour}
\end{figure}

%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.3]{Graficos/effect_filter.eps}
% 	\caption{Example of the system's bad behaviour.}
% 	\label{figure:bad_behaviour}
% 	\end{figure}	
	

% \begin{figure}[!httb]\centering
%   \pgfplotstableread[col sep = comma]{Graficos/filtVSunfilt.csv}\filtVSunfiltcsv
%   \tikzsetnextfilename{filtVSunfilt}
%       \begin{tikzpicture}
% 	\begin{axis}[
% 	  width=1\textwidth,
% 	  height=0.5\textheight,
% 	  font=\scriptsize,
% 	  xmajorgrids,
% 	  ymajorgrids,
% 	  xlabel = Sample,
% 	  ylabel = Force (N),
% 	  %ylabel style={overlay, anchor=north,},
% 	  axis on top, % descriptions over filled area
% 	  legend pos=north east, % customize legend
% 	  ]
% 	  \addplot[mark=none, color=green, thin] table[x index = {0}, y index = {1},]{\filtVSunfiltcsv};
% 	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\filtVSunfiltcsv};
% 	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {3}]{\filtVSunfiltcsv};    
% 	  \legend{Filtered Sensor reading, Unfiltered sensor reading, Filtered - Unfiltered}%, Command torque, Torque reading}
% 	\end{axis}
%       \end{tikzpicture}
%   \caption{Filtered and unfiltered tension during an overshoot.}
%   \label{graphic:filt_VS_unfilt}
% \end{figure}

%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.3]{Graficos/zoom_filtVSunfilt.eps}
% 	\caption{Filtered and unfiltered tension during an overshoot.}
% 	\label{figure:filt_VS_unfilt}
% 	\end{figure}
	
After some more trials, the final result that was achieved can be seen in figure \ref{graphic:best_controller_until_now} using the gain values seen in table \ref{table:gains}. All the trials started with the pendulum at the position represented in figure \ref{figure:extreme_pendulum} that's why the tension is above the 5 N limit. It still has problems because the tension in the ropes wasn't matched and the start wasn't smooth has it should be.
	
%	The values for the gains can be seen in table \ref{table:gains}.
	
\begin{table}[!ht]\centering
\ra{1.3}
\caption{Controller gains obtained and adjusted by trial and error.}
\label{table:gains}
\begin{tabular}{@{}ll@{}}\toprule
Gain             &  Value                \\ \midrule
$K_{P1}$         &  0.1                  \\ 
$K_{P2}$         &  0.7                  \\ 
$K_I$            &  0.00001              \\ 
\bottomrule
\end{tabular}
\end{table}

 \begin{figure}[!httb]\centering
  \pgfplotstableread[col sep = comma]{Graficos/BEST_result_v2.csv}\bestresultcsv
  \tikzsetnextfilename{bestresult}
      \begin{tikzpicture}
	\begin{axis}[
	  width=1\textwidth,
	  height=0.5\textheight,
	  font=\scriptsize,
	  xmajorgrids,
	  ymajorgrids,
	  xlabel = Time (s),
	  ylabel = Force (N),
	  %ylabel style={overlay, anchor=north,},
	  axis on top, % descriptions over filled area
	  legend pos=north east, % customize legend
	  ]
	  \addplot[mark=none, color=green, thin] table[x index = {0}, y index = {1},]{\bestresultcsv};
	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\bestresultcsv};
	  \addplot[mark=none, color=blue, thick] table[x index = {0}, y index = {3}]{\bestresultcsv};    
	  \legend{Sensor 1 reading, Sensor 2 reading, Desired tensile force}%, Command torque, Torque reading}
	\end{axis}
      \end{tikzpicture}
  \caption{Best result achieved with trial and error method.}
  \label{graphic:best_controller_until_now}
\end{figure}


%  	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.3]{Graficos/best_controller_until_now.eps}
% 	\caption{Best result achieved with trial and error method.}
% 	\label{figure:best_controller_until_now}
% 	\end{figure}	
	
%Since it was taking too much time to tune the controller like this, without any assurance that an optimal result could be found, it was decided to make a simulation of the system to assess if it is possible to control it with the requisitions of this project. The next chapter will cover the simulation.
%\ref{annex:simulation}

In the end, it is clear that this controller isn't the optimal, yet it proved useful to identify problems that the concept brings to its control. The fact that the system is coupled makes it very difficult to achieve equal tensile force on the ropes. As it can be seen in figure \ref{graphic:best_controller_until_now} and from the experience tuning the controller if the ropes start to be tensioned with different forces the controller isn't capable of matching the forces. Another problem is the speed of the controller that sometimes times produces a spike in the tensile force as it can be seen in the first two seconds of figure \ref{graphic:best_controller_until_now}.
So, now that the problems of the concept's control have been identified and the controller can achieve a state of balance where the ropes have stable and similar tensile force the next step is to study the application of stimuli.

