\documentclass[a4paper, 12pt]{article}

\usepackage{amsmath}
\usepackage[total={17cm,25cm}, top=2.5cm, left=2cm, includefoot]{geometry}
\usepackage{enumitem}
\usepackage[T1]{fontenc}
\usepackage{hyperref}
\usepackage{listings}
\usepackage{color}
\usepackage{graphicx}
\usepackage{titlesec}

\newcommand{\subsubsubsection}[1]{\paragraph{#1}\mbox{}\\}
\setcounter{secnumdepth}{4}
\setcounter{tocdepth}{4}

\setlist[itemize]{topsep=0pt}
\setlength{\itemindent}{0cm}
\setlength{\parskip}{9pt}
\setlength\parindent{0pt}

\lstset{
	language=Python,
	basicstyle=\fontsize{10}{12}\sffamily,
	numbers=left,
 	numberstyle=\tiny,
	frame=tb,
	tabsize=3,
	columns=fixed,
	showstringspaces=false,
	showtabs=false,
	keepspaces,
	commentstyle=\color{red},
	keywordstyle=\color{blue}
}

\begin{document}

\begin{titlepage}
	\begin{center}
    \vspace*{2cm}
		\Huge \textbf{Robotics Simplified}\\
		\normalsize \vspace{0.5cm}
		Generated \today
	\end{center}
\end{titlepage}

\tableofcontents
\newpage


\section{Preface}


Although you don't need to know any robotics before reading through this website, there are some things that can make your learning experience much more pleasant.


\subsection{Programming Language}
Understand basic concepts of programming will definitely come in handy before reading through the project. There are lots of great resources for learning programming:
\begin{itemize}
\item \href{https://www.reddit.com/r/learnprogramming/}{Reddit r/learnprogramming}.
\item \href{https://www.codecademy.com/}{Codeacademy} and their \href{https://www.codecademy.com/learn/learn-python-3}{Learn Python 3} course.
\item \href{https://projecteuler.net/}{Project Euler} for practicing programming on fun math-based problems.
\end{itemize}
Knowing syntax of Python would also be helpful, since all of the code examples discussed in the project are written in Python. If you don't know much about Python, but already know how to program in a different language, \href{http://histo.ucsf.edu/BMS270/diveintopython3-r802.pdf}{Dive into Python 3} is a great place to start.

It is also recommended to know a little about \href{https://en.wikipedia.org/wiki/Object-oriented_programming}{object-oriented programming}, since we will be basing most of the programs on objects of various classes.

\textbf{If you don't know anything about programming} (and don't have the time to learn) \textbf{but are interested in learning about robotics anyway, you can still read through the chapters.} The code is there mainly as examples to those interested in implementations of the discussed concepts.


\subsection{Libraries and Classes}
Throughout the project, there will be a lot of made-up classes like \texttt{Motor}, \texttt{Joystick} and \texttt{Gyro}. They are only used as placeholders for real classes that you would (likely) have, if you were implementing some of the concepts covered on this website on a platform of your choosing.


\subsection{About comments}
There are a LOT of comments in the examples of code that you are about to see. Way, way more than there should be. I agree that this is considered bad practice for practical purposes and that if you are a relative beginner, you shouldn't write code in a similar fashion.

However, since the purpose of the code on this website is educational and not to serve as a part of a codebase, I would argue that it is fine to use them to such extent.


\subsection{Running the code}
All of the code on this website has been tested on a \href{https://www.vexrobotics.com/vexedr}{VEX EDR} robot programmed in Python using \href{https://www.robotmesh.com/}{RobotMesh}. If you want to try out the code yourself, doing the same would be the easiest way - all you'd have to do is substitute the made-up classes and methods for real ones from the vex library and run the code.

If you don't have a VEX EDR robot at your disposal, an alternative is to use methods from \href{https://github.com/xiaoxiae/Robotics-Simplified/blob/master/Code/algorithms/utilities.py}{\texttt{utilities.py}} to test out the values the objects/methods return.





\section{Drivetrain Control}


Algorithms and techniques used to control the motors of the robot's \textbf{\href{https://en.wikipedia.org/wiki/Drivetrain}{drivetrains}}.


\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/drivetrain-control/drivetrain.png}
      \caption{Drivetrain}
      \end{figure}


\href{https://pictures.topspeed.com/IMG/crop/201604/2017-audi-tt-rs-44_1600x0w.jpg}{Drivetrain image source}



The drivetrain of a vehicle is a group of components that deliver power to the driving wheels, hold them together and allow them to move. Here is a \href{http://www.simbotics.org/resources/mobility/drivetrain-selection}{resource} describing various types of drivetrains.

For the purpose of this guide, however, we will only be discussing some of the most frequently used drivetrains and the methods to operate them.





\subsection{Tank drive}
Tank drive is a method of controlling the motors of a robot using two axes of a controller, where each of the axes operates motors on one side of the robot (see image below or \href{https://www.youtube.com/watch?v=vK2CGj8gAWc}{a video}).

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/drivetrain-control/tank-drive.png}
      \caption{Tank Drive}
      \end{figure}


\href{https://grabcad.com/library/wild-thumper-6wd-chassis-1}{Robot CAD model}, \href{https://target.scene7.com/is/image/Target/GUEST_1e4c1fcb-6962-4533-b961-4e760355db27?wid=488&hei=488&fmt=pjpeg}{Controller image source}



\subsubsection{Implementation}
Suppose that we have objects of the \texttt{Motor} class that set the speed of the motors by calling them with values from -1 to 1. We also have a \texttt{Joystick} object that returns the values of the axes $y_1$ and $y_2$.

Implementing tank drive is really quite straightforward: simply set the left motor to whatever the $y_1$ axis value is, and the right motor to whatever the $y_2$ axis value is:

\begin{lstlisting}
def tank_drive(l_motor_speed, r_motor_speed, left_motor, right_motor):
    """Sets the speed of the left and the right motor."""
    left_motor(l_motor_speed)
    right_motor(r_motor_speed)
\end{lstlisting}


\subsubsection{Examples}
Here's a program that makes the robot drive using the values from the joystick:

\begin{lstlisting}
# create robot's motors and the joystick
left_motor = Motor(1)
right_motor = Motor(2)
joystick = Joystick()

# continuously set motors to the values on the axes
while True:
    # get axis values
    y1 = joystick.get_y1()
    y2 = joystick.get_y2()

    # drive the robot using tank drive
    tank_drive(y1, y2, left_motor, right_motor)
\end{lstlisting}


\subsubsection{Closing remarks}
Tank drive is a very basic and easy way to control the robot. When it comes to FRC, it is a frequently used method for its simplicity, and because it is easier for some drivers to control the robot this way, compared to the other discussed methods.





\subsection{Arcade drive}
Arcade drive is a method of controlling the motors of a robot using two axes of a controller, where one of the axes operates the "turning" component of the robot, and one the "driving" component of the robot (as if you were playing a game at an \textit{arcade}).

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/drivetrain-control/arcade-drive.png}
      \caption{Arcade Drive}
      \end{figure}


\href{https://grabcad.com/library/wild-thumper-6wd-chassis-1}{Robot CAD model}, \href{https://target.scene7.com/is/image/Target/GUEST_1e4c1fcb-6962-4533-b961-4e760355db27?wid=488&hei=488&fmt=pjpeg}{Controller image source}



\subsubsection{Implementation}
Suppose that we have a joystick with an $x$ (horizontal) and a $y$ (vertical) axis.

There are many ways to get to the resulting arcade drive equations (for example by using \href{https://www.chiefdelphi.com/media/papers/download/3495}{linear interpolation} for all of the 4 quadrants of the joystick input).

Here is the implementation we get by splitting the values into quadrants:

\begin{lstlisting}
def arcade_drive(rotate, drive, left_motor, right_motor):
    # variables to determine the quadrants
    maximum = max(abs(drive), abs(rotate))
    total, difference = drive + rotate, drive - rotate

    # set speed according to the quadrant that we're in.
    if drive >= 0:
        if rotate >= 0: # I quadrant
            left_motor(maximum)
            right_motor(difference)
        else:           # II quadrant
            left_motor(total)
            right_motor(maximum)
    else:
        if rotate >= 0: # IV quadrant
            left_motor(total)
            right_motor(-maximum)
        else:           # III quadrant
            left_motor(-maximum)
            right_motor(difference)
\end{lstlisting}


\subsubsection{Examples}
Here's a program that makes the robot drive using the values from the joystick:

\begin{lstlisting}
# create robot's motors and the joystick
left_motor = Motor(1)
right_motor = Motor(2)
joystick = Joystick()

# continuously set motors to the values on the axes
while True:
    # get axis values
    x = joystick.get_y1()
    y = joystick.get_y2()

    # drive the robot using tank drive
    arcade_drive(x, y, left_motor, right_motor)
\end{lstlisting}


\subsubsection{Closing remarks}
If you are interested in reading more about this topic, I would suggest looking at \href{https://www.chiefdelphi.com/media/papers/2661}{this thread on Chief Delphi}, where I learned most of the information about the theory behind arcade drive.





\section{Motor controllers}


Autonomous control of the robot using \href{https://en.wikipedia.org/wiki/Motor_controller}{motor controllers}.



It's nice to be able to drive the robot around using a joystick, but it would sometimes be more useful if the robot could drive \textbf{autonomously} (without any external assistance). This is where controllers come in.

A controller is essentially a box that takes in information about the robot and a goal that we want the robot to achieve (like drive a certain distance / turn a certain angle...) and, when asked, spits out values it thinks the robot should set its motors to to achieve that goal.

The are a lot of various controllers to choose from that differ in many ways such as:
\begin{itemize}
\item \textbf{Accuracy} - how accurate is the controller in getting the robot where it needs to be? How error-prone is it to unexpected situations (hitting a bump on the road, for ex.).
\item \textbf{Input} - what of information does the controller needs to function properly (and accurately)?
\item \textbf{Complexity} - how difficult is it to implement/configure said controller?
\end{itemize}
There is a whole field of study called \href{https://en.wikipedia.org/wiki/Control_theory}{control theory} that examines the controllers much more comprehensively than we can in a few short articles. That's why we're only going to talk about a select few.





\subsection{Sample Controller Class}
Let's look at how classes of each of the controllers described in the sections above will look like:

\begin{lstlisting}
class SampleControllerClass:
    """Description of the class."""

    def __init__(self, ...):
        """Creates a controller object."""
        pass


    def set_goal(self, goal):
        """Sets the goal of the controller."""
        pass


    def get_value(self):
        """Returns the current controller value"""
        pass
\end{lstlisting}

Let's break it down function by function:
\begin{itemize}
\item \texttt{\_\_init\_\_} is called when we want to create the controller object. In the actual controller implementations, \texttt{...} will be replaced by the parameters that the controller takes.
\item \texttt{set\_goal} is called to set the controller's goal, where goal has to be a number. Note that we will need to call this function before we try to get a value from the controller, or things will break. This makes logical sense, because the controller can't really help you to reach the goal if you haven't specified the goal.
\item \texttt{get\_value} will return the value that the controller thinks we should set the motors to to achieve our goal. \textbf{All controllers will return a value between -1 and 1} (including 1 and -1).
\end{itemize}
All of this will make more sense as we go through each of the controllers.





\subsection{Dead reckoning (\href{https://en.wikipedia.org/wiki/Dead_reckoning}{wiki})}
One of the simplest ways of controlling the robot autonomously is using dead reckoning.

It uses one of the first equations you learned in physics: $time = distance / velocity$. We use it to calculate, how long it takes the robot to go a certain distance based on its average speed.

Let's look at an example: say our robot drives an average of $v = 2.5 \frac{m}{s}$. We want it to drive a distance of $d = 10m$. To calculate, how long it will take the robot, all you have to do is divide distance by velocity: $t = d/v = 10/2.5 = 4s$.

This is exactly what dead reckoning does - it calculates the time it will take the robot to drive the distance to the goal. When asked, returns 1 if the time hasn't elapsed yet and 0 if it has.


\subsubsection{Implementation}
There are two things that the controller needs: the average speed of the robot and a way to measure how much time had passed. Taking this into consideration, this is how a class implementing dead reckoning could look like:

\begin{lstlisting}
class DeadReckoning:
    """A class implementing a dead reckoning controller."""

    def __init__(self, speed, get_current_time):
        """Takes the average speed and a function that returns current time."""
        self.get_current_time = get_current_time
        self.speed = speed


    def set_goal(self, goal):
        """Sets the goal of the controller (and also starts the controller)."""
        self.goal = goal
        self.start_time = self.get_current_time()


    def get_value(self):
        """Return the current value the controller is returning."""
        # at what time should we reach the destination (d=d_0 + s/v)
        arrival_time = self.start_time + (self.goal / self.speed)

        # return +-1 if we should have reached the destination and 0 if not
        if self.get_current_time() < arrival_time:
            return 1 if self.goal > 0 else -1
        else:
            return 0
\end{lstlisting}

As we see, the parameters the \texttt{\_\_init\_\_} function is expecting to get are:
\begin{itemize}
\item \texttt{speed} - a number describing how fast the robot drives (on average) in units per second.
\item \texttt{get\_current\_time} - a function returning the current time (used to measure, whether we should have arrived or not).
\end{itemize}

\subsubsection{Example}
Let's implement the example mentioned in Introduction and make the robot drive 10 meters:

\begin{lstlisting}
# create robot's motors
left_motor = Motor(1)
right_motor = Motor(2)

# create the controller object and set its goal
controller = DeadReckoning(2.5, get_current_time)
controller.set_goal(10)

# while the controller is telling us to drive forward, drive forward
while controller.get_value() == 1:
    tank_drive(1, 1, left_motor, right_motor)
\end{lstlisting}

Notice how we used our previously implemented \texttt{tank\_drive} function to set both motors to drive forward at a maximum speed. We could have just written \texttt{left\_motor(1)} and \texttt{right\_motor(1)}, but this is a cleaner way of writing it.


\subsubsection{Closing remarks}
Although this is quite a simple controller to implement, you might realize that it is neither accurate nor practical. If the robot hits a bump on the road or slips on a banana peel, there is nothing it can do to correct the error (since it doesn't know where it is and where it's going).

Another important thing to keep in mind when using this controller is that if you want to change how fast the robot is driving/turning, you will need to re-calculate the speed of the robot to match said speed, which is very tedious.

We'll be focusing on improving accuracy in the next few upcoming chapters by incorporating real-time data from the robot in our controllers.





\subsection{Bang–bang (\href{https://en.wikipedia.org/wiki/Bang\%E2\%80\%93bang_control}{wiki})}
Although our previous controller was quite easy to implement and use, there is no way for it to know whether it reached the target or not. It pretty much just turns the motors on for a while and hopes for the best.

Bang-bang aims to fix this problem by using \href{https://en.wikipedia.org/wiki/Feedback}{\texttt{feedback}} from our robot. Feedback could be values from its \href{https://en.wikipedia.org/wiki/Encoder}{encoders} (to measure how far it has gone), \href{https://en.wikipedia.org/wiki/Gyroscope}{gyro} (to measure where it's heading), or really anything else that we want to control. The important thing is that the data are \textbf{real-time} - the robot constantly gives feedback about what is happening to the controller, so the controller can act accordingly.

Bang-bang is the very first idea that comes to mind when we have real-time data. The controller will return 1 if we haven't passed the goal yet and 0 if we have.


\subsubsection{Implementation}
The only thing the Bang-bang controller needs is the feedback function returning information about the state of whatever we're trying to control.

\begin{lstlisting}
class Bangbang:
    """A class implementing a bang-bang controller."""

    def __init__(self, get_feedback_value):
        """Create the bang-bang controller object from the feedback function."""
        self.get_feedback_value = get_feedback_value


    def set_goal(self, goal):
        """Sets the goal of the bang-bang controller."""
        self.goal = goal


    def get_value(self):
        """Returns +1 or -1 (depending on the value of the goal) when the robot
        should be driving and 0 when it reaches the destination."""
        if self.goal > 0:
            if self.get_feedback_value() < self.goal:
                return 1    # goal not reached and is positive -> drive forward
        else:
            if self.get_feedback_value() > self.goal:
                return -1   # goal not reached and is negative -> drive backward

        # if it shouldn't be driving neither forward nor backward
        return 0
\end{lstlisting}

\subsubsection{Examples}
For this example, we need an \texttt{Encoder} class to measure how far the robot has driven. The objects of this class will return the average of the distance driven by the left wheel and by the right wheel. Her is how a program that drives the robot 10 meters will look like:

\begin{lstlisting}
# create robot's motors and the encoder
left_motor = Motor(1)
right_motor = Motor(2)
encoder = Encoder()

# create the controller object and set its goal
controller = Bangbang(encoder)
controller.set_goal(10)

# while the controller is telling us to drive forward, drive forward
while controller.get_value() == 1:
    tank_drive(1, 1, left_motor, right_motor)
\end{lstlisting}

Notice that pretty much nothing changed between this and the dead reckoning example. This is the main advantage of all of the controllers having the same functions - we can use controller objects almost interchangeably, allowing us to easily try out and compare the accuracies of each of the controllers, without messing with the rest of our code.


\subsubsection{Closing remarks}
This is already markedly better than our previous dead reckoning approach, but it is still relatively inaccurate: the robot's inertia will make the robot drive a little extra distance when the controller tells it that it shouldn't be driving anymore, which means it will overshoot.

We could try to fix this by saying that it should start driving backward once it passed the goal, but the only thing you'd get is a robot that drives back and forth across the goal (which may be amusing, but not very helpful).

In the next chapters, we will try to improve our approach and create controllers that don't just return 1 for driving and 0 for not driving, but also values in-between (when the robot should be driving slower and when faster).





\subsection{PID (\href{https://en.wikipedia.org/wiki/PID_controller}{wiki})}
Our previous attempt at creating a controller that used feedback from the robot could be further improved by considering how the \textbf{error} (difference between feedback value and the goal) changes over time.

Since PID is an abbreviation, let's talk about that the terms $P$, $I$ and $D$ mean:
\begin{itemize}
\item $P$ stands for \textbf{proportional} - how large is the error now (in the \textbf{present}).
\item $I$ stands for \textbf{integral} - how large the error (accumulatively) was in the \textbf{past.}
\item $D$ stands for \textbf{derivative} - what will the error likely be in the \textbf{future.}
\end{itemize}
The controller takes into account what happened, what is happening, and what will likely happen and continuously calculates each of the terms as the error changes:

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/motor-controllers/pid.png}
      \caption{PID}
      \end{figure}


\href{https://upload.wikimedia.org/wikipedia/commons/4/40/Pid-feedback-nct-int-correct.png}{PID image source}



\subsubsection{Implementation}
The controller will need $p$, $i$ and $d$ constants to know, how important each of the aforementioned parts (proportional, integral, derivative) are. It will also need a feedback function and, to correctly calculate the integral and derivative, a function that returns the current time:

\begin{lstlisting}
class PID:
    """A class implementing a PID controller."""

    def __init__(self, p, i, d, get_current_time, get_feedback_value):
        """Initialises PID controller object from P, I, D constants, a function
        that returns current time and the feedback function."""
        # p, i, and d constants
        self.p, self.i, self.d = p, i, d

        # saves the functions that return the time and the feedback
        self.get_current_time = get_current_time
        self.get_feedback_value = get_feedback_value


    def reset(self):
        """Resets/creates variables for calculating the PID values."""
        # reset PID values
        self.proportional, self.integral, self.derivative = 0, 0, 0

        # reset previous time and error variables
        self.previous_time, self.previous_error = 0, 0


    def get_value(self):
        """Calculates and returns the PID value."""
        # calculate the error (how far off the goal are we)
        error = self.goal - self.get_feedback_value()

        # get current time
        time = self.get_current_time()

        # time and error differences to the previous get_value call
        delta_time = time - self.previous_time
        delta_error = error - self.previous_error

        # calculate proportional (just error times the p constant)
        self.proportional = self.p * error

        # calculate integral (error accumulated over time times the constant)
        self.integral += error \textit{ delta_time } self.i

        # calculate derivative (rate of change of the error)
        # for the rate of change, delta_time can't be 0 (divison by zero...)
        self.derivative = 0
        if delta_time > 0:
            self.derivative = delta_error / delta_time * self.d

        # update previous error and previous time values to the current values
        self.previous_time, self.previous_error = time, error

        # add past, present and future
        pid = self.proportional + self.integral + self.derivative

        # return pid adjusted to values from -1 to +1
        return 1 if pid > 1 else -1 if pid < -1 else pid


    def set_goal(self, goal):
        """Sets the goal and resets the controller variables."""
        self.goal = goal
        self.reset()
\end{lstlisting}

To fully understand how the controller works, I suggest you closely examine the \texttt{get\_value()} function - that's where all the computation happens.

Notice a new function called \texttt{reset}, that we haven't seen in any of the other controllers. It is called every time we set the goal, because the controller accumulates error over time in the \texttt{self.integral} variable, and it would therefore take longer to adjust to the new goal.

It doesn't change the versatility of the controller classes, because we don't need to call it in order for the controller to function properly, it's just a useful function to have if we want to call it manually.


\subsubsection{Tuning the controller}
PID is the first discussed controller that needs to be tuned correctly to perform well, because if you set the constants to the wrong values, the controller will perform \href{https://www.youtube.com/watch?v=MxALJU_hp34}{poorly}.

There is a \href{https://en.wikipedia.org/wiki/PID_controller#Loop_tuning}{whole section} on Wikipedia about PID tuning. We won't go into details (read through the Wikipedia article if you're interested), but it is just something to keep in mind when using PID.


\subsubsection{Examples}

\subsubsubsection{Driving a distance}
Here is an example that makes the robot drive 10 meters forward. The constants are values that I used on the VEX EDR robot that I built to test the PID code, you will likely have to use different ones:

\begin{lstlisting}
# create robot's motors, gyro and the encoder
left_motor = Motor(1)
right_motor = Motor(2)
encoder = Encoder()

# create the PID controller (with encoder being the feedback function)
controller = PID(0.07, 0.001, 0.002, time, encoder)
controller.set_goal(10)

while True:
    # get the speed from the controller and apply it using tank drive
    value = controller.get_value()
    tank_drive(value, value, left_motor, right_motor)
\end{lstlisting}


\subsubsubsection{Auto-correct heading}
Auto-correcting the heading of a robot is something PID is great for. What we want is to program the robot so that if something (like an evil human) pushes it, the robot adjusts itself to head the way it was heading before the push.

We could either use values from the encoders on the left and the right side to calculate the angle, but a more elegant (and accurate) solution is to use a gyro. Let's therefore assume that we have a \texttt{Gyro} class whose objects give us the current heading of the robot.

One thing we have to think about is what to set the motors to when we get the value from the controller, because to turn the robot, both of the motors will be going in opposite directions. Luckily, \texttt{arcade\_drive} is our savior: we can plug our PID values directly into the turning part of arcade drive (the \texttt{x} axis) to steer the robot. Refer back to the \href{{{site.baseurl}}drivetrain-control/arcade-drive/}{Arcade Drive article}, if you are unsure as to how/why this works.

\begin{lstlisting}
# create robot's motors and the gyro
left_motor = Motor(1)
right_motor = Motor(2)
gyro = Gyro()

# create the PID controller with gyro being the feedback function
controller = PID(0.2, 0.002, 0.015, time, gyro)
controller.set_goal(0)  # the goal is 0 - we want the heading to be 0

while True:
    # get the value from the controller
    value = controller.get_value()

    # set the turning component of arcade drive to the controller value
    arcade_drive(controller.get_value(), 0, left_motor, right_motor)
\end{lstlisting}


\subsubsubsection{Two controller combination}
What's even nicer is that we can combine the two examples that we just implemented into ONE - a robot that drives forward and corrects itself when it isn't heading the right way.

We will create two controllers - one for driving straight by a certain distance and one for turning to correct possible heading errors.

Arcade drive will again be our dear friend, since we can plug values from the controller that controls driving directly into the driving part of arcade drive, and the controller that controls heading directly into the turning part of arcade drive:

\begin{lstlisting}
# create robot's motors, gyro and the encoder
left_motor = Motor(1)
right_motor = Motor(2)
gyro = Gyro()
encoder = Encoder()

# create separate controllers for turning and driving
drive_controller = PID(0.07, 0.001, 0.002, time, encoder)
turn_controller = PID(0.2, 0.002, 0.015, time, gyro)

# we want to stay at the 0° angle and drive 10 meters at the same time
turn_controller.set_goal(0)
drive_controller.set_goal(10)

while True:
    # get the values from both controllers
    turn_value = turn_controller.get_value()
    drive_value = drive_controller.get_value()

    # drive/turn using arcade drive
    arcade_drive(turn_value, drive_value, left_motor, right_motor)
\end{lstlisting}


\subsubsection{Closing remarks}
PID is one of the most widely used controllers not just in robotics, but in many industries (controlling a boiler/thermostat) because it is reliable, relatively easy to implement and quite precise for most use cases.

For motivation, here is a \href{https://www.youtube.com/watch?v=4Y7zG48uHRo}{great video} demonstrating the power of a correctly configured PID controller.





\subsection{Polynomial Function}
Another way that we can get values that aren't just 1's and 0's is to model a function from points and get the speed of the robot from it - for example: let's say that we start at speed 0.2, drive at full speed when we're at half the distance and slow down to 0 when we're at the end.

Polynomial function is a great candidate for this task. We can pick points that we want the function to pass through and then use \href{https://en.wikipedia.org/wiki/Polynomial_regression}{polynomial regression} to get the coefficients of the function. \href{https://mycurvefit.com/}{MyCurveFit.com} is a great website to use for this exact purpose. Here is how a modeled polynomial function could look like:

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/motor-controllers/polynomial-function.png}
      \caption{Polynomial function}
      \end{figure}


As you can see, it returns all sorts of values from 0 to 1.

One thing you should also notice is that the function starts at $x = 0$ and ends at $x = 1$. This is deliberate - it makes it easy for us to "stretch" the function a little wider if we want to drive some other distance, not just 1 meter.


\subsubsection{Horner's method (\href{https://en.wikipedia.org/wiki/Horner\%27s_method}{wiki})}
When it comes to programming, exponentiation tends to be quite imprecise and slow. Horner's method is a neat solution to this problem. The concept is simple - algebraically change the expression so there is no exponentiation:

$$\large 2x^3 + 4x^2 -x + 5 \quad \rightarrow \quad x(x(x(2) + 4) - 1) + 5$$

This trick can be performed on a polynomial of any size, this is just an example of how the method works.


\subsubsection{Implementation}
The controller only needs the coefficients of the polynomial that we modeled, and a feedback function. Here is how the implementation would look like with Horner's method:

\begin{lstlisting}
class PolynomialFunction:
    """A class implementing a polynomial function controller."""

    def __init__(self, coefficients, get_feedback_value):
        """Initialises the polynomial function controller from the polynomial
        coefficients and the feedback value."""
        self.coefficients = coefficients    # the coefficients of the function
        self.get_feedback_value = get_feedback_value   # the feedback function


    def get_value(self):
        """Returns the polynomial function value at feedback function value."""
        # calculate the x coordinate (by "stretching" the function by goal)
        x = self.get_feedback_value() / abs(self.goal)

        # calculate function value using Horner's method
        value = self.coefficients[0]
        for i in range(1, len(self.coefficients)):
            value = x * value + self.coefficients[i]

        # if the value is over 1, set it to 1
        if value > 1:
            value = 1

        # if goal is negative, function value is negative
        return value if self.goal > 0 else -value


    def set_goal(self, goal):
        """Sets the goal of the controller."""
        self.goal = goal
\end{lstlisting}


\subsubsection{Examples}

\subsubsubsection{Driving a distance}
Once again, the code is almost the exact same as the examples from nearly all of the other controllers, the only difference is that a \texttt{PolynomialFunction} controller takes a list of coefficients of the polynomial to calculate the controller value, compared to the inputs of other controllers:

\begin{lstlisting}
# create robot's motors, gyro and the encoder
left_motor = Motor(1)
right_motor = Motor(2)
encoder = Encoder()

# create the controller (with encoder as the feedback function)
controller = PolynomialFunction([-15.69, 30.56, -21.97, 6.91, 0.2], encoder)
controller.set_goal(10)

while True:
    # get the speed from the controller and apply it using tank drive
    value = controller.get_value()
    tank_drive(value, value, left_motor, right_motor)
\end{lstlisting}


\subsubsection{Generating a polynomial}
An alternative to "stretching" the polynomial to fit the goal is to specify the points the polynomial passes through and generate the coefficients \textit{after} the goal is specified.

Say you have points $\left(0,\ 0.2\right)$, $\left(0.4,\ 1\right)$, $\left(0.6,\ 1\right)$ and $\left(1,0\right)$. Since there are 4 points, the general form of the polynomial is $y=ax^3+bx^2+cx+d$ Using this information, we can create a system of linear equations:

$$
\begin{align*}
0.2&=a(0)^3+b(0)^2+c(0)+d \\
1&=a(0.4)^3+b(0.4)^2+c(0.4)+d \\
1&=a(0.6)^3+b(0.6)^2+c(0.6)+d \\
0&=a(1)^3+b(1)^2+c(1)+d
\end{align*}
$$

Solving this system of linear equations will give us the coefficients of the polynomial. We can apply this method to a polynomial of any degree, given enough points (if $d$ is the degree of the polynomial and $n$ is the number of unique points, the degree of the polynomial the points form is $d=n-1$).


\subsubsection{Closing remarks}
Although this controller isn't as widely used as PID, it can frequently perform better than PID, namely in situations where the ranges of movement of the motors are restricted - forks of a forklift/robot arm.





\section{Odometry}


Tracking the position of the robot.



Odometry is the use of data from sensors of the robot to estimate its position over time.

This means that we can calculate where the robot is at the moment, by getting information from sensors such as an encoder, a gyro, or a camera, and performing calculations using said information.





\subsection{Sensor Values}
There are two things we need to know to perform the approximation:
\begin{itemize}
\item \textbf{Distance $Δ$} - how much did we move by?
\item \textbf{Heading $Δ$} - which way are we heading?
\end{itemize}
Assuming we have encoders on both sides of the robot, the distance is quite easy to calculate: we can read the values the encoders on both of the sides are reading and average them. Assuming we also have a gyro, heading is quite easy too: we can get the heading directly as the values the gyro is returning.

But what if we didn't have a gyro?


\subsubsection{Calculate heading without gyro}
Although gyro is arguably the most precise way to measure the current heading, it's not always available. It might be too expensive, impractical to include on a small robot, or not used because of other conditions that deem it unusable. In cases like these, it is good to know how to calculate heading only from readings of the encoders.

Say the robot drove a small arc. The left encoder measured a distance $l$ and the right side measured a distance $r$. The length between the two wheels is $c$, the angle by which we turned is $ω$ (measured in radians), and $x$ is just a variable to help with our calculations. Here is an illustration:

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/odometry/heading-from-encoders.png}
      \caption{Heading from encoders}
      \end{figure}


From this diagram, we can derive equations for the lengths of the arches $l$ and $r$ (here is an article about \href{https://www.mathopenref.com/arclength.html}{arc length}, if you need further clarification):

$$\large l=x\cdot\omega\qquad r=\left(c+x\right)\cdot\omega$$

We can then combine the equations, simplify, and solve for $ω$:

$$\large \frac{l}{\omega} = \frac{r}{\omega} - c$$

$$\large \omega = \frac{r - l}{c}$$

And that's it! The angle can be calculated from the difference of the readings of the encoders, divided by the length of the axis.





\subsection{Line Approximation}
For our first approximation, let's make an assumption that will simplify our equations: instead of an arc, the robot will first turn to the specified angle, and only then drive the distance in a straight line.

This is quite a reasonable assumption to make for smaller angles, and since we are going to be updating the position multiple times per second, the angles aren't going to be as drastic as the picture portrays:

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/odometry/line-approximation.png}
      \caption{Line Approximation}
      \end{figure}


The robot just moved a distance $d$. It was previously at an angle $θ$ and is now at an angle $θ + ω$. We want to calculate, what the new position of the robot is after this move.


\subsubsection{Deriving the equations}
One of the ways to do this is to imagine a right triangle with $d$ being hypotenuse. We will use \href{https://www2.clarku.edu/faculty/djoyce/trig/formulas.html}{trigonometric formulas} and solve for $Δx$ and $Δy$:

$$\large sin(\theta + \omega)=\frac{\Delta y}{d} \qquad cos(\theta + \omega)=\frac{\Delta x}{d}$$

$$\large \Delta y = d \cdot sin(\theta + \omega) \qquad \Delta x = d \cdot cos(\theta + \omega)$$

The resulting coordinates $(x,y)$ are $x=x_0+Δx$ and $y=y_0+Δy$.


\subsubsection{Implementation}
Here is how one could go about implementing a class that tracks the current position of the robot by getting information from both encoders using the aforementioned line approximation methods:

\begin{lstlisting}
from math import cos, sin

class LineApproximation:
    """A class to track the position of the robot in a system of coordinates
    using only encoders as feedback, using the line approximation method."""

    def __init__(self, axis_width, l_encoder, r_encoder):
        """Saves input values, initializes class variables."""
        self.axis_width = axis_width
        self.l_encoder,  self.r_encoder = l_encoder, r_encoder

        # previous values for the encoder position and heading
        self.prev_l, self.prev_r, self.prev_heading = 0, 0, 0

        # starting position of the robot
        self.x, self.y = 0, 0


    def update(self):
        """Update the position of the robot."""
        # get sensor values and the previous heading
        l, r, heading = self.l_encoder(), self.r_encoder(), self.prev_heading

        # calculate encoder deltas (differences from this and previous readings)
        l_delta, r_delta = l - self.prev_l, r - self.prev_r

        # calculate ω
        h_delta = (r_delta - l_delta) / self.axis_width

        # approximate the position using the line approximation method
        self.x += l_delta * cos(heading + h_delta)
        self.y += r_delta * sin(heading + h_delta)

        # set previous values to current values
        self.prev_l, self.prev_r, self.prev_heading = l, r, heading + h_delta


    def get_position(self):
        """Return the position of the robot."""
        return (self.x, self.y)
\end{lstlisting}

Note that for the position estimation to be accurate, the \texttt{update()} function of the class needs to be called multiple times per second.





\subsection{Circle Approximation}
Although the approximation with the assumption that the robot first turns and only then drives can be decently accurate, we can make it more precise by assuming that the robot drives in an arc (which, in reality, is quite close to what the robot is really doing), as seen on the picture bellow:

\begin{figure}
      \centering
      \includegraphics[width=0.8\textwidth]{../assets/images/odometry/circle-approximation.png}
      \caption{Circle Approximation}
      \end{figure}


The robot just moved in an arch, the left encoder measured a distance $l$ and the right wheel a distance $r$. It was previously at an angle $θ$ and is now at an angle $θ + ω$. We want to calculate, what the new position of the robot is after this move.

\subsubsection{Deriving the equations}
Deriving equations for this model will be a little more difficult than the previous one, but they should still be relatively simple to follow.

The way to calculate the new coordinates is to find the radius $R$ of \href{https://en.wikipedia.org/wiki/Instant_centre_of_rotation}{ICC} (\textit{Instantaneous Center of Curvature} - the point around which the robot is turning) and then rotate $x_0$ and $y_0$ around it.


\subsubsubsection{Calculating R}
Let's start by finding the formula for calculating $R$. We will derive it from the formulas for calculating $l$ and $r$:

$$\large l = \omega \cdot \left(R - \frac{c}{2}\right) \qquad \large r = \omega \cdot \left(R + \frac{c}{2}\right)$$

Combining the equations and solving for R gives us:

$$\large R = \left(\frac{r+l}{\omega \cdot 2} \right)$$

From our previous article about \href{{{site.baseurl}}odometry/sensor-values/}{Sensor Values}, we know that $\omega = \frac{r - l}{c}$. If we don't have a gyro, we can just plug that into our newly derived formula and get:

$$\large R = \frac{r+l}{\frac{r - l}{c} \cdot 2} = \frac{r+l}{r - l} \cdot \frac{c}{2}$$


\subsubsubsection{Rotating $(x_0, y_0)$ around ICC}
For this section, we will assume that you know how to rotate a point around the origin by a certain angle. If not, here is a \href{https://www.khanacademy.org/partner-content/pixar/sets/rotation/v/sets-8}{video} from Khan Academy deriving the equations for rotating a point around the origin.

First of, we will need the coordinates of the $ICC$. Since it's perpendicular to the left of the robot, we will use the same \href{https://stackoverflow.com/questions/4780119/2d-euclidean-vector-rotations}{trick} as rotating a vector by 90 degrees counter-clockwise: switch $x_0$ and $y_0$ and negate the first coordinate:

$$\large ICC_x=x_0-R \; sin(\theta) \qquad ICC_y=y_0+R \cdot cos(\theta)$$

To rotate $(x_0, y_0)$ around ICC (and therefore find $(x, y)$), we will first translate ICC to the origin, then rotate, and then translate back:

$$\large x = (x_0 - ICC_x) \cdot cos(\omega) - (y_0 - ICC_y) \cdot sin(\omega) + ICC_x$$

$$\large y = (x_0 - ICC_x) \cdot sin(\omega) - (y_0 - ICC_y) \cdot cos(\omega) + ICC_y$$

After plugging in values for $ICC_x$, $ICC_y$ and simplifying, we get:

$$\large x = R \; sin(\theta) \cdot cos(\omega) + R \cdot cos(\theta) \cdot sin(\omega) + x_0 - R \; sin(\theta)$$

$$\large y = R \; sin(\theta) \cdot sin(\omega) - R \cdot cos(\theta) \cdot cos(\omega) + y_0 + R \; cos(\theta)$$

Using trigonometric rules, the equations can be further simplified to:

$$\large x = x_0 + R \; sin(\theta + \omega) - R \; sin(\theta)$$

$$\large y = y_0 - R \; cos(\theta + \omega) + R \; cos(\theta)$$


\subsubsubsection{Edge cases}
There is one noteworthy case of values $l$ and $r$ where our circle approximation method won't work.

If $r=l$ (if $\omega = 0$), then the radius cannot be calculated, because it would be "infinite". For this reason, our method can't be used on its own, because if the robot drove in a straight line, the code would crash.

We can, however, still employ or line approximation method, since in this case, it really is driving in a straight line! It is also a good idea to apply the line approximation method for very small angles, since it's almost like driving straight, and it's far less computation-heavy.


\subsubsection{Implementation}
Here is the implementation, combining both of the approximation methods:

\begin{lstlisting}
from math import cos, sin

class CircleApproximation:
    """A class to track the position of the robot in a system of coordinates
    using only encoders as feedback, using a combination of line and circle
    approximation methods."""

    def __init__(self, axis_width, l_encoder, r_encoder):
        """Saves input values, initializes class variables."""
        self.axis_width = axis_width
        self.l_encoder,  self.r_encoder = l_encoder, r_encoder

        # previous values for the encoder position and heading
        self.prev_l, self.prev_r, self.prev_heading = 0, 0, 0

        # starting position of the robot
        self.x, self.y = 0, 0


    def update(self):
        """Update the position of the robot."""
        # get sensor values and the previous heading
        l, r, heading = self.l_encoder(), self.r_encoder(), self.prev_heading

        # calculate encoder deltas (differences from this and previous readings)
        l_delta, r_delta = l - self.prev_l, r - self.prev_r

        # calculate ω
        h_delta = (r_delta - l_delta) / self.axis_width

        # either approximate if we're going (almost) straight or calculate arc
        if abs(l_delta - r_delta) < 1e-5:
            self.x += l_delta * cos(heading)
            self.y += r_delta * sin(heading)
        else:
            # calculate the radius of ICC
            R = (self.axis_width / 2) * (r_delta + l_delta) / (r_delta - l_delta)

            # calculate the robot position by finding a point that is rotated
            # around ICC by heading delta
            self.x +=   R \textit{ sin(h_delta + heading) - R } sin(heading)
            self.y += - R \textit{ cos(h_delta + heading) + R } cos(heading)

        # set previous values to current values
        self.prev_l, self.prev_r, self.prev_heading = l, r, heading + h_delta


    def get_position(self):
        """Return the position of the robot."""
        return (self.x, self.y)
\end{lstlisting}





\section{Resources}


Links to resources either directly used by the website (such as libraries), or that helped me understand the concepts mentioned in the articles.


\subsection{Math}
The website uses \href{https://www.mathjax.org/}{MathJax} to render Latex equations. It previously used \href{http://mathurl.com/}{mathURL}, but it was way more effort to include them, and they weren't interactive.

\subsection{Images}
The images used to illustrate the concepts on this website are modified using \href{https://inkscape.org/cs/}{Inkscape} (free vector graphics editor) and \href{https://www.gimp.org/}{GIMP} (free bitmap graphics editor). CAD model images are generated and adjusted using \href{https://www.autodesk.com/products/fusion-360/students-teachers-educators}{Fusion 360} (free CAD design software).

\subsection{p5.js}
Visualizations on the website are created using the \href{https://p5js.org/}{p5.js} library. This \href{(https://raw.githubusercontent.com/KevinWorkman/HappyCoding/gh-pages/examples/p5js/_posts/2018-07-04-fireworks.md}{example}) helped me understand how it worked with Jekyll. I use the \href{https://editor.p5js.org/}{p5js web editor} to edit the the visualizations before I put them on the website.

\subsection{VEX EDR}
To test the algorithms, I built a custom VEX EDR robot using \href{https://www.vexrobotics.com/276-3000.html}{this kit} that an educational center \href{http://www.vctu.cz/}{VCT} kindly lent me. The robot is programed in Python using \href{https://www.robotmesh.com/studio}{RobotMesh studio} (for more information, see the Python \href{https://www.robotmesh.com/docs/vexcortex-python/html/namespaces.html}{documentation}).

\subsection{Autonomous motion control}
\href{https://github.com/AtsushiSakai/PythonRobotics}{PythonRobotics} is a great repository containing implementations of various robotics algorithms in Python.

\subsection{PID}
I studied a PID Python \href{https://github.com/ivmech/ivPID}{implementation} before writing my own.

\subsection{Drivetrain Control}
There were a few helpful articles that helped me understand equations behind the more complex drivetrains:
\begin{itemize}
\item Arcade drive Chief Delphi forum \href{https://www.chiefdelphi.com/media/papers/2661}{post} by Ether.
\item Simplistic Control of Mecanum Drive \href{https://forums.parallax.com/discussion/download/79828/ControllingMecanumDrive\%255B1\%255D.pdf&sa=U&ved=0ahUKEwiX5LzFiNrfAhVswYsKHTofDrwQFggEMAA&client=internal-uds-cse&cx=002870150170079142498:hq1zjyfbawy&usg=AOvVaw19D74YD--M3YmQ2MGd1rTg}{paper}.
\item Swerve drive Chief Delphi forum \href{https://www.chiefdelphi.com/t/paper-4-wheel-independent-drive-independent-steering-swerve/107383}{post} by Ether.
\end{itemize}
\subsection{Circle Approximation}
There were two main resources that helped me put together the Circle Approximation article.
\begin{itemize}
\item Kinematics Equations for Differential Drive and Articulated Steering \href{http://www8.cs.umu.se/kurser/5DV122/HT13/material/Hellstrom-ForwardKinematics.pdf}{whitepaper}
\item Position Estimation \href{http://people.scs.carleton.ca/~lanthier/teaching/COMP4807/Notes/5\%20-\%20PositionEstimation.pdf}{presentation}
\end{itemize}




\section{About}


Additional information about the project.



\subsection{Motivation}
When I joined the FRC team \href{https://www.metalmoose.org/}{Metal Moose} and started learning about robotics, I didn't find many beginner-friendly resources for people like me. They were hard to find and scattered all around the web. One had to be quite persistent to actually learn something on their own.

That is the main reason for the creation of this website - to serve as a resource for people that want to learn the concepts of robotics, without having to go though all the trouble of finding quality resources.


\subsection{Contact information}
If you'd like to get in touch regarding anything, email me on \texttt{tomas.slama.131@gmail.com}. For more information, see my \href{http://t-slama.cz/}{personal website}.

\subsection{Acknowledgements}
I would like to thank the following people for their help in making this project a reality:
\begin{itemize}
\item \textbf{Kateřina Sulková} for being loving, supportive, and especially helpful in writing the SOČ paper.
\item \textbf{Matěj Halama} (\textit{matejhalama.cz}) for an insightful consultation about the website logo.
\item \textbf{Jan Hladík} for help in designing the logo of the website.
\item \textbf{VCT} for kindly lending me a VEX EDR kit.
\end{itemize}


\end{document}