In this chapter some specifications about PHUA are shown to complete its presentation started in chapter \ref{chap:intro}. As suggested before the stimuli are pull forces and they need to be measured so they can be controlled. To accomplish this several servomotors need to be connected in a bus with the possibility of force or torque control. To measure the stimuli tensile force sensors are needed with a measurement range close to the stimuli magnitude. Finally a software has to communicate with the motors and sensors interfaces. All these devices and software will be presented in this chapter.
  
  % -------------------------------------------------------------
  % phua
  % -------------------------------------------------------------
  \section{University of Aveiro Humanoid Project}\label{sec:phua}
  

  The platform has 27 DOF, visible in figure \ref{figure:kinematic_chain}. HITEC\textregistered{} digital and analogue servomotors, combined with elastic elements, actuate 25 of the joints which are arranged as in figure \ref{figure:kinematic_chain} with the servomotors assembled as in table \ref{table:phua_servos}. The robot uses a belt and pulley adjustable transmission system on the legs and torso, providing torque to the joints with higher demand. The platform is also equipped with a set of 8 force sensors, 4 on each foot, from which weight distribution can be extracted. 
  When assembled, it weights about 6 kg and has an approximated height of 65 cm \cite{Mec2012}.
  
  
%     \begin{table}[!ht]\centering
%   \ra{1.4}
%   \caption{Instruction for Dynamixel.}
%   \label{table:instructions}
%   \begin{tabular}{@{}lp{0.4\linewidth}c@{}}\toprule
%   Name       & Function				                                                                                      & Number of Parameters \\ 
%   \cmidrule{1-3}
  
  
  \begin{table}[!ht]\centering
  \ra{1.3}
  \caption{PHUA platform's degrees-of-freedom and installed servomotors.}
  \label{table:phua_servos}
  \begin{tabular}{@{}lll@{}}\toprule
  Joint    & Number of DOF's   & Servomotor                                                                        \\ \midrule
  Toe      & 1($\times$2)      & (none)                                                                            \\ \hdashline
  Ankle    & 2($\times$2)      & HSR-5980SG                                                                        \\ \hdashline
  Knee     & 1($\times$2)      & HSR-5980SG                                                                        \\ \hdashline
  Hip      & 3($\times$2)      & HSR-5980SG                                                                        \\ \hdashline
  Trunk    & 3                 & \begin{tabular}[c]{@{}c@{}}HSR-5980SG($\times$2)\\ HSR-8498SG($\times$1)\end{tabular} \\ \hdashline
  Shoulder & 3($\times$2)      & \begin{tabular}[c]{@{}c@{}}HSR-5980SG\\ HSR-5498SG\\ HS-82MG\end{tabular}         \\ \hdashline
  Elbow    & 1($\times$2)      & HSR-5498SG                                                                        \\ \hdashline
  Neck     & 2                 & \begin{tabular}[c]{@{}c@{}}HSR-5498SG\\ HS-5056MG\end{tabular}                    \\ \hdashline
  Total    & 27                & --                                                                                 \\ 
  \bottomrule
  \end{tabular}
  \end{table}
  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics{Imagens/kinematic_chain.png}
	  \caption{PHUA kinematic chains \cite{Mec2012}.}
	  \label{figure:kinematic_chain}
	  \end{figure}
  
  Table \ref{table:phua_range} details the robot's joint anthropometric ranges and the total angular range, measured from the home position (in degrees), defined as the robot with all limbs at full stretch downwards, with the torso upright and a zero tilting in the head. 
  
  A simulation of PHUA on V-REP has shown that it's hip can move +/-50 mm in both sagittal and lateral direction and -50 mm vertically\cite{Barros2014}.

  
  \begin{table}[!ht]\centering
  \ra{1.3}
  \caption{PHUA robot degrees-of-freedom range limitation.}
  \label{table:phua_range}
  \begin{tabular}{@{}llll@{}}\toprule
  DOF                             & Minimum (\degree) & Maximum (\degree) & Total Range (\degree) \\ \cmidrule{1-4}
  Head Tilt                       & 0                 & 10                & 10                    \\ 
  Shoulder Flexion/Extension      & -45               & 135               & 180                   \\ 
  Shoulder Abduction/Adduction    & 0                 & 180               & 180                   \\ 
  Elbow Flexion/Extension         & 0                 & 120               & 120                   \\ 
  Torso Rotation                  & -90               & 90                & 180                   \\ 
  Torso Flexion/Extension         & -15               & 90                & 105                   \\ 
  Torso Lateral Flexion/Extension & -90               & 90                & 180                   \\ 
  Ankle Inversion/Eversion        & -45               & 30                & 75                    \\ 
  Ankle Extension/Flexion         & -20               & 40                & 60                    \\ 
  Knee Extension/Flexion          & 0                 & 130               & 130                   \\ 
  Hip Adduction/Abduction         & -40               & 45                & 85                    \\ 
  Hip Extention/Flexion           & -30               & 120               & 150                   \\ 
  \bottomrule
  \end{tabular}
  \end{table}
  
  %\FloatBarrier
  
  
  
  \section{Dynamixel RX-64}

	  The servomotor used for the initial phase of the system's prototype is shown in figure \ref{figure:rx-64}. It was already present in the lab, ready to be used and also it has enough power to apply stimuli to the robot; some specifications can be seen in table \ref{table:Dynamixel_specs}. With a supplied voltage of 15 V the servos can produce a stall torque of approximately 632 \si{N.cm} which is more than enough to lift PHUA's structure, if the radius of the pulling mechanism doesn't exceed 10 cm and minding PHUA's weight.
	  
	  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics{Imagens/rx-64.png}
	  \caption{Dynamixel RX-64 servo}
	  \label{figure:rx-64}
	  \end{figure}
	  
	  % @ 18V -> 77.2kgf.cm -> 757N.cm -> 303N (na roldana r=2.5cm) isto é stall torque em movimento ele nao consegue tanto!! mas o robot pesa <7Kg ou seja, 70N chega para levantar o robot!!!
	  % @ 15V -> 64.4kgf.cm -> 632N.cm -> 253N (na roldana r=2.5cm)

	 
	    \begin{table}[!ht]\centering
	    \ra{1.4}
	    \caption{Dynamixel RX-64 specifications.}
	    \label{table:Dynamixel_specs}
	    \begin{tabular}{@{}p{0.35\linewidth}ll@{}}\toprule
	    Weight(g)                                & \multicolumn{2}{l}{125}                                                             \\ \hdashline
	    Dimension(mm)                            & \multicolumn{2}{l}{40.2 x 61.1 x 41.0}                                              \\ \hdashline
	    Gear Reduction Ratio                     & \multicolumn{2}{l}{1/200}                                                           \\ \hdashline
	    Applied Voltage(V)                       & at 15                                      &| at 18                                 \\ \hdashline
	    Final Reduction Stopping Torque (kgf.cm) & 64.4                                       &| 77.2                                  \\ \hdashline
	    Speed (s/60\degree)                      & 0.188                                      &| 0.157                                 \\ \hdashline
	    Resolution (\degree)                     & \multicolumn{2}{l}{0.29}                                                            \\ \hdashline
	    Running Degree                           & \multicolumn{2}{l}{300\degree, Endless Turn}                                        \\ \hdashline
	    Voltage (V)                              & \multicolumn{2}{l}{12 $\sim$ 21 (Recommended: 18)}                                  \\ \hdashline
	    Max Current (mA)                         & \multicolumn{2}{l}{1200}                                                            \\ \hdashline
	    Running Temperature (\degree C)          & \multicolumn{2}{l}{-5 $\sim$ +85}                                                   \\ \hdashline
	    Command Signal                           & \multicolumn{2}{l}{Digital Packet}                                                  \\ \hdashline
	    Protocol                                 & \multicolumn{2}{p{0.5\linewidth}}{RS485 Asynchronous Serial Communication (8bit,1stop, No Parity)} \\ \hdashline
	    Link (Physical)                          & \multicolumn{2}{l}{RS485 Multi Drop Bus}                                            \\ \hdashline
	    ID                                       & \multicolumn{2}{l}{254 ID (0 $\sim$ 253)}                                           \\ \hdashline
	    Communication Speed                      & \multicolumn{2}{l}{7343bps $\sim$ 1 Mbps}                                           \\ \hdashline
	    Sensing \& Measuring                     & \multicolumn{2}{p{0.5\linewidth}}{Position, Temperature, Load, Input Voltage, etc.}                \\ \hdashline
	    Material Quality                         & \multicolumn{2}{p{0.5\linewidth}}{Full Metal Gear, Engineering Plastic Body}                       \\ \hdashline
	    Motor                                    & \multicolumn{2}{l}{Maxon RE-MAX}                                                    \\ \hdashline
	    Standby Current                          & \multicolumn{2}{l}{50}                                                              \\ 
	    \bottomrule
	    \end{tabular}
	    \end{table}


    To communicate with the servos, Main Controller (\ref{sub-sec-interface}) has to send an Instruction Packet and receive a Status Packet, the structure of these packets can be seen in figure \ref{figure:inst-pack} and  \ref{figure:retu-pack}, respectively. Each box represents a byte; for both Packets, the four first bytes are the same. The first and second are the starting bytes, the third is the identification number of the servo, and the fourth is the length of the packet. For the Instruction Packet, the sixth until the last but one bytes, are the address of the parameters which the controller wants to read or write accordingly with the fifth byte which is the instruction. There are seven types of instructions and they can be seen in table \ref{table:instructions}. The last byte is used to check if the packet is damaged during communication. For the remaining bytes of Status Packet, the fifth byte displays if an error has occurred during the operation of the servo and which error it is. The sixth byte until the last but one, returns the value of the parameter requested by the Instruction. The last byte has the same usage as the checksum byte of the other packet.


  \begin{table}[!ht]
  \centering
  \ra{1.4}
  \caption{Instruction for Dynamixel.}
  \label{table:instructions}
  \begin{tabular}{@{}lp{0.4\linewidth}l@{}}\toprule
  Name       & Function				                                                                                        & Number of Parameters \\ 
  \midrule
  PING       & No execution. It is used when controller is ready to receive Status Packet                                               & 0                    \\ \hdashline
  READ DATA  & This command reads data from RX-64                                                                                       & 2                    \\ \hdashline
  WRITE DATA & This command writes data to RX-64                                                                           & 2 or more            \\ \hdashline
  REG WRITE  & It is similar to WRITE DATA, but it remains in the standby state without being executed until the ACTION command arrives & 2 or more            \\ \hdashline
  ACTION     & This command initiates motions registered with REG WRITE                                                                 & 0                    \\ \hdashline
  RESET      & This command restores the state of RX-64 to the factory default setting                                                  & 0                    \\ \hdashline
  SYNC WRITE & This command is used to control several RX-64 simultaneously at a time                                                   & 4 or more            \\
  \bottomrule
  \end{tabular}
  \end{table}
	  
	  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics{Imagens/instru_packet.png}
	  \caption{Structure of Instruction Packet sent to Dynamixel motors.}
	  \label{figure:inst-pack}
	  \end{figure}

	  \begin{figure}[!ht]
	  \centering
	  \includegraphics{Imagens/return_packet.png}
	  \caption{Structure of Status Packet sent by Dynamixel motors.}
	  \label{figure:retu-pack}
	  \end{figure}

	  To view more detailed information about RX-64 and how to use it, please refer to Dynamixel RX-64 Manual\cite{Robot}.
	  
	  Yet RX-64 manual doesn't cover some important technical features, which are pointed out by E. Tira-Thompson \cite{Tira-Thompson2009}. In this article it's mentioned that Present Speed and Present Load parameters are only updated every 130 ms, furthermore these parameters have some error.
	  
	  From the manufacturer website it's possible to gather more important information about the motors. Load is only useful to discern which direction the torque is being applied. So, it will not be possible to use the Present Load parameter to measure the torque being outputted by the servos. Also it's advised that for stable motions, maximum load can't be higher than 1/5 of stall torque.
	  
	  An important aspect of these servos is their ability to be set to motion with either position or torque control; the latter assumes free spin mode is selected, where the engine is free to turn more than 360 degrees, whereas position control limits the motion to 300 degrees, as is depicted in figure \ref{figure:motion_range}.
	  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.5]{Imagens/angle_restriction.png}
	  \caption{Position control angle restrictions of RX-64 servomotor.}
	  \label{figure:motion_range}
	  \end{figure}



%\FloatBarrier

  \section{DETI Dynamixel Interface} \label{sub-sec-interface}
  As encouraged by ROBOTIS \cite{Robot} the main controller was custom made, instead of being bought, by the Department of Electronics, Telecommunications and Informatics (DETI), to use the servos without restrictions on Linux operating system. It supports RS485 Universal asynchronous receiver/transmitter (UART) because RX-64 uses a Multi-Drop Link method which connects several motors to a Node by using Half Duplex UART.

  It has a FTDI FT232RQ USB to UART interface, responsible for the communication between the computer and PIC32MX795F. The speed of this communication is by default set at 230400 bps. The microprocessor is responsible for arranging the messages accordingly to the Dynamixel protocol (see figures \ref{figure:inst-pack} and \ref{figure:retu-pack}), so that the servos can read the instruction packets and the computer can view the requested information sent by the Dynamixels in the return packet. A MAX3362EKA$\#$T allows the RS485 communication between the microcontroller and the servos, with a speed of 1000000 bps. The board is supplied with 5 V, which is reduced to 3.3 V by a MCP1703 linear regulator.

  This interface was built to control the Dynamixel servos series AX that consume less current then the RX series, so the interface was able to control and supply power to the AX motors. But, when building the interface, the designers made it capable of controlling other servo series of this brand just not powering them. To use only one power supply a 7805CT voltage regulator was connected to the interface, as seen in figure \ref{figure:inter_tension}. The schematic for the voltage regulator board can be seen in figure \ref{figure:voltreg_board}. Figure \ref{figure:inter_connect} describes how to connect the power supply to the interface and the data cable that connects to the servo, the connection to the computer is done with an USB-A to USB-B cable shown in figure \ref{figure:inter_tension} (grey cable).

	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=1.2]{Imagens/interface_tension_regulator_v3.png}
	  \caption{Deti Dynamixel Interface and voltage regulator.}
	  \label{figure:inter_tension}
	  \end{figure}
	  
	  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.4]{Imagens/regulador__.png}
	  \caption{Voltage regulator board schematic.}
	  \label{figure:voltreg_board}
	  \end{figure}

	  \begin{figure}[!ht]
	  \centering
	  \includegraphics{Imagens/interface_conections_front_view_v1.png}
	  \caption{Deti Dynamixel Interface connections used.}
	  \label{figure:inter_connect}
	  \end{figure}

	  
  To protect the DETI Dynamixel Interface a case was designed using the modelling tool Catia V5, shown in figure \ref{figure:caixa_exploded} and then a 3-D printing machine printed the case in thermoplastic (polylactic acid or PLA). The protection case is assembled with four screws that are inserted in the bottom part, figure \ref{figure:case} depicts the plastic case.

	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.8]{Imagens/caixa_exploded.png}
	  \caption{CAD assembly of the Deti Dynamixel Interface protection case.}
	  \label{figure:caixa_exploded}
	  \end{figure}
  
	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.2]{Imagens/interface_assemblled_v1.png}
	  \caption{Deti Dynamixel Interface protection case.}
	  \label{figure:case}
	  \end{figure}

%\FloatBarrier


  \section{Load Sensors}

  The contextualization for this section is that a cheap, easy and reliable tensile force measuring system was needed. The measurements need to be a digital signal readable by the software. Tension cells have very good construction and signal quality but their prices are too high and still require an analogue to digital signal converter, shown in figure \ref{figure:tensioncell}. Strain gauges, shown in figure \ref{figure:straingage} are cheaper than tension cells but require the dimensioning of an appropriate Wheatstone bridge, analogue to digital conversion and a rigid material to measure strain, which is a very time costly solution. After some more thought on a possible solution it was realised that some spare load cells were available in the Laboratory of Automation an Robotics (LAR) at University of Aveiro, and with a simple mechanism (\ref{subsection:tension_mechanism}) tensile force could be measured. So, this is why load cells are being presented to measure tensile force.

  \begin{figure}[!ht]
	  \centering
	  \subfloat[APPLIED Measurements DDE series tension cell.]{
	  \includegraphics[scale=0.7]{Imagens/dde.jpg}
	  }\qquad
	  \subfloat[APPLIED Measurements DSCUSB Digitiser module.]{
	  \includegraphics[scale=0.4]{Imagens/dscusb.png}
	  }
	  \caption{Tension cell.}
	  \label{figure:tensioncell}
  \end{figure}

  	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.2]{Imagens/straingage.png}
	  \caption{Micro Measurements Strain Gauge.}
	  \label{figure:straingage}
	  \end{figure}
  
  The load cells are Interface's LBS-5 and LBS-10, with a weight limit of 5 and 10 pounds of force respectively and their specifications can be seen in table \ref{table:cell_specs}.


  \begin{table}[!ht]\centering
  \ra{1.3}
  \caption{LBS-5 and LBS-10's characteristics \cite{lbs}.}
  \label{table:cell_specs}
  \begin{tabular}{@{}lll@{}}\toprule
  Characteristics                    & LBS-5    & LBS-10   \\ \cmidrule{1-3}
  Nominal Load (lbs)                 & 5        & 10       \\ 
  (N)                                & 22.2     & 44.4     \\ 
  Excitation, nom (V)                & 5        & 5        \\ 
  max (V)                            & 7        & 7        \\ 
  Safe Overload                      & 150\%    & 150\%    \\ 
  Non-linearity-\% FS                 & 0.25     & 0.5      \\ 
  Compensated Temp. Range ($\degree$F) & 60 - 160 & 60 - 160 \\ 
  Bridge Resistance ($\Omega$)         & 350      & 350      \\ 
  Rated Output (mV/V)                & 2.0      & 2.0      \\ 
  \bottomrule
  \end{tabular}
  \end{table}

  To read the load cell's signal properly, a signal conditioning circuit and software that were designed by Estrelinha \cite{Mec2012a} were used. The signal conditioning board has four amplifiers (it can connect to four load cells) each with: i) a gain resistor of 453 $\Omega$ with 1\% error; ii) a Texas Instruments\textregistered's instrumentation amplifier INA 122; iii) a capacitor to ensure a stable power supply; iv) a first order low-pass filter to prevent aliasing that uses a 1k43 $\Omega$ resistor and a 220 nF capacitor. The low pass filter as a cut-off frequency of 506.15 Hz for an expected sample rate of 1k Hz. The circuit schematics can be seen in figure \ref{figure:adc_schematics}. This work was updated by Serra \cite{Serra2014}, and the circuit components were re-arranged to fit in a smaller printed circuit board that can be connected to an Arduino\texttrademark{} nano, as seen in figure \ref{figure:arduino_load_cells}; in this image it can also be seen the instructions to connect the load sensors to the board.

	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.3]{Imagens/adc_schematics.png}
	  \caption{Signal conditioning circuit updated by Serra \cite{Serra2014}.}
	  \label{figure:adc_schematics}
	  \end{figure}
  
  %The baudrate was increased to 230400 bps, after some search in forums, one stated the Arduinos's ATmega328 was capable of higher communication speed \cite{arduinoforum}.
  
  The Arduino is programmed to read the analogue outputs and send the values to the serial port with the order seen in figure \ref{figure:arduino_load_cells}, at a speed of 115200 bps. The baudrate was increased to 230400 bps, after some search in forums one person stated the Arduinos's ATmega328 was capable of higher communication speed. Some tests confirmed this capability which isn't stated in the official  specifications. The Arduino code sent the analogue readings as doubles this was changed to integers to improve communication speed. On the computer side, a buffer collects this data and stores it in a string shown in figure \ref{figure:sensor_message}. The buffer keeps storing data until the byte EOF arrives. Note that the messages starts with sen1, ends with EOF and the spaces count as a byte, so a complete message has at least eight bytes, since the sensor value can have more than one byte. The software was modified to read values only from one Arduino with four load cells and the way that the sensor data was extracted from the string buffer was also modified, using the function \texttt{stringstream}, from the C++ library sstream, the four values  are transferred into four doubles. With these modifications the sensor values are being read at approximately 900 Hz.

  	  \begin{figure}[!ht]
	  \centering
	  \includegraphics[scale=0.7]{Imagens/sensor_message.png}
	  \caption{Message sent by the Arduino with the sensors readings.}
	  \label{figure:sensor_message}
	  \end{figure}
  
% 	  \begin{figure}[!ht]
% 	  \centering
% 	  \includegraphics{Imagens/arduino_load_cells.png}
% 	  \caption{Arduino, signal conditioning board and load cells connections.}
% 	  %\label{figure:arduino_load_cells}
% 	  \end{figure}
	  
	  
	  \begin{figure}[!ht]
	  \centering
	  \subfloat[Arduino Nano.]{
	  \includegraphics[scale=0.15]{Imagens/arduino_low_v1.png}
	  }\qquad
	  \subfloat[Signal conditioning board connected to Arduino Nano.]{
	  \includegraphics[scale=0.13]{Imagens/side_ardui_low_v1.png}
	  } \qquad
	  \subfloat[Load cells connections on the Signal conditioning board.]{
	  \includegraphics[scale=0.225]{Imagens/pcb_conections_legend_low.jpg}
	  }  
	  \caption{Arduino, signal conditioning board and load cells connections.}
	  \label{figure:arduino_load_cells}
	  \end{figure}

%%\FloatBarrier

  \subsection{Tension Compression Mechanism} \label{subsection:tension_mechanism}
  Figure \ref{figure:ten_comp} shows the simple mechanism that allows the measurement of tensile force using load cells. It's made with two pieces of 14 mm U shape 15$\times$15$\times$1.5 aluminium bar (see appendix \ref{annex:load_cell_mechanism}), eight M2 stainless steel nuts and four M2$\times$10 stainless steel bolts. Two nuts are used to prevent them from unscrewing and not compress the cell. %Each nut needs to be screwed against it's pair to prevent them from unscrewing.

	  \begin{figure}[!ht]
	  \centering
	  \subfloat[CAD of the Tension Compression Mechanism.]{
	  \includegraphics[scale=0.5]{Imagens/tension_compression_CAD.png}
	  }\qquad
	  \subfloat[Tension Compression Mechanism Assembled.]{
	  \includegraphics[scale=0.2]{Imagens/load_cell_mechanism_v1.png}
	  }  
	  \caption{Tension Compression Mechanism.}
	  \label{figure:ten_comp}
	  \end{figure}

  \subsection{Cell Calibration} \label{subsection:calibration}
  The main objective of using load cells is to know the force exerted in the cable. As proven by Estrelinha \cite{Mec2012a}, each cell needs its own calibration curve. The same universal testing machine was used, model AGS-X 10kN by Shimadzu Corporation shown in figure \ref{figure:test_machine}. Using the machine to tension the load cell mechanism, force measurements were manually stored as well as the analogue values sent by the Arduino. The linear coefficients and their error of determination were extracted from the collected data and the results are shown in table \ref{table:calibration}.
  The calibration process consist on multiplying the analogue value $x$ by $a$ and adding $b$ giving the tensile force magnitude in Newtons.
  
  %This method wasn't the best because the machine didn't have a compatible resolution with the sensors since its range is much higher than the load cells. For this prototype this error won't be critical, because the tensile forces that will be measured will be much lower than the sensors limits and the admitted variance for the tensile force will be higher than the error.

  
	 \begin{figure}[!ht]
	 \centering
	 \includegraphics{Imagens/maquina_ensaios.png}
	 \caption{AGS-X 10kN by Shimadzu Corporation.}
	 \label{figure:test_machine}
	 \end{figure}

	\begin{table}[!ht]\centering
	\ra{1.3}
	\caption{Load cell linear coefficients $(y=a\times x+b)$ and their error of determination.}
	\label{table:calibration}
	\begin{tabular}{@{}lllll@{}}\toprule
	Cell number & Cell Reference & a    & b     & r$^{2}$ \\ \cmidrule{1-5}
	2           & X281413        & 0.06 & -3.17 & 0.99 \\ 
	3           & X271169        & 0.03 & -1.80 & 0.99 \\ 
	4           & X281448        & 0.07 & 0.25  & 0.99 \\ 
	\bottomrule
	\end{tabular}
	\end{table}
	
%	2           & X281413        & 0.061560223246350 & -3.186239867848130 & 0.999962194659210 \\ 
%	3           & X271169        & 0.025195879612482 & -1.804367039056569 & 0.999883839979616 \\ 
%	4           & X281448        & 0.065493148994631 & 0.246757519470034  & 0.999821522630423 \\ 
	
	%%\FloatBarrier
	
%   \subsection{Force Filter} \label{subsection:force_filter}
% 	
% 	The next step after the calibration of the sensors is to filter the force values, since they are received at 900 Hz they bring a lot of noise. To determine how to filter this data a sample was collected and two filtering methods were tested by changing their parameters until a good result was found. A good filtering process shouldn't delay the data too much, should attenuate it's noise and take little calculation time.
% 	
% 	One of the tested filtering process is the Infinite Impulse Response (IIR).
% 	\begin{quote} \cite{Chassaing1999} The output depends on the inputs as well as past outputs (with feedback). The output $y(n)$, at time $n$, depends not only on the current input $x(n)$, at time $n$,and on past inputs $x(n-1)$, $x(n-2)$,..., $x(n-N)$, but also on past outputs $y(n-1)$, $y(n-2)$,..., $y(n-M)$.       
% 	\end{quote} The equation that represents this filter can be seen \ref{equation:IIRcompressed} and \ref{equation:IIRextended}.
% 	
% 	
% 	
% 	\begin{align}
% 	    y(n) &= \sum_{k = 0}^{N}a_kx(n-k)-\sum_{j = 1}^{M}b_jy(n-j) \label{equation:IIRcompressed}\\
% 	    &= a_0x(n)+a_1x(n-1)+a_2(n-2)+...+a_N(n-N) \nonumber \\
% 	    &-b_1y(n-1)-b_2y(n-2)-...-b_My(n-M)\label{equation:IIRextended}
% 	\end{align}
% 	
% 	
% 	
% 	The other tested filter is the finite impulse response (FIR), contrary to IIR filter the FIR output depends only on the inputs. The equation that represents this filter can be seen in \ref{equation:FIRcompressed} and \ref{equation:FIRextended}. There are many methods to design a FIR filter, this one was chosen because it's simple and can be easily designed in Matlab with the function \texttt{fir1}.
% 	
% 	\begin{align}
% 	  y(n) &= \sum_{i=0}^{N}b_i.x(n-i) \label{equation:FIRcompressed}\\
% 	  &= b_0x(n)+b_1(n-1)+...+b_N(n-N) \label{equation:FIRextended}
% 	\end{align}
% 
% 	As written before the two filters were tested with different orders and cutoff frequencies, the first test served to rule out one filtering method. As it can be seen in figure \ref{graphic:IIRvsFIR} FIR requires a higher order than IIR to attenuate the signal noise, this would be time costly since the calculations need to be done at 900Hz, so finite impulse response filter is ruled out.
% 	
% 	The next step was to tune the IIR filter minding a compromise between delay and signal attenuation. Different values for cut off frequency and order were used, part of this process is depicted in figure \ref{graphic:IRRtuning}, the values that were chosen are 10 Hz cut off frequency and order 5.
% 	
% 	The final step was to implement the filter in c++ code using the constants calculated by Matlab, the comparison between Matlab filtering and the implemented filter are good, as it can be seen in figure \ref{graphic:implemented_filter}.
% 	
% 	
% \begin{figure}[!ht]\centering
%   \pgfplotstableread[col sep = comma]{Graficos/filtro/iirvsfir.csv}\iirvsfircsv
%   \tikzsetnextfilename{iirvsfir}
%       \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=south east, % customize legend 
% 	  ]
% 	  \addplot[mark=none, color=orange, thin] table[x index = {0}, y index = {1}]{\iirvsfircsv};
% 	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\iirvsfircsv};
% 	  \addplot[mark=none, color=blue, thin] table[x index = {0}, y index = {3}]{\iirvsfircsv};
% 	  \legend{Raw data, IIR filter, FIR Filter}
% 	\end{axis}
%       \end{tikzpicture}
%   \caption{IIR filter versus FIR filter.}
%   \label{graphic:IIRvsFIR}
% \end{figure} 
% 
% \begin{figure}[!ht]\centering
%   \pgfplotstableread[col sep = comma]{Graficos/filtro/iirtuning.csv}\iirtuningcsv
%   \tikzsetnextfilename{iirtuning}
%       \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=south east, % customize legend 
% 	  ]
% 	  \addplot[mark=none, color=orange, thin] table[x index = {0}, y index = {1}]{\iirtuningcsv};
% 	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\iirtuningcsv};
% 	  \addplot[mark=none, color=blue, thin] table[x index = {0}, y index = {3}]{\iirtuningcsv};
% 	  \addplot[mark=none, color=purple, thin] table[x index = {0}, y index = {4}]{\iirtuningcsv};
% 	  \legend{Raw data, IIR order=5 \&{} cutoff=10Hz, IIR order=5 \&{} cutoff=9Hz, IIR order=5 \&{} cutoff=11Hz}
% 	\end{axis}
%       \end{tikzpicture}
%   \caption{Tuning IIR parameters.}
%   \label{graphic:IRRtuning}
% \end{figure}
% 
%  \begin{figure}[!ht]\centering
%   \pgfplotstableread[col sep = comma]{Graficos/filtro/implmented_filter.csv}\implmentedcsv
%   \tikzsetnextfilename{implmented}
%       \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=south east, % customize legend 
% 	  ]
% 	  \addplot[mark=none, color=orange, thin] table[x index = {0}, y index = {1}]{\implmentedcsv};
% 	  \addplot[mark=none, color=red, thin] table[x index = {0}, y index = {2}]{\implmentedcsv};
% 	  \addplot[mark=none, color=blue, thin] table[x index = {0}, y index = {3}]{\implmentedcsv};
% 	  \legend{Raw data, Matlal filter, Implemented filter}
% 	\end{axis}
%       \end{tikzpicture}
%   \caption{Implemented IIR filter versus Matalab's IIR filter.}
%   \label{graphic:implemented_filter}
% \end{figure}


% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.3]{Imagens/IIRvsFIR.png}
% 	\caption{IIR filter versus FIR filter.}
% 	\label{figure:IIRvsFIR}
% 	\end{figure}

% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.4]{Imagens/IIRtuning.png}
% 	\caption{Tuning IIR parameters.}
% 	\label{figure:IRRtuning}
% 	\end{figure}

% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=0.3]{Imagens/implemented_filter.png}
% 	\caption{Implemented IIR filter versus Matalab's IIR filter.}
% 	\label{figure:implemented_filter}
% 	\end{figure}	

	%\FloatBarrier
	
  \section{Software Platform}
  The software used is Robot Operating System (ROS) with C++ language on a catkin workspace. This platform was chosen because it is open source, uses the same programming language as Dynamixel and it allows the use of previous work done in LAR. %This is a well known set of tools, to learn more about them refer to ROS wiki.

  The package \verb|dyna_comm| was created to carry out the main operations which are communicating with the servomotors and acquire data from the load sensors.

  The node \verb|dyna_comm| is responsible for communicating with the motors and calculate the required action to be sent to the Dynamixel. It connects with the servos using DETI libraries which establish the communication protocol with the main controller and have the functions needed to send the necessary commands. This node can publish servo information, commands sent and other values related with the servo control on the topic \verb|/servo_info|; this is used for debugging and to adjust the control parameters. It subscribes to a topic called \verb|/servo_command| to receive user commands and send them to the Dynamixels, this is mainly used to start a motor motion. If this node sends only one instruction to the servos (per ROS loop) its frequency is $\sim$333 Hz, which is its current frequency; if more messages are sent, the frequency will drop. To use the highest frequency possible with this setup, the only message sent per ROS loop has to be of the type sync write, seen in table \ref{table:instructions}. 
  
  Figure \ref{figure:node_diagram} shows the nodes interactions and figure \ref{figure:software_diagram} shows, in a simple way, the node's architecture. The ROS tool \verb|rqt_plot| is an option to see in real time the sensor values and some servo information. The other ROS tool used is \verb|rostopic| and its function \texttt{pub} to publish commands that the node \verb|dyna_comm| will interpret.

  The node \verb|dyna_pressure_cells| is responsible for gathering a complete message sent by the force acquisition unit and then calibrate those values as described in \ref{subsection:calibration}. After these calculations are done, the node publishes the tensile force measured by the sensors in the topic \verb|/values| with a frequency of 900 Hz. This node doesn't subscribe to a topic, only publishes on one.
  
%    	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=1]{Imagens/node_diagram.png}
% 	\caption{ROS Nodes interactions and Subscribed/Published topics.}
% 	\label{figure:node_diagram}
% 	\end{figure}
	
	\begin{figure}[!ht]
	\centering
	\includegraphics[scale=0.5]{Imagens/svg/nodes.eps}
	\caption{ROS Nodes interactions and Subscribed/Published topics.}
	\label{figure:node_diagram}
	\end{figure}

% \begin{figure}
%     \centering
%     \def\svgwidth{\columnwidth}
%     \input{nodes.pdf_tex}
%     	\caption{ROS Nodes interactions and Subscribed/Published topics.}
% 	\label{figure:node_diagram}
% \end{figure}


% 
% 	\begin{figure}[!ht]
% 	\centering
% 	\includegraphics[scale=1]{Imagens/diagram.png}
% 	\cprotect\caption{Simplified scheme of the \verb|pressure_cells_dyna| and \verb|dyna_comm| nodes.}
% 	\label{figure:software_diagram}
% 	\end{figure}
	
	\begin{figure}[!ht]
	\centering
	\includegraphics[scale=0.34]{Imagens/svg/flow.eps}
	\cprotect\caption{Simplified scheme of the \verb|pressure_cells_dyna| and \verb|dyna_comm| ROS nodes.}
	\label{figure:software_diagram}
	\end{figure}
