diff --git a/src/drivers/servo.cpp b/src/drivers/servo.cpp
new file mode 100644
index 0000000..fe14ca5
--- /dev/null
+++ b/src/drivers/servo.cpp
@@ -0,0 +1,152 @@
+#include "src/drivers/Servo.h"
+
+#include "mbed-os/rtos/Thread.h"
+
+namespace drivers {
+
+namespace {
+
+    Thread* pServoThread;
+
+    double posToPulseWidthInUs(double position)
+    {
+        return (position/2+1.5)*1000;
+    }
+
+    typedef struct {
+        double from;
+        double to;
+        double intervalInSec;
+        PwmOut* pServo;
+    } SweepData;
+
+    typedef struct {
+        double latestPosition;
+        PwmOut* pServo;
+    } NodData;
+    
+    void sweepWorker(SweepData* pSweepData)
+    {
+        SweepData sweepData = *pSweepData;
+
+        double resolution(24);
+        double range(sweepData.to - sweepData.from);
+        double value(sweepData.to);
+        double increment(range/resolution);
+
+        while (true)
+        {
+            value += increment;
+            
+            if (value >= sweepData.to)
+            {
+                increment = -abs(increment);
+            }
+            else if (value <= sweepData.from)
+            {
+                increment = abs(increment);
+            }
+            sweepData.pServo->pulsewidth_us(posToPulseWidthInUs(value));
+
+            // E.g resolution 10 will give a 100ms delay between steps
+            Thread::wait(1000/resolution);
+        }
+    }
+
+    void nodWorker(NodData* pNodData)
+    {
+        pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition-0.4));
+        Thread::wait(500);
+        pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition));
+    }
+}
+
+Servo::Servo(PinName servoArm)
+: m_servo(servoArm)
+, m_dir(1)
+, m_inMovement(false)
+, m_latestPosition(0.0)
+{
+    center();
+}
+
+void Servo::center()
+{
+    //stop();
+    
+    setPosition(0.0);
+}
+
+void Servo::resetTimers()
+{
+     m_servo.period_ms(20);
+}
+
+void Servo::setPosition(double position)
+{
+    /* TODO: For now we need to call "stop()" 
+    before setting pos when we know we've ran a thread before
+    Stop does not work here for some reason */
+    //stop(); 
+
+    double increment = position*500.0;
+    
+    if (increment < -500)
+    {
+        increment = -500;
+    }
+    else if (increment > 500)
+    {
+        increment = 500;
+    }
+
+    m_servo.pulsewidth_us(1500 + increment*(double)m_dir);
+
+    m_latestPosition = position;
+}
+
+void Servo::invert()
+{
+    m_dir = -1;
+}
+
+void Servo::sweep(double from, double to, double intervalInSec)
+{
+    pServoThread = new Thread;
+
+    SweepData sweepData;
+    sweepData.from = from;
+    sweepData.to = to;
+    sweepData.intervalInSec = intervalInSec;
+    sweepData.pServo = &m_servo;
+
+    pServoThread->start(callback(sweepWorker, &sweepData));
+    wait(5); // Task will run for at least this long (can get destroyed after)
+} 
+
+void Servo::nod()
+{   
+    NodData nodData;
+    nodData.pServo = &m_servo;
+    nodData.latestPosition = m_latestPosition;
+
+    pServoThread = new Thread;
+
+    pServoThread->start(callback(nodWorker, &nodData));
+    wait(1);
+    pServoThread->join(); // Wait for the nod to end
+}
+
+void Servo::stop()
+{
+    if (pServoThread->get_state() != Thread::Deleted)
+    {
+        pServoThread->terminate();
+    }
+    if (pServoThread != NULL)
+    {
+        delete pServoThread;
+    }
+}
+
+} // namespace drivers
\ No newline at end of file
diff --git a/src/drivers/servo.h b/src/drivers/servo.h
new file mode 100644
index 0000000..dabc84c
--- /dev/null
+++ b/src/drivers/servo.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include "mbed.h"
+
+namespace drivers {
+
+class Servo
+{
+public:
+    
+    Servo(PinName servoArm);
+
+    void center();
+
+    //! Will set the reset the period and center the servo
+    void resetTimers();
+
+    //! in range [-1,1]
+    void setPosition(double position);
+
+    //! changes the direction of this arm
+    void invert();
+
+    //! Will start sweeping the servo without end
+    void sweep(double from, double to, double intervalInSec);
+
+    //! The servo will nod like a gentleman
+    void nod();
+
+    //! Stops any active movement (Thread)
+    void stop();
+
+public:
+    
+    PwmOut m_servo;
+
+private:
+
+    int m_dir;
+
+    bool m_inMovement;
+
+    double m_latestPosition;
+
+};
+
+} // namespace drivers