\subsection{Collision avoidance}
\subsectionauthor{Author: Lars Jonsson \\Reviewer: Max Unander}

To be able to fly the copter up and down these shafts without colliding, we have put six ultrasonic sensors on the copter in each direction, see figure \ref{label}.
In this case there will be three pair of sensors, where each pair represent one axis in the Cartesian coordinate system.
With help of the measurement from each sensor, a controller for collision avoidance will be implemented.
The collision avoidance will consist of three individual PID-controllers, where each one will control the position on each axis.

To get this to work we will have to map the copters movement in the Cartesian coordinate system to roll, pitch and throttle commands that will be sent to the flight controller.
This will work in the way that the output from the PIDs in each direction will be represented by either a pitch, roll or throttle command to the flight controller.

\subsubsection{PID - controller}
The PID controller will have the difference value between the measurement of the two ultrasonic sensors as control variable, and it's task is to converge this difference into 0. 
When the difference value goes to 0 it means that the copter is right in the middle between two objects.
The output of the PID will be a number between 1000 and 2000, because that is the input the flight controller can receive.
The input to the flight controller is a linear function where for the roll and pitch command where 1000 represents by a setpoint of $-10^\circ$ and 2000 will induce a setpoint of $+10^\circ$.
This function is linear so 1500 represent no inclination.
For the throttle command 1000 will be no throttle at all and 2000 will set full throttle to the motors. 
The yaw command do differ a bit, it will not set a specific set-point, it will instead set an angular velocity to the copter.
This so that the copter can spin freely around it's own axis.
With this setup we wont have any controller to keep the heading of the copter, which should be implemented in the future.

The first controller we started out with was the hovering controller.
We wanted to set an height that we wanted the copter to hold, this height was measured with a Sonar placed under the copter.
As we were implementing the flight controller we saw that the flight controller already had an built-in PID that used one Sonar to hover.
We tested this out, tuned the values a bit so it would fit our large copter, and saw that it worked pretty well, and therefore we didn't program our own hovering function.
One drawback with this function was you couldn't set an height that you wanted the copter to hold, instead it will hold the hegiht the copter have when you turn on the hovering.

When we had the hovering function working quite good, we started out with trying to get the copter flying stable between two walls.
To do this we used the SonarDifference algorithm mentioned above.
We started out with just the P variable and saw that it "bounced" a lot between the walls.
We concluded that we needed the D parameter as well to reduce the bouncing.
When we saw that this worked we started to fine tune the variables.
This was done by logging data while flying, and afterwards we could check the graphs and from those do some conclusions on how we should change the variables.
The graphs from the final tuning of the \texttt{PID\_Roll} is shown in Fiugre~\ref{fig:PIDtuning1}.

\begin{figure}[H]
	\centering
	\includegraphics[width=0.7\textwidth]{Pictures/PIDtuning1}
	\caption{This shows how the intended communication between the radio-transmitter, MCU and flight controller was going to be implemented.}
	\label{fig:PIDtuning1}
\end{figure}

\begin{figure}[H]
	\centering
	\includegraphics[width=0.7\textwidth]{Pictures/PIDtuning3}
	\caption{This shows how the intended communication between the radio-transmitter, MCU and flight controller was going to be implemented.}
	\label{fig:PIDtuning3}
\end{figure}


\begin{table}[h]
	\centering
	\caption{Table showing the how the switches of the RC controller are mapped, and which function each channel represents.}
	\label{tab:controller_channels}
	\begin{tabular}{|c|c|c|c|c|}
		\hline
		\textbf{SWITCH:} & \textbf{SWA (3-way)} & \textbf{SWB (2-way)} & \textbf{SWC (2-way)} & \textbf{SWH (2-way)} \\ \hline
		\textbf{Channels:} & AUX2 & AUX3 & AUX1 & AUX4 \\ \hline
		\textbf{\begin{tabular}[c]{@{}c@{}}HIGH:\\ (2000)\end{tabular}} & \begin{tabular}[c]{@{}c@{}}HOVER/LANDNING \\ (OFF)\end{tabular} & \begin{tabular}[c]{@{}c@{}}PID\_PITCH \\ (OFF)\end{tabular} & \begin{tabular}[c]{@{}c@{}}MOTOR \\ DISARM\end{tabular} & \begin{tabular}[c]{@{}c@{}}PID\_ROLL \\ (OFF)\end{tabular} \\ \hline
		\textbf{\begin{tabular}[c]{@{}c@{}}MIDDLE:\\ (1500)\end{tabular}} & \begin{tabular}[c]{@{}c@{}}HOVER \\ (ON)\end{tabular} & - - - - - - & - - - - - - & - - - - - - \\ \hline
		\textbf{\begin{tabular}[c]{@{}c@{}}LOW:\\ (1000)\end{tabular}} & \begin{tabular}[c]{@{}c@{}}LANDING \\ (ON)\end{tabular} & \begin{tabular}[c]{@{}c@{}}PID\_PITCH \\ (ON)\end{tabular} & \begin{tabular}[c]{@{}c@{}}MOTOR \\ ARM\end{tabular} & \begin{tabular}[c]{@{}c@{}}PID\_ROLL \\ (ON)\end{tabular} \\ \hline
	\end{tabular}
\end{table}